Merge pull request #1092 from tier4/hotfix/v0.20.0/start_planner

feat(start_planner): cherry pick

Author: kyoichi-sugahara

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

@@ -2,17 +2,23 @@
 
 ## Purpose / Role
 
-The Start Planner module is designed to generate a path from the current ego position to the driving lane, avoiding static obstacles and implementing safety checks against dynamic obstacles. (Note: The feature of safety checks against dynamic obstacles is currently a work in progress.)
-This module is activated when a new route is received.
+The Start Planner module is designed to generate a path from the current ego position to the driving lane, avoiding static obstacles and stopping in response to dynamic obstacles when a collision is detected.
 
-Use cases are as follows
+Use cases include:
 
-- start smoothly from the current ego position to centerline.
-  ![case1](./images/start_from_road_lane.drawio.svg)
 - pull out from the side of the road lane to centerline.
-  ![case2](./images/start_from_road_side.drawio.svg)
+
+<figure markdown>
+  ![case1](images/start_from_road_side.drawio.svg){width=1000}
+  <figcaption>pull out from side of the road lane</figcaption>
+</figure>
+
 - pull out from the shoulder lane to the road lane centerline.
-  ![case3](./images/start_from_road_shoulder.drawio.svg)
+
+<figure markdown>
+  ![case2](images/start_from_start_from_road_shoulder.drawio.svg){width=1000}
+  <figcaption>pull out from the shoulder lane</figcaption>
+</figure>
 
 ## Design
 
@@ -59,18 +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 point. currently only for pull out        | 15.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
 
@@ -243,6 +250,52 @@ If a safe path cannot be generated from the current position, search backwards f
 
 [pull out after backward driving video](https://user-images.githubusercontent.com/39142679/181025149-8fb9fb51-9b8f-45c4-af75-27572f4fba78.mp4)
 
+### **search priority**
+
+If a safe path with sufficient clearance for static obstacles cannot be generated forward, a backward search from the vehicle's current position is conducted to locate a suitable start point for a pull out path generation.
+
+During this backward search, different policies can be applied based on `search_priority` parameters:
+
+Selecting `efficient_path` focuses on creating a shift pull out path, regardless of how far back the vehicle needs to move.
+Opting for `short_back_distance` aims to find a location with the least possible backward movement.
+
+![priority_order](./images/priority_order.drawio.svg)
+
+`PriorityOrder` is defined as a vector of pairs, where each element consists of a `size_t` index representing a start pose candidate index, and the planner type. The PriorityOrder vector is processed sequentially from the beginning, meaning that the pairs listed at the top of the vector are given priority in the selection process for pull out path generation.
+
+#### `efficient_path`
+
+When `search_priority` is set to `efficient_path` and the preference is for prioritizing `shift_pull_out`, the `PriorityOrder` array is populated in such a way that `shift_pull_out` is grouped together for all start pose candidates before moving on to the next planner type. This prioritization is reflected in the order of the array, with `shift_pull_out` being listed before geometric_pull_out.
+
+| Index | Planner Type       |
+| ----- | ------------------ |
+| 0     | shift_pull_out     |
+| 1     | shift_pull_out     |
+| ...   | ...                |
+| N     | shift_pull_out     |
+| 0     | geometric_pull_out |
+| 1     | geometric_pull_out |
+| ...   | ...                |
+| N     | geometric_pull_out |
+
+This approach prioritizes trying all candidates with `shift_pull_out` before proceeding to `geometric_pull_out`, which may be efficient in situations where `shift_pull_out` is likely to be appropriate.
+
+#### `short_back_distance`
+
+For `search_priority` set to `short_back_distance`, the array alternates between planner types for each start pose candidate, which can minimize the distance the vehicle needs to move backward if the earlier candidates are successful.
+
+| Index | Planner Type       |
+| ----- | ------------------ |
+| 0     | shift_pull_out     |
+| 0     | geometric_pull_out |
+| 1     | shift_pull_out     |
+| 1     | geometric_pull_out |
+| ...   | ...                |
+| N     | shift_pull_out     |
+| N     | geometric_pull_out |
+
+This ordering is beneficial when the priority is to minimize the backward distance traveled, giving an equal chance for each planner to succeed at the closest possible starting position.
+
 ### **parameters for backward pull out start point search**
 
 | Name                          | Unit | Type   | Description                                                                                                                                                          | Default value  |

planning/behavior_path_start_planner_module/config/start_planner.param.yaml

@@ -5,8 +5,9 @@
       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
       th_distance_to_middle_of_the_road: 0.5
       center_line_path_interval: 1.0

planning/behavior_path_start_planner_module/images/priority_order.drawio.svg

@@ -172,9 +172,9 @@ 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 extractCollisionCheckPath(const PullOutPath & path);
+  PathWithLaneId extractCollisionCheckSection(const PullOutPath & path);
   void updateStatusWithCurrentPath(
     const behavior_path_planner::PullOutPath & path, const Pose & start_pose,
     const behavior_path_planner::PlannerType & planner_type);

planning/behavior_path_start_planner_module/images/start_from_road_lane.drawio.svg

@@ -42,8 +42,9 @@ 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};
   double center_line_path_interval{0.0};
 

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -43,9 +43,12 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
   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/include/behavior_path_start_planner_module/start_planner_parameters.hpp

@@ -585,9 +585,16 @@ void StartPlannerModule::planWithPriority(
   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,7 +625,7 @@ PriorityOrder StartPlannerModule::determinePriorityOrder(
 
 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(
@@ -645,8 +652,8 @@ bool StartPlannerModule::findPullOutPath(
 
   // check collision
   if (utils::checkCollisionBetweenPathFootprintsAndObjects(
-        vehicle_footprint, extractCollisionCheckPath(*pull_out_path), pull_out_lane_stop_objects,
-        parameters_->collision_check_margin)) {
+        vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects,
+        collision_check_margin)) {
     return false;
   }
 
@@ -660,30 +667,28 @@ bool StartPlannerModule::findPullOutPath(
   return true;
 }
 
-PathWithLaneId StartPlannerModule::extractCollisionCheckPath(const PullOutPath & path)
+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;
 }
 
@@ -839,8 +844,10 @@ void StartPlannerModule::updatePullOutStatus()
     return {*refined_start_pose};
   });
 
-  planWithPriority(
-    start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority);
+  if (!status_.backward_driving_complete) {
+    planWithPriority(
+      start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority);
+  }
 
   start_planner_data_.refined_start_pose = *refined_start_pose;
   start_planner_data_.start_pose_candidates = start_pose_candidates;
@@ -910,6 +917,10 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
     pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity,
     backward_path_length, std::numeric_limits<double>::max());
 
+  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.
   const double current_arc_length =
@@ -922,9 +933,12 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
        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) {
+    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;
-    }
 
     const double backed_pose_arc_length =
       lanelet::utils::getArcCoordinates(pull_out_lanes, *backed_pose).length;
@@ -941,7 +955,7 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
 
     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.
     }