feat(start_planner): define collision check margin as list

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp

@@ -41,6 +41,8 @@ struct StartGoalPlannerData
 
   Pose refined_start_pose;
   std::vector<Pose> start_pose_candidates;
+  size_t selected_start_pose_candidate_index;
+  double margin_for_start_pose_candidate;
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_start_planner_module/README.md

@@ -65,19 +65,19 @@ PullOutPath --o PullOutPlannerBase
 
 ## General parameters for start_planner
 
-| Name                                                        | Unit  | Type   | Description                                                                 | Default value |
-| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------ |
-| th_arrived_distance_m                                       | [m]   | double | distance threshold for arrival of path termination                          | 1.0           |
-| th_distance_to_middle_of_the_road                           | [m]   | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1           |
-| th_stopped_velocity_mps                                     | [m/s] | double | velocity threshold for arrival of path termination                          | 0.01          |
-| th_stopped_time_sec                                         | [s]   | double | time threshold for arrival of path termination                              | 1.0           |
-| th_turn_signal_on_lateral_offset                            | [m]   | double | lateral distance threshold for turning on blinker                           | 1.0           |
-| intersection_search_length                                  | [m]   | double | check if intersections exist within this length                             | 30.0          |
-| length_ratio_for_turn_signal_deactivation_near_intersection | [m]   | double | deactivate turn signal of this module near intersection                     | 0.5           |
-| collision_check_margin                                      | [m]   | double | Obstacle collision check margin                                             | 1.0           |
-| collision_check_distance_from_end                           | [m]   | double | collision check distance from end shift end pose                            | 1.0           |
-| collision_check_margin_from_front_object                    | [m]   | double | collision check margin from front object                                    | 5.0           |
-| center_line_path_interval                                   | [m]   | double | reference center line path point interval                                   | 1.0           |
+| Name                                                        | Unit  | Type     | Description                                                                 | Default value   |
+| :---------------------------------------------------------- | :---- | :------- | :-------------------------------------------------------------------------- | :-------------- |
+| th_arrived_distance_m                                       | [m]   | double   | distance threshold for arrival of path termination                          | 1.0             |
+| th_distance_to_middle_of_the_road                           | [m]   | double   | distance threshold to determine if the vehicle is on the middle of the road | 0.1             |
+| th_stopped_velocity_mps                                     | [m/s] | double   | velocity threshold for arrival of path termination                          | 0.01            |
+| th_stopped_time_sec                                         | [s]   | double   | time threshold for arrival of path termination                              | 1.0             |
+| th_turn_signal_on_lateral_offset                            | [m]   | double   | lateral distance threshold for turning on blinker                           | 1.0             |
+| intersection_search_length                                  | [m]   | double   | check if intersections exist within this length                             | 30.0            |
+| length_ratio_for_turn_signal_deactivation_near_intersection | [m]   | double   | deactivate turn signal of this module near intersection                     | 0.5             |
+| collision_check_margins                                     | [m]   | [double] | Obstacle collision check margins list                                       | [2.0, 1.5, 1.0] |
+| collision_check_distance_from_end                           | [m]   | double   | collision check distance from end shift end pose                            | 1.0             |
+| collision_check_margin_from_front_object                    | [m]   | double   | collision check margin from front object                                    | 5.0             |
+| center_line_path_interval                                   | [m]   | double   | reference center line path point interval                                   | 1.0             |
 
 ## Safety check with static obstacles
 

planning/behavior_path_start_planner_module/config/start_planner.param.yaml

