From 242928065df21cfe1b7e3753046491ba0e81e97f Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 22 Nov 2024 16:05:11 +0900 Subject: [PATCH] fix(goal_planner): use departure_check_lane for path (#9423) --- .../src/geometric_pull_over.cpp | 5 +++-- .../src/shift_pull_over.cpp | 18 ++++++------------ 2 files changed, 9 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp index 62c51408a000b..ee28b64d5bac3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp @@ -51,7 +51,6 @@ std::optional GeometricPullOver::plan(const Pose & goal_pose) if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } - const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes); const auto & p = parallel_parking_parameters_; const double max_steer_angle = @@ -65,10 +64,12 @@ std::optional GeometricPullOver::plan(const Pose & goal_pose) return {}; } + const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet( + pull_over_lanes, *planner_data_->route_handler, left_side_parking_); const auto arc_path = planner_.getArcPath(); // check lane departure with road and shoulder lanes - if (lane_departure_checker_.checkPathWillLeaveLane(lanes, arc_path)) return {}; + if (lane_departure_checker_.checkPathWillLeaveLane({departure_check_lane}, arc_path)) return {}; PullOverPath pull_over_path{}; pull_over_path.type = getPlannerType(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp index 9f320337b3d08..4038194f4072f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -283,18 +283,12 @@ std::optional ShiftPullOver::generatePullOverPath( return is_footprint_in_any_polygon(footprint); }); }); - const bool is_in_lanes = std::invoke([&]() -> bool { - const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_over_lanes); - const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip); - const auto combined_drivable = utils::combineDrivableLanes( - expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes); - return !lane_departure_checker_.checkPathWillLeaveLane( - utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath()); - }); + + const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet( + pull_over_lanes, *planner_data_->route_handler, left_side_parking_); + const bool is_in_lanes = !lane_departure_checker_.checkPathWillLeaveLane( + {departure_check_lane}, pull_over_path.getParkingPath()); + if (!is_in_parking_lots && !is_in_lanes) { return {}; }