feat(goal_planner): expand outer collision check margin

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

margin comment

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

update svg

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Author: kosuke55

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md

@@ -375,6 +375,7 @@ Then there is the concept of soft and hard margins. Although not currently param
 | object_recognition_collision_check_soft_margins              | [m]  | vector[double] | soft margins for collision check when path generation. It is not strictly the distance between footprints, but the maximum distance when ego and objects are oriented. | [5.0, 4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0] |
 | object_recognition_collision_check_hard_margins              | [m]  | vector[double] | hard margins for collision check when path generation                                                                                                                  | [0.6]                                         |
 | object_recognition_collision_check_max_extra_stopping_margin | [m]  | double         | maximum value when adding longitudinal distance margin for collision check considering stopping distance                                                               | 1.0                                           |
+| collision_check_outer_margin_factor                          | [-]  | double         | factor to extend the collision check margin from the inside margin to the outside in the curved path                                                                   | 2.0                                           |
 | detection_bound_offset                                       | [m]  | double         | expand pull over lane with this offset to make detection area for collision check of path generation                                                                   | 15.0                                          |
 
 ## **safety check**

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml

@@ -42,6 +42,7 @@
         collision_check_soft_margins: [5.0, 4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]  # the maximum margin when ego and objects are oriented.
         collision_check_hard_margins: [0.6]  # this should be larger than `surround_check_distance` of surround_obstacle_checker
         object_recognition_collision_check_max_extra_stopping_margin: 1.0
+        collision_check_outer_margin_factor: 2.0
         th_moving_object_velocity: 1.0
         detection_bound_offset: 15.0
         outer_road_detection_offset: 1.0

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg

@@ -80,6 +80,7 @@ struct GoalPlannerParameters
   std::vector<double> object_recognition_collision_check_soft_margins{};
   std::vector<double> object_recognition_collision_check_hard_margins{};
   double object_recognition_collision_check_max_extra_stopping_margin{0.0};
+  double collision_check_outer_margin_factor{0.0};
   double th_moving_object_velocity{0.0};
   double detection_bound_offset{0.0};
   double outer_road_detection_offset{0.0};

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp

@@ -128,14 +128,32 @@ bool isWithinAreas(
  */
 std::vector<lanelet::BasicPolygon2d> getBusStopAreaPolygons(const lanelet::ConstLanelets & lanes);
 
+/**
+ * @brief check collision between objects and ego path footprints
+ * @param path ego path to check collision
+ * @param curvatures curvatures of ego path
+ * @param static_target_objects static objects to check collision
+ * @param dynamic_target_objects dynamic objects to check collision
+ * @param behavior_path_parameters behavior path parameters
+ * @param collision_check_margin margin to check collision
+ * @param extract_static_objects flag to extract static objects
+ * @param maximum_deceleration maximum deceleration
+ * @param object_recognition_collision_check_max_extra_stopping_margin maximum extra stopping margin
+ * @param collision_check_outer_margin_factor factor to extend the collision check margin from the
+ *                                            inside margin to the outside in the curved path
+ * @param ego_polygons_expanded expanded ego polygons
+ * @param update_debug_data flag to update debug data
+ * @return true if collision is detected
+ */
 bool checkObjectsCollision(
   const PathWithLaneId & path, const std::vector<double> & curvatures,
   const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects,
   const BehaviorPathPlannerParameters & behavior_path_parameters,
   const double collision_check_margin, const bool extract_static_objects,
   const double maximum_deceleration,
   const double object_recognition_collision_check_max_extra_stopping_margin,
-  std::vector<Polygon2d> & ego_polygons_expanded, const bool update_debug_data = false);
+  const double collision_check_outer_margin_factor, std::vector<Polygon2d> & ego_polygons_expanded,
+  const bool update_debug_data = false);
 
 // debug
 MarkerArray createPullOverAreaMarkerArray(

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp

@@ -114,7 +114,7 @@ PathDecisionState PathDecisionStateController::get_next_state(
           planner_data->parameters, margin,
           /*extract_static_objects=*/false, parameters.maximum_deceleration,
           parameters.object_recognition_collision_check_max_extra_stopping_margin,
-          ego_polygons_expanded, true)) {
+          parameters.collision_check_outer_margin_factor, ego_polygons_expanded, true)) {
       RCLCPP_DEBUG(
         logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects");
       next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED;

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp

@@ -966,7 +966,7 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte
         parameters_.object_recognition_collision_check_hard_margins.back(),
         /*extract_static_objects=*/false, parameters_.maximum_deceleration,
         parameters_.object_recognition_collision_check_max_extra_stopping_margin,
-        debug_data_.ego_polygons_expanded)) {
+        parameters_.collision_check_outer_margin_factor, debug_data_.ego_polygons_expanded)) {
     return false;
   }
 
@@ -1258,7 +1258,8 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
           context_data.dynamic_target_objects, planner_data_->parameters, collision_check_margin,
           true, parameters_.maximum_deceleration,
           parameters_.object_recognition_collision_check_max_extra_stopping_margin,
