diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index f8a894c2ff3c1..2112e5263fd4a 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());