Refactor start planner module to use back path

from start pose for pull out start pose search

Signed-off-by: kyoichi-sugahara <[email protected]>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp

@@ -174,7 +174,7 @@ class StartPlannerModule : public SceneModuleInterface
 
   PathWithLaneId getFullPath() const;
   PathWithLaneId calcBackwardPathFromStartPose() const;
-  std::vector<Pose> searchPullOutStartPoses(const PathWithLaneId & start_pose_candidates) const;
+  std::vector<Pose> searchPullOutStartPoses(const PathWithLaneId & back_path_from_start_pose) const;
 
   std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
 

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -142,18 +142,18 @@ void StartPlannerModule::updateData()
   // 1) backward driving is not allowed: use refined pose just as start pose.
   // 2) backward driving is allowed: use refined pose to check if backward driving is needed.
 
-  const PathWithLaneId start_pose_candidates_path = calcBackwardPathFromStartPose();
+  const PathWithLaneId back_path_from_start_pose = calcBackwardPathFromStartPose();
   // start pose along the backward path aligned to the start pose
   const auto refined_start_pose = calcLongitudinalOffsetPose(
-    start_pose_candidates_path.points,
-    planner_data_->route_handler->getOriginalStartPose().position, 0.0);
+    back_path_from_start_pose.points, planner_data_->route_handler->getOriginalStartPose().position,
+    0.0);
 
   if (!refined_start_pose) return;
 
   // search pull out start candidates backward
   const std::vector<Pose> start_pose_candidates = std::invoke([&]() -> std::vector<Pose> {
     if (parameters_->enable_back) {
-      return searchPullOutStartPoses(start_pose_candidates_path);
+      return searchPullOutStartPoses(back_path_from_start_pose);
     }
     return {*refined_start_pose};
   });
@@ -807,26 +807,25 @@ PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const
 }
 
 std::vector<Pose> StartPlannerModule::searchPullOutStartPoses(
-  const PathWithLaneId & start_pose_candidates) const
+  const PathWithLaneId & back_path_from_start_pose) const
 {
   std::vector<Pose> pull_out_start_pose{};
   const auto & start_pose = planner_data_->route_handler->getOriginalStartPose();
-
+  const auto & local_vehicle_footprint = createVehicleFootprint(vehicle_info_);
   const auto & pull_out_lanes = start_planner_utils::getPullOutLanes(
     planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
 
-  const auto stop_objects_in_pull_out_lanes =
+  const auto & stop_objects_in_pull_out_lanes =
     filterStopObjectsInPullOutLanes(pull_out_lanes, parameters_->th_moving_object_velocity);
 
   const double s_current = lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose).length;
   const double max_back_distance = std::clamp(
     s_current - planner_data_->parameters.base_link2rear, 0.0, parameters_->max_back_distance);
 
-  const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_);
   for (double back_distance = 0.0; back_distance <= max_back_distance;
        back_distance += parameters_->backward_search_resolution) {
-    const auto backed_pose =
-      calcLongitudinalOffsetPose(start_pose_candidates.points, start_pose.position, -back_distance);
+    const auto backed_pose = calcLongitudinalOffsetPose(
+      back_path_from_start_pose.points, start_pose.position, -back_distance);
     if (!backed_pose) {
       continue;
     }