Merge pull request #1834 from tier4/sync-awf-latest

chore: sync awf-latest
tier4 · Feb 17, 2025 · e8b2340 · e8b2340
2 parents 6200006 + 51d9e75
commit e8b2340
Showing 1 changed file with 16 additions and 0 deletions.
diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp
@@ -1928,6 +1928,22 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
         }
       }
     }
+    if (getClosestLaneletWithinRoute(goal_checkpoint, &closest_lanelet)) {
+      if (std::find(candidates.begin(), candidates.end(), closest_lanelet) != candidates.end()) {
+        if (lanelet::utils::isInLanelet(goal_checkpoint, closest_lanelet)) {
+          std::stringstream preferred_lanelets_str;
+          for (const auto & preferred_lanelet : preferred_lanelets_) {
+            preferred_lanelets_str << preferred_lanelet.id() << ", ";
+          }
+          RCLCPP_WARN(
+            logger_,
+            "Failed to find reroute on previous preferred lanelets %s, but on previous route "
+            "segment %ld still",
+            preferred_lanelets_str.str().c_str(), closest_lanelet.id());
+          return closest_lanelet;
+        }
+      }
+    }
     return std::nullopt;
   };
   if (auto closest_lanelet = findGoalClosestPreferredLanelet()) {