diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index 80114889d7e74..fe98d7aa2a839 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -8,6 +8,7 @@ collision_check_distance_from_end: 1.0 # shift pull out enable_shift_pull_out: true + check_shift_path_lane_departure: false minimum_shift_pull_out_distance: 0.0 deceleration_interval: 15.0 lateral_jerk: 0.5 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md index 37ff48bb717d5..345cb5d1f4329 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md @@ -105,6 +105,7 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral | Name | Unit | Type | Description | Default value | | :------------------------------ | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | | enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true | +| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | false | | shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | | pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | | maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 02a3b6169166a..73d922cfde270 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -35,6 +35,7 @@ struct StartPlannerParameters double collision_check_distance_from_end; // shift pull out bool enable_shift_pull_out; + bool check_shift_path_lane_departure; double minimum_shift_pull_out_distance; int lateral_acceleration_sampling_num; double lateral_jerk; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index bba6f2b7b0da3..23265e4058956 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -44,6 +44,8 @@ StartPlannerModuleManager::StartPlannerModuleManager( node->declare_parameter(ns + "collision_check_distance_from_end"); // shift pull out p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); + p.check_shift_path_lane_departure = + node->declare_parameter(ns + "check_shift_path_lane_departure"); p.minimum_shift_pull_out_distance = node->declare_parameter(ns + "minimum_shift_pull_out_distance"); p.lateral_acceleration_sampling_num = diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index b11bb7731b826..fc24c3d9f2d20 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -100,8 +100,10 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) 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); - if (lane_departure_checker_->checkPathWillLeaveLane( - utils::transformToLanelets(expanded_lanes), path_start_to_end)) { + if ( + parameters_.check_shift_path_lane_departure && + lane_departure_checker_->checkPathWillLeaveLane( + utils::transformToLanelets(expanded_lanes), path_start_to_end)) { continue; }