diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 216d5cd1d20d8..8057bd327ea9d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -135,7 +135,7 @@ class StartPlannerModule : public SceneModuleInterface void initializeSafetyCheckParameters(); bool isModuleRunning() const; - bool isStartPoseOnMiddleOfTheRoad() const; + bool isCurrentPoseOnMiddleOfTheRoad() const; bool isCloseToOriginalStartPose() const; bool hasArrivedAtGoal() const; bool isMoving() const; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 8fc3571e07500..a86d2e5a062bf 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -770,9 +770,6 @@ lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId std::vector StartPlannerModule::generateDrivableLanes( const PathWithLaneId & path) const { - const auto pull_out_lanes = start_planner_utils::getPullOutLanes( - planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); - const auto path_road_lanes = getPathRoadLanes(path); const auto & pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); @@ -891,10 +888,7 @@ void StartPlannerModule::updateStatusAfterBackwardDriving() PathWithLaneId StartPlannerModule::calcStartPoseCandidatesBackwardPath() const { const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const auto pull_out_lanes = start_planner_utils::getPullOutLanes( - planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); - - const auto pull_out_lanes = start_planner_utils::getPullOutLanes( + const auto & pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); // get backward shoulder path @@ -917,14 +911,11 @@ std::vector StartPlannerModule::searchPullOutStartPoses( const PathWithLaneId & start_pose_candidates) const { const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const auto pull_out_lanes = start_planner_utils::getPullOutLanes( + const auto & pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); std::vector pull_out_start_pose{}; - const auto pull_out_lanes = start_planner_utils::getPullOutLanes( - planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); - // filter pull out lanes stop objects const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets(