From ce3db689c924f192350c23f3e35ed8f3acc52e5a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 25 Oct 2023 11:39:12 +0900 Subject: [PATCH] fix(lane_change): fix terminal stop distance (#5392) * fix(lane_change): fix terminal stop distance Signed-off-by: kosuke55 * make current lanes include path front id Signed-off-by: kosuke55 * fixup! make current lanes include path front id --------- Signed-off-by: kosuke55 --- .../behavior_path_planner/src/utils/utils.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 9b10dfc32457f..35dd7675ae3fc 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -3081,9 +3081,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 'current_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(