From 14553d1db47337c50ce776c6ee87e2e373cd4970 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Mon, 3 Jun 2024 13:54:14 +0900 Subject: [PATCH] fix function getClosestRouteLaneletFromLanelet Signed-off-by: mohammad alqudah --- planning/route_handler/src/route_handler.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index caf3e5b6ee73b..4d1632382e20f 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -903,9 +903,7 @@ bool RouteHandler::getClosestRouteLaneletFromLanelet( lanelet_sequence = previous_lanelets; } - const auto & ref_lanelet_neighbors = getNeighborsWithinRoute(reference_lanelet); - lanelet_sequence.insert( - lanelet_sequence.end(), ref_lanelet_neighbors.begin(), ref_lanelet_neighbors.end()); + lanelet_sequence.push_back(reference_lanelet); if (getNextLaneletsWithinRoute(reference_lanelet, &next_lanelets)) { lanelet_sequence.insert(lanelet_sequence.end(), next_lanelets.begin(), next_lanelets.end());