fix(route_handler): make new route continuous with previous route (autowarefoundation#8238)

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

Author: rej55

planning/autoware_route_handler/src/route_handler.cpp

@@ -2143,6 +2143,14 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
       continue;
     }
     is_route_found = true;
+    lanelet::ConstLanelet preferred_lane{};
+    if (getClosestPreferredLaneletWithinRoute(start_checkpoint, &preferred_lane)) {
+      if (st_llt.id() == preferred_lane.id()) {
+        shortest_path = optional_route->shortestPath();
+        start_lanelet = st_llt;
+        break;
+      }
+    }
     if (angle_diff < smallest_angle_diff) {
       smallest_angle_diff = angle_diff;
       shortest_path = optional_route->shortestPath();