From b0bdcd0cf85bfe99cbc4d8c90a3012ec40506673 Mon Sep 17 00:00:00 2001 From: t4-x2 Date: Thu, 26 Oct 2023 11:36:59 +0900 Subject: [PATCH] Revert "fix(lane_change): fix terminal stop distance (#5392)" This reverts commit 7da42ee0f8cd835ac96f65a528e5a3933685a9a7. --- .../behavior_path_planner/src/utils/utils.cpp | 19 ++----------------- 1 file changed, 2 insertions(+), 17 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 9eb79170ebe1..d08074b63350 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2994,24 +2994,9 @@ lanelet::ConstLanelets getCurrentLanesFromPath( lanelet::ConstLanelet current_lane; lanelet::utils::query::getClosestLanelet(reference_lanes, current_pose, ¤t_lane); - 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; + return route_handler->getLaneletSequence( + current_lane, current_pose, p.backward_path_length, p.forward_path_length); } lanelet::ConstLanelets extendNextLane(