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 b7b4ad83de362..14307261ce7f5 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 @@ -16,6 +16,7 @@ #include "autoware/behavior_path_goal_planner_module/util.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include #include @@ -77,6 +78,10 @@ std::optional GeometricPullOver::plan( const auto arc_path = planner_.getArcPath(); // check lane departure with road and shoulder lanes + // To improve the accuracy of lane departure detection, make the sampling interval finer + // todo: Implement lane departure detection that does not depend on the footprint + const auto resampled_arc_path = + utils::resamplePathWithSpline(arc_path, parameters_.center_line_path_interval / 2); if (lane_departure_checker_.checkPathWillLeaveLane({departure_check_lane}, arc_path)) return {}; auto pull_over_path_opt = PullOverPath::create( 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 c8e0788e39b6f..83628bb483e56 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 @@ -299,8 +299,12 @@ std::optional ShiftPullOver::generatePullOverPath( 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()); + // To improve the accuracy of lane departure detection, make the sampling interval finer + // todo: Implement lane departure detection that does not depend on the footprint + const auto resampled_parking_path = utils::resamplePathWithSpline( + pull_over_path.parking_path(), parameters_.center_line_path_interval / 2); + const bool is_in_lanes = + !lane_departure_checker_.checkPathWillLeaveLane({departure_check_lane}, resampled_parking_path); if (!is_in_parking_lots && !is_in_lanes) { return {};