Update StartPlannerModule with pull_out_lanes

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

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

@@ -730,13 +730,15 @@ void StartPlannerModule::updateStatusIfNotBackwardDrivingComplete()
   std::cerr << "status is updated in " << __func__ << std::endl;
   const auto & route_handler = planner_data_->route_handler;
   const auto & 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 std::lock_guard<std::mutex> lock(mutex_);
   std::cerr << "backward_path_is_enabled is disabled " << __func__ << std::endl;
   status_.backward_path_is_enabled = true;
   status_.stop_path = PathWithLaneId{};
   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);
   status_.found_pull_out_path = true;
   // status_.planner_type = PlannerType::BACK;
@@ -765,8 +767,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;
   };
@@ -811,20 +816,21 @@ std::vector<DrivableLanes> StartPlannerModule::generateDrivableLanes(
   const PathWithLaneId & path) const
 {
   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);
   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;
@@ -842,8 +848,12 @@ void StartPlannerModule::updatePullOutStatus()
   // save pull out lanes which is generated using current pose before starting pull out
   // (before approval)
   // postProcessに入れられる？
-  status_.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);
+  // 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