make current lanes include path front id

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -403,9 +403,7 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal()
 
 lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const
 {
-  return utils::getCurrentLanes(
-    planner_data_, lane_change_parameters_->backward_lane_length,
-    planner_data_->parameters.forward_path_length);
+  return utils::getCurrentLanesFromPath(prev_module_path_, planner_data_);
 }
 
 lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes(

planning/behavior_path_planner/src/utils/utils.cpp

@@ -3068,8 +3068,24 @@ lanelet::ConstLanelets getCurrentLanesFromPath(
   lanelet::ConstLanelet current_lane;
   lanelet::utils::query::getClosestLanelet(reference_lanes, current_pose, &current_lane);
 
-  return route_handler->getLaneletSequence(
+  auto current_lanes = route_handler->getLaneletSequence(
     current_lane, current_pose, p.backward_path_length, p.forward_path_length);
+
+  // Extend the 'lanes' with previous lanes until it contains 'front_lane_ids'.
+  const auto front_lane_ids = path.points.front().lane_ids;
+  auto have_front_lanes = [front_lane_ids](const auto & lanes) {
+    return std::any_of(lanes.begin(), lanes.end(), [&](const auto & lane) {
+      return std::find(front_lane_ids.begin(), front_lane_ids.end(), lane.id()) !=
+             front_lane_ids.end();
+    });
+  };
+  while (!have_front_lanes(current_lanes)) {
+    const auto extended_lanes = extendNextLane(route_handler, current_lanes);
+    if (extended_lanes.size() == current_lanes.size()) break;
+    current_lanes = extended_lanes;
+  }
+
+  return current_lanes;
 }
 
 lanelet::ConstLanelets extendNextLane(