diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index 09be040019338..f75358601d877 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -55,7 +55,6 @@ std::optional GeometricPullOver::plan( 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 = @@ -69,10 +68,12 @@ std::optional GeometricPullOver::plan( 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 {}; auto pull_over_path_opt = PullOverPath::create( getPlannerType(), id, planner_.getPaths(), planner_.getStartPose(), modified_goal_pose, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index e1184c49e40ba..da99c830c68bb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -292,18 +292,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.parking_path()); - }); + + 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.parking_path()); + if (!is_in_parking_lots && !is_in_lanes) { return {}; }