feat(start_planner): cherry pick

planning/behavior_path_start_planner_module/config/start_planner.param.yaml

@@ -5,15 +5,14 @@
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 1.0
-      collision_check_margins: [2.0, 1.5, 1.0]
-      collision_check_distance_from_end: 1.0
+      collision_check_margins: [2.0, 1.0, 0.5, 0.1]
       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
       # shift pull out
       enable_shift_pull_out: true
-      check_shift_path_lane_departure: false
+      shift_collision_check_distance_from_end: -10.0
       minimum_shift_pull_out_distance: 0.0
       deceleration_interval: 15.0
       lateral_jerk: 0.5
@@ -23,6 +22,7 @@
       maximum_curvature: 0.07
       # geometric pull out
       enable_geometric_pull_out: true
+      geometric_collision_check_distance_from_end: 0.0
       divide_pull_out_path: true
       geometric_pull_out_velocity: 1.0
       arc_path_interval: 1.0
@@ -32,7 +32,7 @@
       # search start pose backward
       enable_back: true
       search_priority: "efficient_path"  # "efficient_path" or "short_back_distance"
-      max_back_distance: 30.0
+      max_back_distance: 20.0
       backward_search_resolution: 2.0
       backward_path_update_duration: 3.0
       ignore_distance_from_lane_end: 15.0

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -202,7 +202,8 @@ class StartPlannerModule : public SceneModuleInterface
     const Pose & start_pose_candidate, const std::shared_ptr<PullOutPlannerBase> & planner,
     const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin);
 
-  PathWithLaneId extractCollisionCheckSection(const PullOutPath & path);
+  PathWithLaneId extractCollisionCheckSection(
+    const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type);
   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/include/behavior_path_start_planner_module/start_planner_parameters.hpp

@@ -43,14 +43,14 @@ struct StartPlannerParameters
   double intersection_search_length{0.0};
   double length_ratio_for_turn_signal_deactivation_near_intersection{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};
 
   // shift pull out
   bool enable_shift_pull_out{false};
   bool check_shift_path_lane_departure{false};
+  double shift_collision_check_distance_from_end{0.0};
   double minimum_shift_pull_out_distance{0.0};
   int lateral_acceleration_sampling_num{0};
   double lateral_jerk{0.0};
@@ -60,6 +60,7 @@ struct StartPlannerParameters
   double deceleration_interval{0.0};
   // geometric pull out
   bool enable_geometric_pull_out{false};
+  double geometric_collision_check_distance_from_end;
   bool divide_pull_out_path{false};
   ParallelParkingParameters parallel_parking_parameters{};
   // search start pose backward

planning/behavior_path_start_planner_module/src/manager.cpp

@@ -45,8 +45,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
     ns + "length_ratio_for_turn_signal_deactivation_near_intersection");
   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");
@@ -55,6 +53,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
   p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");
   p.check_shift_path_lane_departure =
     node->declare_parameter<bool>(ns + "check_shift_path_lane_departure");
+  p.shift_collision_check_distance_from_end =
+    node->declare_parameter<double>(ns + "shift_collision_check_distance_from_end");
   p.minimum_shift_pull_out_distance =
     node->declare_parameter<double>(ns + "minimum_shift_pull_out_distance");
   p.lateral_acceleration_sampling_num =
@@ -66,6 +66,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
   p.deceleration_interval = node->declare_parameter<double>(ns + "deceleration_interval");
   // geometric pull out
   p.enable_geometric_pull_out = node->declare_parameter<bool>(ns + "enable_geometric_pull_out");
+  p.geometric_collision_check_distance_from_end =
+    node->declare_parameter<double>(ns + "geometric_collision_check_distance_from_end");
   p.divide_pull_out_path = node->declare_parameter<bool>(ns + "divide_pull_out_path");
   p.parallel_parking_parameters.pull_out_velocity =
     node->declare_parameter<double>(ns + "geometric_pull_out_velocity");

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

@@ -69,25 +69,15 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
       pull_out_path.partial_paths.front();  // shift path is not separate but only one.
 
     // check lane_departure with path between pull_out_start to pull_out_end
-    PathWithLaneId path_start_to_end{};
+    PathWithLaneId path_shift_start_to_end{};
     {
       const size_t pull_out_start_idx = findNearestIndex(shift_path.points, start_pose.position);
+      const size_t pull_out_end_idx =
+        findNearestIndex(shift_path.points, pull_out_path.end_pose.position);
 
-      // calculate collision check end idx
-      const size_t collision_check_end_idx = std::invoke([&]() {
-        const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(
-          shift_path.points, pull_out_path.end_pose.position,
-          parameters_.collision_check_distance_from_end);
-
-        if (collision_check_end_pose) {
-          return findNearestIndex(shift_path.points, collision_check_end_pose->position);
-        } else {
-          return shift_path.points.size() - 1;
-        }
-      });
-      path_start_to_end.points.insert(
-        path_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx,
-        shift_path.points.begin() + collision_check_end_idx + 1);
+      path_shift_start_to_end.points.insert(
+        path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx,
+        shift_path.points.begin() + pull_out_end_idx + 1);
     }
 
     // extract shoulder lanes from pull out lanes
@@ -131,7 +121,7 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
     // check lane departure
     if (
       parameters_.check_shift_path_lane_departure &&
-      lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_start_to_end)) {
+      lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_shift_start_to_end)) {
       continue;
     }
 

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -657,8 +657,8 @@ bool StartPlannerModule::findPullOutPath(
 
   // check collision
   if (utils::checkCollisionBetweenPathFootprintsAndObjects(
-        vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects,
-        collision_check_margin)) {
+        vehicle_footprint, extractCollisionCheckSection(*pull_out_path, planner->getPlannerType()),
+        pull_out_lane_stop_objects, collision_check_margin)) {
     return false;
   }
 
@@ -672,8 +672,17 @@ bool StartPlannerModule::findPullOutPath(
   return true;
 }
 
-PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path)
+PathWithLaneId StartPlannerModule::extractCollisionCheckSection(
+  const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type)
 {
+  const std::map<PlannerType, double> collision_check_distances = {
+    {behavior_path_planner::PlannerType::SHIFT,
+     parameters_->shift_collision_check_distance_from_end},
+    {behavior_path_planner::PlannerType::GEOMETRIC,
+     parameters_->geometric_collision_check_distance_from_end}};
+
+  const double collision_check_distance_from_end = collision_check_distances.at(planner_type);
+
   PathWithLaneId combined_path;
   for (const auto & partial_path : path.partial_paths) {
     combined_path.points.insert(
@@ -682,7 +691,7 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPat
   // calculate collision check end idx
   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);
+      combined_path.points, path.end_pose.position, collision_check_distance_from_end);
 
     if (collision_check_end_pose) {
       return motion_utils::findNearestIndex(
@@ -1371,9 +1380,14 @@ void StartPlannerModule::setDebugData()
   // visualize collision_check_end_pose and footprint
   {
     const auto local_footprint = vehicle_info_.createFootprint();
+    std::map<PlannerType, double> collision_check_distances = {
+      {PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end},
+      {PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}};
+
+    double collision_check_distance_from_end = collision_check_distances[status_.planner_type];
     const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(
       getFullPath().points, status_.pull_out_path.end_pose.position,
-      parameters_->collision_check_distance_from_end);
+      collision_check_distance_from_end);
     if (collision_check_end_pose) {
       add(createPoseMarkerArray(
         *collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0));