From 87e95bf00a6ed23162d652fe38c97695f3ed5bd8 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Thu, 5 Dec 2024 08:13:07 +0900 Subject: [PATCH] feat(lane_changing): improve computation of lane changing acceleration (#9545) * allow decelerating in lane changing phase near terminal Signed-off-by: mohammad alqudah * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> * run pre-commit check Signed-off-by: mohammad alqudah * fix spelling Signed-off-by: mohammad alqudah * fix format Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --- .../README.md | 1 + .../config/lane_change.param.yaml | 1 + .../utils/calculation.hpp | 5 +++-- .../utils/parameters.hpp | 1 + .../src/manager.cpp | 4 ++++ .../src/utils/calculation.cpp | 14 ++++++++++---- 6 files changed, 20 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index d24a89bf03000..499327e3b8483 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -893,6 +893,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `trajectory.lat_acc_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 | | `trajectory.max_longitudinal_acc` | [m/s2] | double | maximum longitudinal acceleration for lane change | 1.0 | | `trajectory.min_longitudinal_acc` | [m/s2] | double | maximum longitudinal deceleration for lane change | -1.0 | +| `trajectory.lane_changing_decel_factor` | [-] | double | longitudinal deceleration factor during lane changing phase | 0.5 | | `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | | `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | | `lateral_acceleration.min_values` | [m/s2] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 6baf05edad484..572caedb58306 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -20,6 +20,7 @@ min_lane_changing_velocity: 2.78 lon_acc_sampling_num: 5 lat_acc_sampling_num: 3 + lane_changing_decel_factor: 0.5 # delay lane change delay_lane_change: diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index b6e6cb968ace8..b2aa21e4dc085 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -106,8 +106,9 @@ double calc_ego_dist_to_lanes_start( const lanelet::ConstLanelets & target_lanes); double calc_lane_changing_acceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc); + const CommonDataPtr & common_data_ptr, const double initial_lane_changing_velocity, + const double max_path_velocity, const double lane_changing_time, + const double prepare_longitudinal_acc); /** * @brief Calculates the distance required during a lane change operation. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp index 7f97eb872f795..31bd5d94f09c8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp @@ -122,6 +122,7 @@ struct TrajectoryParameters double th_prepare_length_diff{0.5}; double th_lane_changing_length_diff{0.5}; double min_lane_changing_velocity{5.6}; + double lane_changing_decel_factor{0.5}; int lon_acc_sampling_num{10}; int lat_acc_sampling_num{10}; LateralAccelerationMap lat_acc_map{}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index b193ce33b17fa..baba4311e2175 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -59,6 +59,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("trajectory.th_lane_changing_length_diff")); p.trajectory.min_lane_changing_velocity = getOrDeclareParameter(*node, parameter("trajectory.min_lane_changing_velocity")); + p.trajectory.lane_changing_decel_factor = + getOrDeclareParameter(*node, parameter("trajectory.lane_changing_decel_factor")); p.trajectory.lon_acc_sampling_num = getOrDeclareParameter(*node, parameter("trajectory.lon_acc_sampling_num")); p.trajectory.lat_acc_sampling_num = @@ -318,6 +320,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.min_longitudinal_acc); updateParam( parameters, ns + "max_longitudinal_acc", p->trajectory.max_longitudinal_acc); + updateParam( + parameters, ns + "lane_changing_decel_factor", p->trajectory.lane_changing_decel_factor); int longitudinal_acc_sampling_num = 0; updateParam(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num); if (longitudinal_acc_sampling_num > 0) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 9cbf9d4beb5ff..68b2debecc4d2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -382,11 +382,17 @@ std::vector calc_lon_acceleration_samples( } double calc_lane_changing_acceleration( - const double initial_lane_changing_velocity, const double max_path_velocity, - const double lane_changing_time, const double prepare_longitudinal_acc) + const CommonDataPtr & common_data_ptr, const double initial_lane_changing_velocity, + const double max_path_velocity, const double lane_changing_time, + const double prepare_longitudinal_acc) { if (prepare_longitudinal_acc <= 0.0) { - return 0.0; + const auto & params = common_data_ptr->lc_param_ptr->trajectory; + const auto lane_changing_acc = + common_data_ptr->transient_data.is_ego_near_current_terminal_start + ? prepare_longitudinal_acc * params.lane_changing_decel_factor + : 0.0; + return lane_changing_acc; } return std::clamp( @@ -497,7 +503,7 @@ std::vector calc_shift_phase_metrics( shift_length, common_data_ptr->lc_param_ptr->trajectory.lateral_jerk, lat_acc); const double lane_changing_accel = calc_lane_changing_acceleration( - initial_velocity, max_path_velocity, lane_changing_duration, lon_accel); + common_data_ptr, initial_velocity, max_path_velocity, lane_changing_duration, lon_accel); const auto lane_changing_length = calculation::calc_phase_length( initial_velocity, max_vel, lane_changing_accel, lane_changing_duration);