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

…anes (autowarefoundation#5135)

* feat(behavior_path_planner): lane change collided polygon intersect lanes

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

* use wrapper instead of std::optional

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

* rearrange code

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

* Revert "chore: add workaround for tf2 update (autowarefoundation#5127)"

This reverts commit 19ab508.

* fix(autoware_auto_tf2): remove tf2 geometry function duplicated in tf2 geometry msgs

Signed-off-by: wep21 <daisuke.nishimatsu1021@gmail.com>

* revet change for local build

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: wep21 <daisuke.nishimatsu1021@gmail.com>
Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Co-authored-by: wep21 <daisuke.nishimatsu1021@gmail.com>
Co-authored-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

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

@@ -188,5 +188,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

@@ -83,7 +83,6 @@ boost::optional<PoseWithVelocityStamped> calcInterpolatedPoseWithVelocity(
 boost::optional<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVelocityAndPolygonStamped(
   const std::vector<PoseWithVelocityStamped> & pred_path, const double current_time,
   const VehicleInfo & ego_info);
-
 /**
  * @brief Iterate the points in the ego and target's predicted path and
  *        perform safety check for each of the iterated points.
@@ -106,6 +105,28 @@ bool checkCollision(
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
   const double hysteresis_factor, CollisionCheckDebug & debug);
 
+/**
+ * @brief Iterate the points in the ego and target's predicted path and
+ *        perform safety check for each of the iterated points.
+ * @param planned_path The predicted path of the ego vehicle.
+ * @param predicted_ego_path Ego vehicle's predicted path
+ * @param ego_current_velocity Current velocity of the ego vehicle.
+ * @param target_object The predicted object to check collision with.
+ * @param target_object_path The predicted path of the target object.
+ * @param common_parameters The common parameters used in behavior path planner.
+ * @param front_object_deceleration The deceleration of the object in the front.(used in RSS)
+ * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS)
+ * @param debug The debug information for collision checking.
+ * @return a list of polygon when collision is expected.
+ */
+std::vector<Polygon2d> getCollidedPolygons(
+  const PathWithLaneId & planned_path,
+  const std::vector<PoseWithVelocityStamped> & predicted_ego_path,
+  const ExtendedPredictedObject & target_object,
+  const PredictedPathWithPolygon & target_object_path,
+  const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
+  const double hysteresis_factor, CollisionCheckDebug & debug);
+
 /**
  * @brief Check collision between ego path footprints with extra longitudinal stopping margin and
  * objects.

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

@@ -1448,18 +1448,31 @@ 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 collided_polygons = utils::path_safety_checker::getCollidedPolygons(
+        path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0,
+        current_debug_data.second);
+
+      if (collided_polygons.empty()) {
+        continue;
       }
+
+      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/utils/lane_change/utils.cpp

@@ -1111,4 +1111,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

@@ -246,6 +246,20 @@ boost::optional<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVeloci
 }
 
 bool checkCollision(
+  const PathWithLaneId & planned_path,
+  const std::vector<PoseWithVelocityStamped> & predicted_ego_path,
+  const ExtendedPredictedObject & target_object,
+  const PredictedPathWithPolygon & target_object_path,
+  const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
+  const double hysteresis_factor, CollisionCheckDebug & debug)
+{
+  const auto collided_polygons = getCollidedPolygons(
+    planned_path, predicted_ego_path, target_object, target_object_path, common_parameters,
+    rss_parameters, hysteresis_factor, debug);
+  return collided_polygons.empty();
+}
+
+std::vector<Polygon2d> getCollidedPolygons(
   [[maybe_unused]] const PathWithLaneId & planned_path,
   const std::vector<PoseWithVelocityStamped> & predicted_ego_path,
   const ExtendedPredictedObject & target_object,
@@ -259,6 +273,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;
 
@@ -290,7 +306,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
@@ -329,13 +346,12 @@ 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;
+  return collided_polygons;
 }
-
 bool checkCollisionWithExtraStoppingMargin(
   const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects,
   const double base_to_front, const double base_to_rear, const double width,