fix function getClosestRouteLaneletFromLanelet

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>
autowarefoundation · Jun 10, 2024 · 14553d1 · 14553d1
1 parent 11a4478
commit 14553d1
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());