feat(start_planner): keep distance back static objects from start pose

planning/behavior_path_start_planner_module/README.md

@@ -319,19 +319,20 @@ 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_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             |
+| 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                  |
+| objects_collision_check_margins                             | [m]   | [double] | obstacle collision check margins against static objects from the footprint on the trajectory in pull_out_lanes      | [2.0, 1.0, 0.5, 0.1] |
+| collision_check_distance_from_end                           | [m]   | double   | collision check distance from end shift end pose                                                                    | 1.0                  |
+| back_objects_collision_check_margin                         | [m]   | double   | obstacle collision check margin against back static objects from the footprint on the start pose in pull_out_lanes  | 3.0                  |
+| collision_check_margin_from_front_object                    | [m]   | double   | obstacle collision check margin against front static objects from the footprint on the start pose in pull_out_lanes | 5.0                  |
+| center_line_path_interval                                   | [m]   | double   | reference center line path point interval                                                                           | 1.0                  |
 
 ### **Ego vehicle's velocity planning**
 

planning/behavior_path_start_planner_module/config/start_planner.param.yaml

@@ -5,7 +5,8 @@
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 1.0
-      collision_check_margins: [2.0, 1.0, 0.5, 0.1]
+      objects_collision_check_margins: [2.0, 1.0, 0.5, 0.1]
+      back_objects_collision_check_margin: 3.0
       collision_check_margin_from_front_object: 5.0
       th_moving_object_velocity: 1.0
       th_distance_to_middle_of_the_road: 0.5

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp

@@ -63,7 +63,8 @@ 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};
-  std::vector<double> collision_check_margins{};
+  std::vector<double> objects_collision_check_margins{};
+  double back_objects_collision_check_margin{0.0};
   double collision_check_margin_from_front_object{0.0};
   double th_moving_object_velocity{0.0};
   double center_line_path_interval{0.0};

planning/behavior_path_start_planner_module/src/manager.cpp

@@ -43,8 +43,10 @@
   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_margins =
-    node->declare_parameter<std::vector<double>>(ns + "collision_check_margins");
+  p.objects_collision_check_margins =
+    node->declare_parameter<std::vector<double>>(ns + "objects_collision_check_margins");
+  p.back_objects_collision_check_margin =
+    node->declare_parameter<double>(ns + "back_objects_collision_check_margin");
   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");

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -627,7 +627,7 @@
   const PriorityOrder order_priority =
     determinePriorityOrder(search_priority, start_pose_candidates.size());
 
-  for (const auto & collision_check_margin : parameters_->collision_check_margins) {
+  for (const auto & collision_check_margin : parameters_->objects_collision_check_margins) {
     for (const auto & [index, planner] : order_priority) {
       if (findPullOutPath(
             start_pose_candidates[index], planner, refined_start_pose, goal_pose,
@@ -964,13 +964,13 @@
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
 
-  const auto stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes(
-    pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity,
-    backward_path_length, std::numeric_limits<double>::max());
+  const auto back_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes(
+    pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0,
+    backward_path_length);
 
   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());
+    pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity,
+    std::numeric_limits<double>::max(), 0);
 
   // 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.
@@ -1005,8 +1005,8 @@
     }
 
     if (utils::checkCollisionBetweenFootprintAndObjects(
-          local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes,
-          parameters_->collision_check_margins.back())) {
+          local_vehicle_footprint, *backed_pose, back_stop_objects_in_pull_out_lanes,
+          parameters_->back_objects_collision_check_margin)) {
       break;  // poses behind this has a collision, so break.
     }