feat(goal_planner): check opposite lane for lane departure_check (aut…

…owarefoundation#9460) * feat(goal_planner): check opposite lane for lane departure_check Signed-off-by: kosuke55 <[email protected]> * refactor getMostInnerLane Signed-off-by: kosuke55 <[email protected]> --------- Signed-off-by: kosuke55 <[email protected]>
vividf · Dec 6, 2024 · ede3f23 · ede3f23
1 parent 0bbdbe1
commit ede3f23
Showing 1 changed file with 7 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
@@ -723,8 +723,13 @@ 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);
+    const auto getInnerLane =
+      left_side_parking ? &RouteHandler::getMostRightLanelet : &RouteHandler::getMostLeftLanelet;
+    const auto getOppositeLane = left_side_parking ? &RouteHandler::getRightOppositeLanelets
+                                                   : &RouteHandler::getLeftOppositeLanelets;
+    const auto inner_lane = (route_handler.*getInnerLane)(lane, true, true);
+    const auto opposite_lanes = (route_handler.*getOppositeLane)(inner_lane);
+    return opposite_lanes.empty() ? inner_lane : opposite_lanes.front();
   };
 
   lanelet::Points3d outer_bound_points{};