From e229603c22fc00ae693b37e983664148adb96783 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 15 May 2023 18:52:25 +0900 Subject: [PATCH] fix(behavior_path_planner): handle overlap lane in isEgoOutOfRoute() (#3715) Signed-off-by: Fumiya Watanabe --- planning/behavior_path_planner/src/utils/utils.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 1fbdd6b542394..12e054cd0531f 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1018,7 +1018,10 @@ bool isEgoOutOfRoute( } // If ego vehicle is over goal on goal lane, return true - if (lanelet::utils::isInLanelet(self_pose, goal_lane)) { + lanelet::ConstLanelet closest_lane; + const double yaw_threshold = tier4_autoware_utils::deg2rad(90); + if (lanelet::utils::query::getClosestLaneletWithConstrains( + {goal_lane}, self_pose, &closest_lane, 0.0, yaw_threshold)) { constexpr double buffer = 1.0; const auto ego_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, self_pose); const auto goal_arc_coord =