-          debug_data_.ego_polygons_expanded, true)) {
+          parameters_.collision_check_outer_margin_factor, debug_data_.ego_polygons_expanded,
+          true)) {
       continue;
     }
     if (
@@ -1933,7 +1934,7 @@ bool FreespaceParkingPlanner::isStuck(
         parameters.object_recognition_collision_check_hard_margins.back(),
         /*extract_static_objects=*/false, parameters.maximum_deceleration,
         parameters.object_recognition_collision_check_max_extra_stopping_margin,
-        ego_polygons_expanded)) {
+        parameters.collision_check_outer_margin_factor, ego_polygons_expanded)) {
     return true;
   }
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -113,6 +113,8 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters(
     p.object_recognition_collision_check_max_extra_stopping_margin =
       node->declare_parameter<double>(
         ns + "object_recognition_collision_check_max_extra_stopping_margin");
+    p.collision_check_outer_margin_factor =
+      node->declare_parameter<double>(ns + "collision_check_outer_margin_factor");
     p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
     p.detection_bound_offset = node->declare_parameter<double>(ns + "detection_bound_offset");
     p.outer_road_detection_offset =

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp

@@ -309,6 +309,7 @@ bool checkObjectsCollision(
   const double collision_check_margin, const bool extract_static_objects,
   const double maximum_deceleration,
   const double object_recognition_collision_check_max_extra_stopping_margin,
+  const double collision_check_outer_margin_factor,
   std::vector<Polygon2d> & debug_ego_polygons_expanded, const bool update_debug_data)
 {
   if (path.points.size() != curvatures.size()) {
@@ -324,18 +325,6 @@ bool checkObjectsCollision(
     return false;
   }
 
-  // check collision roughly with {min_distance, max_distance} between ego footprint and objects
-  // footprint
-  std::pair<bool, bool> has_collision_rough =
-    utils::path_safety_checker::checkObjectsCollisionRough(
-      path, target_objects, collision_check_margin, behavior_path_parameters, false);
-  if (!has_collision_rough.first) {
-    return false;
-  }
-  if (has_collision_rough.second) {
-    return true;
-  }
-
   std::vector<Polygon2d> obj_polygons;
   for (const auto & object : target_objects.objects) {
     obj_polygons.push_back(autoware_utils::to_polygon2d(object));
@@ -352,20 +341,21 @@ bool checkObjectsCollision(
     const double extra_stopping_margin = std::min(
       std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration,
       object_recognition_collision_check_max_extra_stopping_margin);
-
     // The square is meant to imply centrifugal force, but it is not a very well-founded formula.
-    // TODO(kosuke55): It is needed to consider better way because there is an inherently
-    // different conception of the inside and outside margins.
+    const double curvature = curvatures.at(i);
     const double extra_lateral_margin = std::min(
-      extra_stopping_margin,
-      std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2)));
-
+      extra_stopping_margin, std::abs(curvature * std::pow(p.point.longitudinal_velocity_mps, 2)));
+    // The outer margin is `collision_check_outer_margin_factor` times larger than the inner margin.
+    const double sign = curvature > 0.0 ? -1.0 : 1.0;
+    const auto ego_pose_offset = calc_offset_pose(
+      p.point.pose, 0.0, sign * (collision_check_outer_margin_factor - 1.0) * extra_lateral_margin,
+      0.0);
     const auto ego_polygon = autoware_utils::to_footprint(
-      p.point.pose,
+      ego_pose_offset,
       behavior_path_parameters.base_link2front + collision_check_margin + extra_stopping_margin,
       behavior_path_parameters.base_link2rear + collision_check_margin,
       behavior_path_parameters.vehicle_width + collision_check_margin * 2.0 +
-        extra_lateral_margin * 2.0);
+        extra_lateral_margin * (collision_check_outer_margin_factor + 1.0));
     ego_polygons_expanded.push_back(ego_polygon);
   }
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp

@@ -255,15 +255,17 @@ double calc_obstacle_max_length(const Shape & shape);
  * @brief Calculate collision roughly by comparing minimum/maximum distance with margin.
  * @param path The path of the ego vehicle.
  * @param objects The predicted objects.
- * @param margin Distance margin to judge collision.
+ * @param min_margin_threshold threshold for collision check when the minimum distance between ego
+ * @param max_margin_threshold threshold for collision check when the maximum distance between ego
  * @param parameters The common parameters used in behavior path planner.
  * @param use_offset_ego_point If true, the closest point to the object is calculated by
  * interpolating the path points.
  * @return Collision (rough) between minimum distance and maximum distance
  */
 std::pair<bool, bool> checkObjectsCollisionRough(
-  const PathWithLaneId & path, const PredictedObjects & objects, const double margin,
-  const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_point);
+  const PathWithLaneId & path, const PredictedObjects & objects, const double min_margin_threshold,
+  const double max_margin_threshold, const BehaviorPathPlannerParameters & parameters,
+  const bool use_offset_ego_point);
 
 /**
  * @brief Calculate the rough distance between the ego vehicle and the objects.