@@ -5,7 +5,7 @@
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 1.0
-      collision_check_margin: 1.0
+      collision_check_margins: [2.0, 1.5, 1.0]
       collision_check_distance_from_end: 1.0
       collision_check_margin_from_front_object: 5.0
       th_moving_object_velocity: 1.0

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -172,7 +172,7 @@ class StartPlannerModule : public SceneModuleInterface
     const std::string & search_priority, const size_t start_pose_candidates_num);
   bool findPullOutPath(
     const Pose & start_pose_candidate, const std::shared_ptr<PullOutPlannerBase> & planner,
-    const Pose & refined_start_pose, const Pose & goal_pose);
+    const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin);
 
   PathWithLaneId extractCollisionCheckSection(const PullOutPath & path);
   void updateStatusWithCurrentPath(

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp

@@ -42,7 +42,7 @@ struct StartPlannerParameters
   double th_distance_to_middle_of_the_road{0.0};
   double intersection_search_length{0.0};
   double length_ratio_for_turn_signal_deactivation_near_intersection{0.0};
-  double collision_check_margin{0.0};
+  std::vector<double> collision_check_margins{};
   double collision_check_distance_from_end{0.0};
   double collision_check_margin_from_front_object{0.0};
   double th_moving_object_velocity{0.0};

planning/behavior_path_start_planner_module/src/manager.cpp

@@ -43,11 +43,12 @@
   p.intersection_search_length = node->declare_parameter<double>(ns + "intersection_search_length");
   p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter<double>(
     ns + "length_ratio_for_turn_signal_deactivation_near_intersection");
-  p.collision_check_margin = node->declare_parameter<double>(ns + "collision_check_margin");
+  p.collision_check_margins =
+    node->declare_parameter<std::vector<double>>(ns + "collision_check_margins");
   p.collision_check_distance_from_end =
     node->declare_parameter<double>(ns + "collision_check_distance_from_end");
   p.collision_check_margin_from_front_object =
     node->declare_parameter<double>(ns + "collision_check_margin_from_front_object");
   p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
   p.center_line_path_interval = node->declare_parameter<double>(ns + "center_line_path_interval");
   // shift pull out

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -585,9 +585,16 @@
   const PriorityOrder order_priority =
     determinePriorityOrder(search_priority, start_pose_candidates.size());
 
-  for (const auto & [index, planner] : order_priority) {
-    if (findPullOutPath(start_pose_candidates[index], planner, refined_start_pose, goal_pose))
-      return;
+  for (const auto & collision_check_margin : parameters_->collision_check_margins) {
+    for (const auto & [index, planner] : order_priority) {
+      if (findPullOutPath(
+            start_pose_candidates[index], planner, refined_start_pose, goal_pose,
+            collision_check_margin)) {
+        start_planner_data_.selected_start_pose_candidate_index = index;
+        start_planner_data_.margin_for_start_pose_candidate = collision_check_margin;
+        return;
+      }
+    }
   }
 
   updateStatusIfNoSafePathFound();
@@ -618,35 +625,35 @@
 
 bool StartPlannerModule::findPullOutPath(
   const Pose & start_pose_candidate, const std::shared_ptr<PullOutPlannerBase> & planner,
-  const Pose & refined_start_pose, const Pose & goal_pose)
+  const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin)
 {
   const auto & dynamic_objects = planner_data_->dynamic_object;
   const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
     planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
   const auto & vehicle_footprint = createVehicleFootprint(vehicle_info_);
   // extract stop objects in pull out lane for collision check
   const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
     *dynamic_objects, parameters_->th_moving_object_velocity);
   const auto [pull_out_lane_stop_objects, others] =
     utils::path_safety_checker::separateObjectsByLanelets(
       stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
 
   // if start_pose_candidate is far from refined_start_pose, backward driving is necessary
   const bool backward_is_unnecessary =
     tier4_autoware_utils::calcDistance2d(start_pose_candidate, refined_start_pose) < 0.01;
 
   planner->setPlannerData(planner_data_);
   const auto pull_out_path = planner->plan(start_pose_candidate, goal_pose);
 
   // If no path is found, return false
   if (!pull_out_path) {
     return false;
   }
 
   // check collision
   if (utils::checkCollisionBetweenPathFootprintsAndObjects(
         vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects,
-        parameters_->collision_check_margin)) {
+        collision_check_margin)) {
     return false;
   }
 
@@ -660,30 +667,28 @@
   return true;
 }
 
 PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path)
 {
   PathWithLaneId combined_path;
   for (const auto & partial_path : path.partial_paths) {
     combined_path.points.insert(
       combined_path.points.end(), partial_path.points.begin(), partial_path.points.end());
   }
-
   // calculate collision check end idx
-  size_t collision_check_end_idx = 0;
-  const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(
-    combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end);
-
-  if (collision_check_end_pose) {
-    collision_check_end_idx =
-      motion_utils::findNearestIndex(combined_path.points, collision_check_end_pose->position);
-  }
+  const size_t collision_check_end_idx = std::invoke([&]() {
+    const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(
+      combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end);
 
+    if (collision_check_end_pose) {
+      return motion_utils::findNearestIndex(
+        combined_path.points, collision_check_end_pose->position);
+    } else {
+      return combined_path.points.size() - 1;
+    }
+  });
   // remove the point behind of collision check end pose
-  if (collision_check_end_idx + 1 < combined_path.points.size()) {
-    combined_path.points.erase(
-      combined_path.points.begin() + collision_check_end_idx + 1, combined_path.points.end());
-  }
-
+  combined_path.points.erase(
+    combined_path.points.begin() + collision_check_end_idx + 1, combined_path.points.end());
   return combined_path;
 }
 
@@ -914,7 +919,7 @@
 
   const auto front_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes(
     pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0,
     std::numeric_limits<double>::max());
 
   // Set the maximum backward distance less than the distance from the vehicle's base_link to the
   // lane's rearmost point to prevent lane departure.
@@ -928,9 +933,9 @@
        back_distance += parameters_->backward_search_resolution) {
     const auto backed_pose = calcLongitudinalOffsetPose(
       back_path_from_start_pose.points, start_pose.position, -back_distance);
     if (!backed_pose) continue;
 
     if (utils::checkCollisionBetweenFootprintAndObjects(
           local_vehicle_footprint, *backed_pose, front_stop_objects_in_pull_out_lanes,
           parameters_->collision_check_margin_from_front_object))
       continue;
@@ -950,7 +955,7 @@
 
     if (utils::checkCollisionBetweenFootprintAndObjects(
           local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes,
-          parameters_->collision_check_margin)) {
+          parameters_->collision_check_margins.back())) {
       break;  // poses behind this has a collision, so break.
     }