Skip to content

Commit 87e95bf

Browse files
mkqudamaxime-clem
andauthored
feat(lane_changing): improve computation of lane changing acceleration (#9545)
* allow decelerating in lane changing phase near terminal Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * 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 <alqudah.mohammad@tier4.jp> * fix spelling Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> * fix format Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> --------- Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp> Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>
1 parent 1594487 commit 87e95bf

File tree

6 files changed

+20
-6
lines changed

6 files changed

+20
-6
lines changed

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md

+1
Original file line numberDiff line numberDiff line change
@@ -893,6 +893,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
893893
| `trajectory.lat_acc_sampling_num` | [-] | int | Number of possible lane-changing trajectories that are being influenced by lateral acceleration | 3 |
894894
| `trajectory.max_longitudinal_acc` | [m/s2] | double | maximum longitudinal acceleration for lane change | 1.0 |
895895
| `trajectory.min_longitudinal_acc` | [m/s2] | double | maximum longitudinal deceleration for lane change | -1.0 |
896+
| `trajectory.lane_changing_decel_factor` | [-] | double | longitudinal deceleration factor during lane changing phase | 0.5 |
896897
| `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 |
897898
| `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] |
898899
| `lateral_acceleration.min_values` | [m/s2] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] |

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
min_lane_changing_velocity: 2.78
2121
lon_acc_sampling_num: 5
2222
lat_acc_sampling_num: 3
23+
lane_changing_decel_factor: 0.5
2324

2425
# delay lane change
2526
delay_lane_change:

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -106,8 +106,9 @@ double calc_ego_dist_to_lanes_start(
106106
const lanelet::ConstLanelets & target_lanes);
107107

108108
double calc_lane_changing_acceleration(
109-
const double initial_lane_changing_velocity, const double max_path_velocity,
110-
const double lane_changing_time, const double prepare_longitudinal_acc);
109+
const CommonDataPtr & common_data_ptr, const double initial_lane_changing_velocity,
110+
const double max_path_velocity, const double lane_changing_time,
111+
const double prepare_longitudinal_acc);
111112

112113
/**
113114
* @brief Calculates the distance required during a lane change operation.

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -122,6 +122,7 @@ struct TrajectoryParameters
122122
double th_prepare_length_diff{0.5};
123123
double th_lane_changing_length_diff{0.5};
124124
double min_lane_changing_velocity{5.6};
125+
double lane_changing_decel_factor{0.5};
125126
int lon_acc_sampling_num{10};
126127
int lat_acc_sampling_num{10};
127128
LateralAccelerationMap lat_acc_map{};

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

+4
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s
5959
getOrDeclareParameter<double>(*node, parameter("trajectory.th_lane_changing_length_diff"));
6060
p.trajectory.min_lane_changing_velocity =
6161
getOrDeclareParameter<double>(*node, parameter("trajectory.min_lane_changing_velocity"));
62+
p.trajectory.lane_changing_decel_factor =
63+
getOrDeclareParameter<double>(*node, parameter("trajectory.lane_changing_decel_factor"));
6264
p.trajectory.lon_acc_sampling_num =
6365
getOrDeclareParameter<int>(*node, parameter("trajectory.lon_acc_sampling_num"));
6466
p.trajectory.lat_acc_sampling_num =
@@ -318,6 +320,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
318320
parameters, ns + "min_longitudinal_acc", p->trajectory.min_longitudinal_acc);
319321
updateParam<double>(
320322
parameters, ns + "max_longitudinal_acc", p->trajectory.max_longitudinal_acc);
323+
updateParam<double>(
324+
parameters, ns + "lane_changing_decel_factor", p->trajectory.lane_changing_decel_factor);
321325
int longitudinal_acc_sampling_num = 0;
322326
updateParam<int>(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num);
323327
if (longitudinal_acc_sampling_num > 0) {

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp

+10-4
Original file line numberDiff line numberDiff line change
@@ -382,11 +382,17 @@ std::vector<double> calc_lon_acceleration_samples(
382382
}
383383

384384
double calc_lane_changing_acceleration(
385-
const double initial_lane_changing_velocity, const double max_path_velocity,
386-
const double lane_changing_time, const double prepare_longitudinal_acc)
385+
const CommonDataPtr & common_data_ptr, const double initial_lane_changing_velocity,
386+
const double max_path_velocity, const double lane_changing_time,
387+
const double prepare_longitudinal_acc)
387388
{
388389
if (prepare_longitudinal_acc <= 0.0) {
389-
return 0.0;
390+
const auto & params = common_data_ptr->lc_param_ptr->trajectory;
391+
const auto lane_changing_acc =
392+
common_data_ptr->transient_data.is_ego_near_current_terminal_start
393+
? prepare_longitudinal_acc * params.lane_changing_decel_factor
394+
: 0.0;
395+
return lane_changing_acc;
390396
}
391397

392398
return std::clamp(
@@ -497,7 +503,7 @@ std::vector<PhaseMetrics> calc_shift_phase_metrics(
497503
shift_length, common_data_ptr->lc_param_ptr->trajectory.lateral_jerk, lat_acc);
498504

499505
const double lane_changing_accel = calc_lane_changing_acceleration(
500-
initial_velocity, max_path_velocity, lane_changing_duration, lon_accel);
506+
common_data_ptr, initial_velocity, max_path_velocity, lane_changing_duration, lon_accel);
501507

502508
const auto lane_changing_length = calculation::calc_phase_length(
503509
initial_velocity, max_vel, lane_changing_accel, lane_changing_duration);

0 commit comments

Comments
 (0)