diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index ba9c46f157f2b..cf309a294d81f 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -855,12 +855,11 @@ bool RouteHandler::getNextLaneletWithinRoute( return false; } - lanelet::ConstLanelet start_lanelet; - const bool flag_check = getClosestLaneletWithinRoute(route_ptr_->start_pose, &start_lanelet); + const auto start_lane_id = route_ptr_->segments.front().preferred_primitive.id; const auto following_lanelets = routing_graph_ptr_->following(lanelet); for (const auto & llt : following_lanelets) { - if (!(flag_check && start_lanelet.id() == llt.id()) && exists(route_lanelets_, llt)) { + if (start_lane_id != llt.id() && exists(route_lanelets_, llt)) { *next_lanelet = llt; return true; }