diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 2d402bb3523b5..de120592e7505 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/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( diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 6927327e24ef4..9c42d1b5e90ef 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -3067,9 +3067,24 @@ lanelet::ConstLanelets getCurrentLanesFromPath( lanelet::ConstLanelet current_lane; lanelet::utils::query::getClosestLanelet(reference_lanes, current_pose, ¤t_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 = extendPrevLane(route_handler, current_lanes); + if (extended_lanes.size() == current_lanes.size()) break; + current_lanes = extended_lanes; + } + + return current_lanes; } lanelet::ConstLanelets extendNextLane(