refactor(behavior_path_planner): remove status_.pull_over_lanes (auto…

…warefoundation#5520)

* Remove unused variable and update function arguments in start_planner_module.hpp and util.hpp/cpp

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

---------

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

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

@@ -59,7 +59,6 @@ struct PullOutStatus
   size_t current_path_idx{0};
   PlannerType planner_type{PlannerType::NONE};
   PathWithLaneId backward_path{};
-  lanelet::ConstLanelets pull_out_lanes{};
   bool found_pull_out_path{false};      // current path is safe against static objects
   bool is_safe_dynamic_objects{false};  // current path is safe against dynamic objects
   bool driving_forward{false};          // if ego is driving on backward path, this is set to false

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

@@ -619,8 +619,11 @@ PathWithLaneId StartPlannerModule::generateStopPath() const
     PathPointWithLaneId p{};
     p.point.pose = pose;
     p.point.longitudinal_velocity_mps = 0.0;
+    const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
+      planner_data_,
+      planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
     lanelet::Lanelet closest_lanelet;
-    lanelet::utils::query::getClosestLanelet(status_.pull_out_lanes, pose, &closest_lanelet);
+    lanelet::utils::query::getClosestLanelet(pull_out_lanes, pose, &closest_lanelet);
     p.lane_ids.push_back(closest_lanelet.id());
     return p;
   };
@@ -664,21 +667,23 @@ 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);
   if (!path_road_lanes.empty()) {
     lanelet::ConstLanelets shoulder_lanes;
     const auto & rh = planner_data_->route_handler;
     std::copy_if(
-      status_.pull_out_lanes.begin(), status_.pull_out_lanes.end(),
-      std::back_inserter(shoulder_lanes),
+      pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes),
       [&rh](const auto & pull_out_lane) { return rh->isShoulderLanelet(pull_out_lane); });
 
     return utils::generateDrivableLanesWithShoulderLanes(path_road_lanes, shoulder_lanes);
   }
 
   // if path_road_lanes is empty, use only pull_out_lanes as drivable lanes
   std::vector<DrivableLanes> drivable_lanes;
-  for (const auto & lane : status_.pull_out_lanes) {
+  for (const auto & lane : pull_out_lanes) {
     DrivableLanes drivable_lane;
     drivable_lane.right_lane = lane;
     drivable_lane.left_lane = lane;
@@ -693,11 +698,6 @@ void StartPlannerModule::updatePullOutStatus()
     !planner_data_->prev_route_id ||
     *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid();
 
-  // save pull out lanes which is generated using current pose before starting pull out
-  // (before approval)
-  status_.pull_out_lanes = start_planner_utils::getPullOutLanes(
-    planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
-
   // skip updating if enough time has not passed for preventing chattering between back and
   // start_planner
   if (!has_received_new_route) {
@@ -734,12 +734,15 @@ void StartPlannerModule::updatePullOutStatus()
   planWithPriority(
     start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority);
 
+  const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
+    planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
+
   if (isBackwardDrivingComplete()) {
     updateStatusAfterBackwardDriving();
     current_state_ = ModuleStatus::SUCCESS;  // for breaking loop
   } else {
     status_.backward_path = start_planner_utils::getBackwardPath(
-      *route_handler, status_.pull_out_lanes, current_pose, status_.pull_out_start_pose,
+      *route_handler, pull_out_lanes, current_pose, status_.pull_out_start_pose,
       parameters_->backward_velocity);
   }
 }
@@ -761,12 +764,14 @@ 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);
+
   // get backward shoulder path
-  const auto arc_position_pose =
-    lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose);
+  const auto arc_position_pose = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose);
   const double check_distance = parameters_->max_back_distance + 30.0;  // buffer
   auto path = planner_data_->route_handler->getCenterLinePath(
-    status_.pull_out_lanes, arc_position_pose.length - check_distance,
+    pull_out_lanes, arc_position_pose.length - check_distance,
     arc_position_pose.length + check_distance);
 
   // lateral shift to current_pose
@@ -785,18 +790,20 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoses(
 
   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(
-      *planner_data_->dynamic_object, status_.pull_out_lanes,
+      *planner_data_->dynamic_object, pull_out_lanes,
       utils::path_safety_checker::isPolygonOverlapLanelet);
   const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
     pull_out_lane_objects, parameters_->th_moving_object_velocity);
 
   // Set the maximum backward distance less than the distance from the vehicle's base_link to the
   // lane's rearmost point to prevent lane departure.
-  const double s_current =
-    lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose).length;
+  const double s_current = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose).length;
   const double max_back_distance = std::clamp(
     s_current - planner_data_->parameters.base_link2rear, 0.0, parameters_->max_back_distance);
 
@@ -812,10 +819,10 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoses(
 
     // check the back pose is near the lane end
     const double length_to_backed_pose =
-      lanelet::utils::getArcCoordinates(status_.pull_out_lanes, *backed_pose).length;
+      lanelet::utils::getArcCoordinates(pull_out_lanes, *backed_pose).length;
 
     const double length_to_lane_end = std::accumulate(
-      std::begin(status_.pull_out_lanes), std::end(status_.pull_out_lanes), 0.0,
+      std::begin(pull_out_lanes), std::end(pull_out_lanes), 0.0,
       [](double acc, const auto & lane) { return acc + lanelet::utils::getLaneletLength2d(lane); });
     const double distance_from_lane_end = length_to_lane_end - length_to_backed_pose;
     if (distance_from_lane_end < parameters_->ignore_distance_from_lane_end) {

planning/behavior_path_planner/src/utils/start_planner/util.cpp

@@ -89,15 +89,14 @@ lanelet::ConstLanelets getPullOutLanes(
 {
   const double & vehicle_width = planner_data->parameters.vehicle_width;
   const auto & route_handler = planner_data->route_handler;
-  const auto & current_pose = planner_data->self_odometry->pose.pose;
+  const auto & start_pose = planner_data->route_handler->getOriginalStartPose();
 
   lanelet::ConstLanelet current_shoulder_lane;
   lanelet::ConstLanelets shoulder_lanes;
   if (route_handler->getPullOutStartLane(
-        route_handler->getShoulderLanelets(), current_pose, vehicle_width,
-        &current_shoulder_lane)) {
+        route_handler->getShoulderLanelets(), start_pose, vehicle_width, &current_shoulder_lane)) {
     // pull out from shoulder lane
-    return route_handler->getShoulderLaneletSequence(current_shoulder_lane, current_pose);
+    return route_handler->getShoulderLaneletSequence(current_shoulder_lane, start_pose);
   }
 
   // pull out from road lane