From ede3f237c0b4c3632ba2a24aaddb4f2a4deb5573 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 6 Dec 2024 16:14:09 +0900 Subject: [PATCH] feat(goal_planner): check opposite lane for lane departure_check (#9460) * feat(goal_planner): check opposite lane for lane departure_check Signed-off-by: kosuke55 * refactor getMostInnerLane Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../src/util.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 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 index 8ca9d9ab45dc3..a2a64fc700676 100644 --- 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{};