From 58603c5cce31dd01d1121103c8876add0ff06e79 Mon Sep 17 00:00:00 2001 From: yutaka Date: Tue, 25 Apr 2023 00:39:16 +0900 Subject: [PATCH 1/2] refactor(behavior_path_planeer): use common.params for lane change Signed-off-by: yutaka --- .../behavior_planning/behavior_planning.launch.py | 6 ++++-- .../config/lane_change/lane_change.param.yaml | 1 - .../include/behavior_path_planner/parameters.hpp | 4 ++++ .../utils/lane_change/lane_change_module_data.hpp | 1 - .../src/behavior_path_planner_node.cpp | 12 ++++-------- .../src/scene_module/lane_change/normal.cpp | 14 +++++++------- .../src/utils/lane_change/utils.cpp | 6 +++--- 7 files changed, 22 insertions(+), 22 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index d9ced3f1100f..dc0769abac58 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -35,6 +35,9 @@ def launch_setup(context, *args, **kwargs): with open(vehicle_param_path, "r") as f: vehicle_param = yaml.safe_load(f)["/**"]["ros__parameters"] + # common parameter + with open(LaunchConfiguration("common_param_path").perform(context), "r") as f: + common_param = yaml.safe_load(f)["/**"]["ros__parameters"] # nearest search parameter with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f: nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] @@ -82,6 +85,7 @@ def launch_setup(context, *args, **kwargs): ("~/output/modified_goal", "/planning/scenario_planning/modified_goal"), ], parameters=[ + common_param, nearest_search_param, side_shift_param, avoidance_param, @@ -113,8 +117,6 @@ def launch_setup(context, *args, **kwargs): ) # smoother param - with open(LaunchConfiguration("common_param_path").perform(context), "r") as f: - common_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open( LaunchConfiguration("motion_velocity_smoother_param_path").perform(context), "r" ) as f: 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 905a3037036b..91c15a2d4c86 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 @@ -15,7 +15,6 @@ minimum_lane_changing_velocity: 2.78 # [m/s] prediction_time_resolution: 0.5 # [s] - maximum_deceleration: 1.0 # [m/s2] lane_change_sampling_num: 3 # target object diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index b8e911d8dc8b..a49a13b0ffe0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -49,6 +49,10 @@ struct BehaviorPathPlannerParameters double backward_length_buffer_for_end_of_pull_over; double backward_length_buffer_for_end_of_pull_out; + // common parameters + double min_acc; + double max_acc; + // lane change parameters double lane_changing_lateral_jerk{0.5}; double lane_changing_lateral_acc{0.315}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index a7bab00979c9..5278cb6ca9ac 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -29,7 +29,6 @@ struct LaneChangeParameters double prepare_duration{2.0}; double lane_change_finish_judge_buffer{3.0}; double prediction_time_resolution{0.5}; - double maximum_deceleration{1.0}; int lane_change_sampling_num{10}; // collision check 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 bd995f556da5..83c3a59cde5d 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -377,6 +377,10 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.backward_path_length = declare_parameter("backward_path_length") + backward_offset; p.forward_path_length = declare_parameter("forward_path_length"); + // acceleration parameters + p.min_acc = declare_parameter("normal.min_acc"); + p.max_acc = declare_parameter("normal.max_acc"); + // lane change parameters p.backward_length_buffer_for_end_of_lane = declare_parameter("lane_change.backward_length_buffer_for_end_of_lane"); @@ -652,7 +656,6 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.lane_change_finish_judge_buffer = declare_parameter(parameter("lane_change_finish_judge_buffer")); p.prediction_time_resolution = declare_parameter(parameter("prediction_time_resolution")); - p.maximum_deceleration = declare_parameter(parameter("maximum_deceleration")); p.lane_change_sampling_num = declare_parameter(parameter("lane_change_sampling_num")); // collision check @@ -695,13 +698,6 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() << "Terminating the program..."); exit(EXIT_FAILURE); } - if (p.maximum_deceleration < 0.0) { - RCLCPP_FATAL_STREAM( - get_logger(), "maximum_deceleration cannot be negative value. Given parameter: " - << p.maximum_deceleration << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); - } if (p.abort_delta_time < 1.0) { RCLCPP_FATAL_STREAM( diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index ebf95cef2b0b..21a5287bd3f4 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -310,13 +310,13 @@ bool NormalLaneChange::getLaneChangePaths( const auto current_velocity = getEgoTwist().linear.x; // compute maximum_deceleration - const auto maximum_deceleration = std::invoke([&minimum_lane_changing_velocity, ¤t_velocity, - this]() { - const double min_a = - (minimum_lane_changing_velocity - current_velocity) / parameters_->prepare_duration; - return std::clamp( - min_a, -std::abs(parameters_->maximum_deceleration), -std::numeric_limits::epsilon()); - }); + const auto maximum_deceleration = + std::invoke([&minimum_lane_changing_velocity, ¤t_velocity, &common_parameter, this]() { + const double min_a = + (minimum_lane_changing_velocity - current_velocity) / parameters_->prepare_duration; + return std::clamp( + min_a, -std::abs(common_parameter.min_acc), -std::numeric_limits::epsilon()); + }); const auto acceleration_resolution = std::abs(maximum_deceleration) / lane_change_sampling_num; diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index e148d47e0ae1..7c2f292c1a68 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -230,12 +230,12 @@ bool getLaneChangePaths( const auto current_velocity = twist.linear.x; // compute maximum_deceleration - const auto maximum_deceleration = - std::invoke([&minimum_lane_changing_velocity, ¤t_velocity, ¶meter]() { + const auto maximum_deceleration = std::invoke( + [&minimum_lane_changing_velocity, ¤t_velocity, &common_parameter, ¶meter]() { const double min_a = (minimum_lane_changing_velocity - current_velocity) / parameter.prepare_duration; return std::clamp( - min_a, -std::abs(parameter.maximum_deceleration), -std::numeric_limits::epsilon()); + min_a, -std::abs(common_parameter.min_acc), -std::numeric_limits::epsilon()); }); const auto acceleration_resolution = std::abs(maximum_deceleration) / lane_change_sampling_num; From 2604dbc106e2b891bee2cebb43e3af8c101a6023 Mon Sep 17 00:00:00 2001 From: yutaka Date: Tue, 25 Apr 2023 11:19:55 +0900 Subject: [PATCH 2/2] update Signed-off-by: yutaka --- .../behavior_planning.launch.xml | 1 + ...ehavior_path_planner_lane_change_design.md | 27 +++++++++---------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index bc8846e5e41f..35b8da526e98 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -14,6 +14,7 @@ + diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index 03d2edeee53f..a4a1164cfc47 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -407,20 +407,19 @@ The last behavior will also occur if the ego vehicle has departed from the curre The following parameters are configurable in `lane_change.param.yaml`. -| Name | Unit | Type | Description | Default value | -| :--------------------------------------- | ------- | ------ | --------------------------------------------------------------------------------------- | ------------- | -| `lane_change_prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | -| `lane_changing_safety_check_duration` | [m] | double | The total time that is taken to complete the lane-changing task. | 8.0 | -| `minimum_lane_change_prepare_distance` | [m] | double | Minimum prepare distance for lane change | 2.0 | -| `minimum_lane_change_length` | [m] | double | The minimum distance needed for changing lanes. | 16.5 | -| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 2.0 | -| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | -| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | -| `lane_changing_lateral_acc` | [m/s2] | double | Lateral acceleration value for lane change path generation | 0.5 | -| `minimum_lane_change_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | -| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `maximum_deceleration` | [m/s^2] | double | Ego vehicle maximum deceleration when performing lane change. | 1.0 | -| `lane_change_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by deceleration | 10 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------------- | ------ | ------ | --------------------------------------------------------------------------------------- | ------------- | +| `lane_change_prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `lane_changing_safety_check_duration` | [m] | double | The total time that is taken to complete the lane-changing task. | 8.0 | +| `minimum_lane_change_prepare_distance` | [m] | double | Minimum prepare distance for lane change | 2.0 | +| `minimum_lane_change_length` | [m] | double | The minimum distance needed for changing lanes. | 16.5 | +| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 2.0 | +| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | +| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | +| `lane_changing_lateral_acc` | [m/s2] | double | Lateral acceleration value for lane change path generation | 0.5 | +| `minimum_lane_change_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | +| `prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | +| `lane_change_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by deceleration | 10 | ### Collision checks during lane change