fix(route_handler): fix getting next lane logic to prevent from unexp…

…ected path cut (autowarefoundation#5355)

Signed-off-by: satoshi-ota <[email protected]>

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;
     }