fix(start_planner): invalid lane id access (autowarefoundation#4511)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

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

@@ -551,7 +551,9 @@ lanelet::ConstLanelets StartPlannerModule::getPathLanes(const PathWithLaneId & p
   lanelet::ConstLanelets path_lanes;
   path_lanes.reserve(lane_ids.size());
   for (const auto & id : lane_ids) {
-    path_lanes.push_back(lanelet_layer.get(id));
+    if (id != lanelet::InvalId) {
+      path_lanes.push_back(lanelet_layer.get(id));
+    }
   }
 
   return path_lanes;
@@ -573,6 +575,11 @@ void StartPlannerModule::updatePullOutStatus()
     status_ = PullOutStatus();
   }
 
+  // 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 && !last_pull_out_start_update_time_ && !status_.back_finished) {
@@ -590,11 +597,6 @@ void StartPlannerModule::updatePullOutStatus()
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
   const auto & goal_pose = planner_data_->route_handler->getGoalPose();
 
-  // 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);
-
   // search pull out start candidates backward
   std::vector<Pose> start_pose_candidates = searchPullOutStartPoses();
   planWithPriority(start_pose_candidates, goal_pose, parameters_->search_priority);