From 30ba1bdbd8ff8ea21bdc7563f1f22a0c13355206 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 3 Dec 2024 20:55:22 +0900 Subject: [PATCH] refactor getMostInnerLane Signed-off-by: kosuke55 --- .../src/util.cpp | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 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 0f54842c80cef..12fbb02acd65c 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 @@ -719,17 +719,13 @@ lanelet::Lanelet createDepartureCheckLanelet( }; const auto getMostInnerLane = [&](const lanelet::ConstLanelet & lane) -> lanelet::ConstLanelet { - 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(); - } + const auto getInnerLanelet = + left_side_parking ? &RouteHandler::getMostRightLanelet : &RouteHandler::getMostLeftLanelet; + const auto getOppositeLanelets = left_side_parking ? &RouteHandler::getRightOppositeLanelets + : &RouteHandler::getLeftOppositeLanelets; + const auto inner_lane = (route_handler.*getInnerLanelet)(lane, true, true); + const auto opposite_lanes = (route_handler.*getOppositeLanelets)(inner_lane); + return opposite_lanes.empty() ? inner_lane : opposite_lanes.front(); }; lanelet::Points3d outer_bound_points{};