From 51d9e752d53eb17eb3a404bc6eee3e391c82dbb9 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 17 Feb 2025 18:10:28 +0900 Subject: [PATCH] fix(route_handler): use non-preferred route segment even if checkpoint is not on preferred lanelet at best (#10138) Signed-off-by: Mamoru Sobue --- .../autoware_route_handler/src/route_handler.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index ac525c8915780..46e8a01ad31c3 100644 --- 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()) {