feat(behavior_path_planner): lane change collided polygon intersect l…

…anes

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp

@@ -185,5 +185,8 @@ std::optional<lanelet::BasicPolygon2d> createPolygon(
 ExtendedPredictedObject transform(
   const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters,
   const LaneChangeParameters & lane_change_parameters);
+
+bool isCollidedPolygonsInLanelet(
+  const std::vector<Polygon2d> & collided_polygons, const lanelet::ConstLanelets & lanes);
 }  // namespace behavior_path_planner::utils::lane_change
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp

@@ -103,7 +103,7 @@ boost::optional<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVeloci
  * @param debug The debug information for collision checking.
  * @return true if distance is safe.
  */
-bool checkCollision(
+std::optional<std::vector<Polygon2d>> checkCollision(
   const PathWithLaneId & planned_path,
   const std::vector<PoseWithVelocityStamped> & predicted_ego_path,
   const ExtendedPredictedObject & target_object,

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -1889,7 +1889,7 @@ bool AvoidanceModule::isSafePath(
                                         : ego_predicted_path_for_rear_object;
 
     for (const auto & obj_path : obj_predicted_paths) {
-      if (!utils::path_safety_checker::checkCollision(
+      if (utils::path_safety_checker::checkCollision(
             shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params,
             hysteresis_factor, current_debug_data.second)) {
         marker_utils::updateCollisionCheckDebugMap(

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -1587,7 +1587,7 @@ bool GoalPlannerModule::isSafePath() const
 
     // If a collision is detected, mark the object as unsafe and break the loop
     for (const auto & obj_path : obj_predicted_paths) {
-      if (!utils::path_safety_checker::checkCollision(
+      if (utils::path_safety_checker::checkCollision(
             pull_over_path, ego_predicted_path, object, obj_path, planner_data_->parameters,
             safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) {
         marker_utils::updateCollisionCheckDebugMap(

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -1454,18 +1454,32 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
       obj, lane_change_parameters_->use_all_predicted_path);
     for (const auto & obj_path : obj_predicted_paths) {
-      if (!utils::path_safety_checker::checkCollision(
-            path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0,
-            current_debug_data.second)) {
-        path_safety_status.is_safe = false;
-        marker_utils::updateCollisionCheckDebugMap(
-          debug_data, current_debug_data, path_safety_status.is_safe);
-        const auto & obj_pose = obj.initial_pose.pose;
-        const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape);
-        path_safety_status.is_object_coming_from_rear |=
-          !utils::path_safety_checker::isTargetObjectFront(
-            path, current_pose, common_parameters.vehicle_info, obj_polygon);
+      const auto have_collision = utils::path_safety_checker::checkCollision(
+        path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0,
+        current_debug_data.second);
+
+      if (!have_collision) {
+        continue;
       }
+
+      const auto & collided_polygons = *have_collision;
+      const auto collision_in_current_lanes = utils::lane_change::isCollidedPolygonsInLanelet(
+        collided_polygons, lane_change_path.info.current_lanes);
+      const auto collision_in_target_lanes = utils::lane_change::isCollidedPolygonsInLanelet(
+        collided_polygons, lane_change_path.info.target_lanes);
+
+      if (!collision_in_current_lanes && !collision_in_target_lanes) {
+        continue;
+      }
+
+      path_safety_status.is_safe = false;
+      marker_utils::updateCollisionCheckDebugMap(
+        debug_data, current_debug_data, path_safety_status.is_safe);
+      const auto & obj_pose = obj.initial_pose.pose;
+      const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape);
+      path_safety_status.is_object_coming_from_rear |=
+        !utils::path_safety_checker::isTargetObjectFront(
+          path, current_pose, common_parameters.vehicle_info, obj_polygon);
     }
     marker_utils::updateCollisionCheckDebugMap(
       debug_data, current_debug_data, path_safety_status.is_safe);

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -1030,7 +1030,7 @@ bool StartPlannerModule::isSafePath() const
 
     // If a collision is detected, mark the object as unsafe and break the loop
     for (const auto & obj_path : obj_predicted_paths) {
-      if (!utils::path_safety_checker::checkCollision(
+      if (utils::path_safety_checker::checkCollision(
             pull_out_path, ego_predicted_path, object, obj_path, planner_data_->parameters,
             safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) {
         marker_utils::updateCollisionCheckDebugMap(

planning/behavior_path_planner/src/utils/lane_change/utils.cpp

@@ -1103,4 +1103,16 @@ ExtendedPredictedObject transform(
 
   return extended_object;
 }
+
+bool isCollidedPolygonsInLanelet(
+  const std::vector<Polygon2d> & collided_polygons, const lanelet::ConstLanelets & lanes)
+{
+  const auto lanes_polygon = createPolygon(lanes, 0.0, std::numeric_limits<double>::max());
+
+  const auto is_in_lanes = [&](const auto & collided_polygon) {
+    return lanes_polygon && boost::geometry::intersects(lanes_polygon.value(), collided_polygon);
+  };
+
+  return std::any_of(collided_polygons.begin(), collided_polygons.end(), is_in_lanes);
+}
 }  // namespace behavior_path_planner::utils::lane_change

planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp

@@ -257,7 +257,7 @@ boost::optional<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVeloci
   return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, ego_polygon};
 }
 
-bool checkCollision(
+std::optional<std::vector<Polygon2d>> checkCollision(
   [[maybe_unused]] const PathWithLaneId & planned_path,
   const std::vector<PoseWithVelocityStamped> & predicted_ego_path,
   const ExtendedPredictedObject & target_object,
@@ -271,6 +271,8 @@ bool checkCollision(
     debug.current_obj_pose = target_object.initial_pose.pose;
   }
 
+  std::vector<Polygon2d> collided_polygons{};
+  collided_polygons.reserve(target_object_path.path.size());
   for (const auto & obj_pose_with_poly : target_object_path.path) {
     const auto & current_time = obj_pose_with_poly.time;
 
@@ -302,7 +304,8 @@ bool checkCollision(
     // check overlap
     if (boost::geometry::overlaps(ego_polygon, obj_polygon)) {
       debug.unsafe_reason = "overlap_polygon";
-      return false;
+      collided_polygons.push_back(obj_polygon);
+      continue;
     }
 
     // compute which one is at the front of the other
@@ -341,11 +344,15 @@ bool checkCollision(
     // check overlap with extended polygon
     if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) {
       debug.unsafe_reason = "overlap_extended_polygon";
-      return false;
+      collided_polygons.push_back(obj_polygon);
     }
   }
 
-  return true;
+  if (!collided_polygons.empty()) {
+    return collided_polygons;
+  }
+
+  return std::nullopt;
 }
 
 bool checkCollisionWithExtraStoppingMargin(