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>

Author: mkquda

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]    |

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:

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.

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{};

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<double>(*node, parameter("trajectory.th_lane_changing_length_diff"));
     p.trajectory.min_lane_changing_velocity =
       getOrDeclareParameter<double>(*node, parameter("trajectory.min_lane_changing_velocity"));
+    p.trajectory.lane_changing_decel_factor =
+      getOrDeclareParameter<double>(*node, parameter("trajectory.lane_changing_decel_factor"));
     p.trajectory.lon_acc_sampling_num =
       getOrDeclareParameter<int>(*node, parameter("trajectory.lon_acc_sampling_num"));
     p.trajectory.lat_acc_sampling_num =
@@ -318,6 +320,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
       parameters, ns + "min_longitudinal_acc", p->trajectory.min_longitudinal_acc);
     updateParam<double>(
       parameters, ns + "max_longitudinal_acc", p->trajectory.max_longitudinal_acc);
+    updateParam<double>(
+      parameters, ns + "lane_changing_decel_factor", p->trajectory.lane_changing_decel_factor);
     int longitudinal_acc_sampling_num = 0;
     updateParam<int>(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num);
     if (longitudinal_acc_sampling_num > 0) {

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

@@ -382,11 +382,17 @@ std::vector<double> 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<PhaseMetrics> 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);