fix(lane_change): check prepare phase in intersection

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Author: zulfaqar-azmi-t4

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp

@@ -23,6 +23,7 @@
 #include "rclcpp/logger.hpp"
 
 #include <route_handler/route_handler.hpp>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
@@ -31,6 +32,7 @@
 
 #include <lanelet2_core/Forward.h>
 
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -189,7 +191,7 @@ std::optional<lanelet::BasicPolygon2d> createPolygon(
 
 ExtendedPredictedObject transform(
   const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters,
-  const LaneChangeParameters & lane_change_parameters);
+  const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase);
 
 bool isCollidedPolygonsInLanelet(
   const std::vector<Polygon2d> & collided_polygons, const lanelet::ConstLanelets & lanes);
@@ -224,6 +226,13 @@ lanelet::ConstLanelets generateExpandedLanelets(
  * @return rclcpp::Logger The logger instance configured for the specified lane change type.
  */
 rclcpp::Logger getLogger(const std::string & type);
+
+Polygon2d getEgoCurrentFootprint(
+  const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info);
+
+bool isWithinIntersection(
+  const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelet & lanelet,
+  const Polygon2d & polygon);
 }  // namespace behavior_path_planner::utils::lane_change
 
 namespace behavior_path_planner::utils::lane_change::debug

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -856,23 +856,38 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
   target_objects.other_lane.reserve(target_obj_index.other_lane.size());
 
   // objects in current lane
+
+  lanelet::ConstLanelet current_lane;
+  if (getRouteHandler()->getClosestLaneletWithinRoute(getEgoPose(), &current_lane)) {
+  }
+  const auto ego_footprint =
+    utils::lane_change::getEgoCurrentFootprint(getEgoPose(), common_parameters.vehicle_info);
+  const auto is_ego_within_intersection =
+    utils::lane_change::isWithinIntersection(getRouteHandler(), current_lane, ego_footprint);
+
+  const auto check_prepare_phase =
+    is_ego_within_intersection || lane_change_parameters_->enable_prepare_segment_collision_check;
+
   for (const auto & obj_idx : target_obj_index.current_lane) {
     const auto extended_object = utils::lane_change::transform(
-      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_);
+      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_,
+      check_prepare_phase);
     target_objects.current_lane.push_back(extended_object);
   }
 
   // objects in target lane
   for (const auto & obj_idx : target_obj_index.target_lane) {
     const auto extended_object = utils::lane_change::transform(
-      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_);
+      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_,
+      check_prepare_phase);
     target_objects.target_lane.push_back(extended_object);
   }
 
   // objects in other lane
   for (const auto & obj_idx : target_obj_index.other_lane) {
     const auto extended_object = utils::lane_change::transform(
-      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_);
+      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_,
+      check_prepare_phase);
     target_objects.other_lane.push_back(extended_object);
   }
 
@@ -930,14 +945,25 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
     target_backward_polygons.push_back(lane_polygon);
   }
 
+  lanelet::ConstLanelet current_lane;
+  if (!route_handler.getClosestLaneletWithinRoute(getEgoPose(), &current_lane)) {
+  }
+  const auto ego_footprint =
+    utils::lane_change::getEgoCurrentFootprint(getEgoPose(), common_parameters.vehicle_info);
+  const auto is_ego_within_intersection =
+    utils::lane_change::isWithinIntersection(getRouteHandler(), current_lane, ego_footprint);
+
+  const auto check_prepare_phase =
+    is_ego_within_intersection || lane_change_parameters_->enable_prepare_segment_collision_check;
+
   LaneChangeTargetObjectIndices filtered_obj_indices;
   for (size_t i = 0; i < objects.objects.size(); ++i) {
     const auto & object = objects.objects.at(i);
     const auto obj_velocity_norm = std::hypot(
       object.kinematics.initial_twist_with_covariance.twist.linear.x,
       object.kinematics.initial_twist_with_covariance.twist.linear.y);
-    const auto extended_object =
-      utils::lane_change::transform(object, common_parameters, *lane_change_parameters_);
+    const auto extended_object = utils::lane_change::transform(
+      object, common_parameters, *lane_change_parameters_, check_prepare_phase);
 
     const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object);
 

planning/behavior_path_lane_change_module/src/utils/utils.cpp

@@ -31,7 +31,11 @@
 #include <motion_utils/trajectory/path_with_lane_id.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 #include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
+#include <vehicle_info_util/vehicle_info.hpp>
+
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/geometry/LineString.h>
@@ -1123,7 +1127,7 @@ std::optional<lanelet::BasicPolygon2d> createPolygon(
 ExtendedPredictedObject transform(
   const PredictedObject & object,
   [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters,
-  const LaneChangeParameters & lane_change_parameters)
+  const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase)
 {
   ExtendedPredictedObject extended_object;
   extended_object.uuid = object.object_id;
@@ -1133,8 +1137,6 @@ ExtendedPredictedObject transform(
   extended_object.shape = object.shape;
 
   const auto & time_resolution = lane_change_parameters.prediction_time_resolution;
-  const auto & check_at_prepare_phase =
-    lane_change_parameters.enable_prepare_segment_collision_check;
   const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration;
   const auto & velocity_threshold =
     lane_change_parameters.prepare_segment_ignore_object_velocity_thresh;
@@ -1192,6 +1194,32 @@ rclcpp::Logger getLogger(const std::string & type)
 {
   return rclcpp::get_logger("lane_change").get_child(type);
 }
+
+Polygon2d getEgoCurrentFootprint(
+  const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info)
+{
+  const auto base_to_front = ego_info.max_longitudinal_offset_m;
+  const auto base_to_rear = ego_info.rear_overhang_m;
+  const auto width = ego_info.vehicle_width_m;
+
+  return tier4_autoware_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width);
+}
+
+bool isWithinIntersection(
+  const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelet & lanelet,
+  const Polygon2d & polygon)
+{
+  const std::string id = lanelet.attributeOr("intersection_area", "else");
+  if (id == "else") {
+    return false;
+  }
+
+  const auto lanelet_polygon =
+    route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str()));
+
+  return boost::geometry::within(
+    polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet_polygon.basicPolygon())));
+}
 }  // namespace behavior_path_planner::utils::lane_change
 
 namespace behavior_path_planner::utils::lane_change::debug
@@ -1231,4 +1259,5 @@ geometry_msgs::msg::Polygon createExecutionArea(
 
   return polygon;
 }
+
 }  // namespace behavior_path_planner::utils::lane_change::debug