From dd508d0af47bc8cc1ccfcdd047d48477615eb116 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 3 Jul 2023 20:46:54 +0900 Subject: [PATCH] feat(start_planner): add option for lane departure (#4151) Signed-off-by: kosuke55 shift --- .../config/start_planner/start_planner.param.yaml | 1 + .../docs/behavior_path_planner_start_planner_design.md | 1 + .../utils/start_planner/start_planner_parameters.hpp | 1 + .../src/behavior_path_planner_node.cpp | 2 ++ .../src/utils/start_planner/shift_pull_out.cpp | 6 ++++-- 5 files changed, 9 insertions(+), 2 deletions(-) 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 61167b37f875d..bea78664c65a3 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 @@ -9,6 +9,7 @@ th_moving_object_velocity: 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 524b647b74c76..2b98775e4fe81 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 dab5094413fd5..c379b0d513f9b 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 @@ -46,6 +46,7 @@ struct StartPlannerParameters double th_moving_object_velocity; // 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/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 787fc306a2f99..7965495b29cf1 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1067,6 +1067,8 @@ StartPlannerParameters BehaviorPathPlannerNode::getStartPlannerParam() p.th_moving_object_velocity = declare_parameter(ns + "th_moving_object_velocity"); // shift pull out p.enable_shift_pull_out = declare_parameter(ns + "enable_shift_pull_out"); + p.check_shift_path_lane_departure = + declare_parameter(ns + "check_shift_path_lane_departure"); p.minimum_shift_pull_out_distance = 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 d4b6b0a72a6db..90598f70c0ed6 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 @@ -103,8 +103,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; }