From bf43e911d48f4814f6ea1c866ead44f9ae58aa7f Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 7 Apr 2023 01:20:56 +0900 Subject: [PATCH] feat(behavior_path_planner): add new lateral acc condition Signed-off-by: yutaka --- .../config/lane_change/lane_change.param.yaml | 6 ++++-- .../util/lane_change/lane_change_module_data.hpp | 4 +++- .../src/behavior_path_planner_node.cpp | 4 ++++ .../behavior_path_planner/src/util/lane_change/util.cpp | 5 ++++- 4 files changed, 15 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 7b619c1711d74..750cfe4bff161 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -8,8 +8,10 @@ backward_length_buffer_for_end_of_lane: 3.0 # [m] lane_change_finish_judge_buffer: 2.0 # [m] - lane_changing_lateral_jerk: 0.5 # [m/s3] - lane_changing_lateral_acc: 0.5 # [m/s2] + lane_changing_lateral_jerk: 0.5 # [m/s3] + lane_changing_lateral_acc: 0.315 # [m/s2] + lane_changing_lateral_acc_at_low_velocity: 0.15 # [m/s2] + lateral_acc_switching_velocity: 4.0 #[m/s] minimum_lane_changing_velocity: 2.78 # [m/s] prediction_time_resolution: 0.5 # [s] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp index ae388a9a3a0b8..a775e8a584c1c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp @@ -28,7 +28,9 @@ struct LaneChangeParameters // trajectory generation double prepare_duration{2.0}; double lane_changing_lateral_jerk{0.5}; - double lane_changing_lateral_acc{0.5}; + double lane_changing_lateral_acc{0.315}; + double lane_changing_lateral_acc_at_low_velocity{0.15}; + double lateral_acc_switching_velocity{0.4}; double lane_change_finish_judge_buffer{3.0}; double minimum_lane_changing_velocity{5.6}; double prediction_time_resolution{0.5}; 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 473d8596d0af3..6afeced899062 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -718,6 +718,10 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.prepare_duration = declare_parameter(parameter("prepare_duration")); p.lane_changing_lateral_jerk = declare_parameter(parameter("lane_changing_lateral_jerk")); p.lane_changing_lateral_acc = declare_parameter(parameter("lane_changing_lateral_acc")); + p.lane_changing_lateral_acc_at_low_velocity = + declare_parameter(parameter("lane_changing_lateral_acc_at_low_velocity")); + p.lateral_acc_switching_velocity = + declare_parameter(parameter("lateral_acc_switching_velocity")); p.lane_change_finish_judge_buffer = declare_parameter(parameter("lane_change_finish_judge_buffer")); p.minimum_lane_changing_velocity = diff --git a/planning/behavior_path_planner/src/util/lane_change/util.cpp b/planning/behavior_path_planner/src/util/lane_change/util.cpp index 45cd38f761cf3..d513de1a664ca 100644 --- a/planning/behavior_path_planner/src/util/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/util/lane_change/util.cpp @@ -741,8 +741,11 @@ double calcLaneChangingDistance( const double lane_changing_speed, const double shift_length, const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param) { + const double lateral_acc = lane_changing_speed < lc_param.lateral_acc_switching_velocity + ? lc_param.lane_changing_lateral_acc_at_low_velocity + : lc_param.lane_changing_lateral_acc; const auto required_time = PathShifter::calcShiftTimeFromJerk( - shift_length, lc_param.lane_changing_lateral_jerk, lc_param.lane_changing_lateral_acc); + shift_length, lc_param.lane_changing_lateral_jerk, lateral_acc); const double & min_lane_change_length = com_param.minimum_lane_changing_length; const double lane_changing_distance =