feat(goal_planner): check opposite lane for lane departure_check

Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Dec 3, 2024 · a125a20 · a125a20
1 parent fdc7a55
commit a125a20
Showing 1 changed file with 11 additions and 2 deletions.
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp
@@ -719,8 +719,17 @@ lanelet::Lanelet createDepartureCheckLanelet(
   };
 
   const auto getMostInnerLane = [&](const lanelet::ConstLanelet & lane) -> lanelet::ConstLanelet {
-    return left_side_parking ? route_handler.getMostRightLanelet(lane, false, true)
-                             : route_handler.getMostLeftLanelet(lane, false, true);
+    if (left_side_parking) {
+      const auto most_right_lane = route_handler.getMostRightLanelet(lane, true, true);
+      const auto most_right_opposite_lanes =
+        route_handler.getRightOppositeLanelets(most_right_lane);
+      return most_right_opposite_lanes.empty() ? most_right_lane
+                                               : most_right_opposite_lanes.front();
+    } else {
+      const auto most_left_lane = route_handler.getMostLeftLanelet(lane, true, true);
+      const auto most_left_opposite_lanes = route_handler.getLeftOppositeLanelets(most_left_lane);
+      return most_left_opposite_lanes.empty() ? most_left_lane : most_left_opposite_lanes.front();
+    }
   };
 
   lanelet::Points3d outer_bound_points{};