change shoulder lanes to pull put lanes

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -227,8 +227,8 @@ class StartPlannerModule : public SceneModuleInterface
   std::vector<DrivableLanes> generateDrivableLanes(const PathWithLaneId & path) const;
   void updatePullOutStatus();
   void updateStatusAfterBackwardDriving();
-  PredictedObjects filterStopObjectsInShoulderLanes(
-    const lanelet::ConstLanelets & shoulder_lanes, const geometry_msgs::msg::Point & current_pose,
+  PredictedObjects filterStopObjectsInPullOutLanes(
+    const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose,
     const double velocity_threshold, const double object_check_backward_distance,
     const double object_check_forward_distance) const;
   bool hasFinishedPullOut() const;

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -676,7 +676,7 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckPath(const PullOutPath &
       return motion_utils::findNearestIndex(
         combined_path.points, collision_check_end_pose->position);
     } else {
-      return combined_path.points.size() - 1;
+      return 0;
     }
   });
   // remove the point behind of collision check end pose
@@ -904,7 +904,7 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
 
-  const auto stop_objects_in_shoulder_lanes = filterStopObjectsInShoulderLanes(
+  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());
 
@@ -948,27 +948,27 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
   return pull_out_start_pose_candidates;
 }
 
-PredictedObjects StartPlannerModule::filterStopObjectsInShoulderLanes(
-  const lanelet::ConstLanelets & shoulder_lanes, const geometry_msgs::msg::Point & current_point,
+PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes(
+  const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_point,
   const double velocity_threshold, const double object_check_forward_distance,
   const double object_check_backward_distance) const
 {
   const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
     *planner_data_->dynamic_object, velocity_threshold);
 
   // filter for objects located in shoulder lanes and moving at a speed below the threshold
-  auto [stop_objects_in_shoulder_lanes, others] =
+  auto [stop_objects_in_pull_out_lanes, others] =
     utils::path_safety_checker::separateObjectsByLanelets(
-      stop_objects, shoulder_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
+      stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
 
   const auto path = planner_data_->route_handler->getCenterLinePath(
-    shoulder_lanes, object_check_backward_distance, object_check_forward_distance);
+    pull_out_lanes, object_check_backward_distance, object_check_forward_distance);
 
   utils::path_safety_checker::filterObjectsByPosition(
-    stop_objects_in_shoulder_lanes, path.points, current_point, object_check_forward_distance,
+    stop_objects_in_pull_out_lanes, path.points, current_point, object_check_forward_distance,
     object_check_backward_distance);
 
-  return stop_objects_in_shoulder_lanes;
+  return stop_objects_in_pull_out_lanes;
 }
 
 bool StartPlannerModule::hasFinishedPullOut() const