Update start planner module to use current pose

instead of start pose for checking if on middle of the road Signed-off-by: kyoichi-sugahara <[email protected]>
kyoichi-sugahara · Nov 8, 2023 · 8f8d8d5 · 8f8d8d5
1 parent 110c4f3
commit 8f8d8d5
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 12 deletions.
diff --git a/...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/...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
@@ -770,9 +770,6 @@ lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId
 std::vector<DrivableLanes> 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<Pose> 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<Pose> 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(