fix(behavior_path_planner): handle overlap lane in isEgoOutOfRoute() (a…

…utowarefoundation#3715) Signed-off-by: Fumiya Watanabe <[email protected]>
Interplai · May 15, 2023 · e229603 · e229603
1 parent 7afa349
commit e229603
Showing 1 changed file with 4 additions and 1 deletion.
diff --git 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 =