fix function getClosestRouteLaneletFromLanelet

Signed-off-by: mohammad alqudah <[email protected]>
autowarefoundation · Jun 3, 2024 · a5bad49 · a5bad49
1 parent a7cf3b6
commit a5bad49
Showing 1 changed file with 1 addition and 3 deletions.
diff --git 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());