From c215d6f46c3517c411136f8e8023ecc102b9de97 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 27 Jun 2024 17:33:31 +0900 Subject: [PATCH] fix(lane_change): prevent empty path when rerouting (#7717) fix(lane_change): prevent empty path when routing Signed-off-by: kosuke55 --- .../src/scene.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 505bb3ef93340..ffee23dfc25d4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -467,8 +467,16 @@ void NormalLaneChange::insertStopPoint( PathWithLaneId NormalLaneChange::getReferencePath() const { - return utils::getCenterLinePathFromLanelet( - status_.lane_change_path.info.target_lanes.front(), planner_data_); + lanelet::ConstLanelet closest_lanelet; + if (!lanelet::utils::query::getClosestLanelet( + status_.lane_change_path.info.target_lanes, getEgoPose(), &closest_lanelet)) { + return prev_module_output_.reference_path; + } + const auto reference_path = utils::getCenterLinePathFromLanelet(closest_lanelet, planner_data_); + if (reference_path.points.empty()) { + return prev_module_output_.reference_path; + } + return reference_path; } std::optional NormalLaneChange::extendPath()