feat(lane_change): reduce prepare duration when blinker has been activated

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md

@@ -886,13 +886,15 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
 | `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                                  | 3.0                |
 | `backward_length_buffer_for_blocking_object` | [m]    | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0                |
 | `backward_length_from_intersection`          | [m]    | double | Distance threshold from the last intersection to invalidate or cancel the lane change path                             | 5.0                |
-| `trajectory.prepare_duration`                | [m]    | double | The preparation time for the ego vehicle to be ready to perform lane change.                                           | 4.0                |
+| `trajectory.max_prepare_duration`            | [s]    | double | The maximum preparation time for the ego vehicle to be ready to perform lane change.                                   | 4.0                |
+| `trajectory.min_prepare_duration`            | [s]    | double | The minimum preparation time for the ego vehicle to be ready to perform lane change.                                   | 2.0                |
 | `trajectory.lateral_jerk`                    | [m/s3] | double | Lateral jerk value for lane change path generation                                                                     | 0.5                |
 | `trajectory.minimum_lane_changing_velocity`  | [m/s]  | double | Minimum speed during lane changing process.                                                                            | 2.78               |
 | `trajectory.lon_acc_sampling_num`            | [-]    | int    | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration                   | 3                  |
 | `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

@@ -11,7 +11,8 @@
 
       # trajectory generation
       trajectory:
-        prepare_duration: 4.0
+        max_prepare_duration: 4.0
+        min_prepare_duration: 2.0
         lateral_jerk: 0.5
         min_longitudinal_acc: -1.0
         max_longitudinal_acc: 1.0
@@ -20,6 +21,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/scene.hpp

@@ -54,7 +54,7 @@ class NormalLaneChange : public LaneChangeBase
 
   void update_lanes(const bool is_approved) final;
 
-  void update_transient_data() final;
+  void update_transient_data(const bool is_approved) final;
 
   void update_filtered_objects() final;
 

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

@@ -68,7 +68,7 @@
 
   virtual void update_lanes(const bool is_approved) = 0;
 
-  virtual void update_transient_data() = 0;
+  virtual void update_transient_data(const bool is_approved) = 0;
 
   virtual void update_filtered_objects() = 0;
 
@@ -267,6 +267,15 @@
     return turn_signal;
   }
 
+  void set_signal_activation_time(const bool reset = false) const
+  {
+    if (reset) {
+      signal_activation_time_ = std::nullopt;
+    } else if (!signal_activation_time_) {
+      signal_activation_time_ = clock_.now();
+    }
+  }
+
   LaneChangeStatus status_{};
   PathShifter path_shifter_{};
 
@@ -292,6 +301,7 @@
 
   mutable StopWatch<std::chrono::milliseconds> stop_watch_;
   mutable lane_change::Debug lane_change_debug_;
+  mutable std::optional<rclcpp::Time> signal_activation_time_{std::nullopt};
 
   rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr());
   mutable rclcpp::Clock clock_{RCL_ROS_TIME};

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

@@ -75,7 +75,7 @@ double calc_dist_to_last_fit_width(
  * of the maximum lane change preparation duration and the maximum velocity of the ego vehicle.
  *
  * @param common_data_ptr Shared pointer to a CommonData structure, which should include:
- *  - `lc_param_ptr->lane_change_prepare_duration`: The duration allowed for lane change
+ *  - `lc_param_ptr->maximum_prepare_duration`: The maximum duration allowed for lane change
  * preparation.
  *  - `bpp_param_ptr->max_vel`: The maximum velocity of the ego vehicle.
  *
@@ -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.
@@ -130,6 +131,24 @@ std::vector<double> calc_lon_acceleration_samples(
   const CommonDataPtr & common_data_ptr, const double max_path_velocity,
   const double prepare_duration);
 
+/**
+ * @brief calculates the actual prepare duration that will be used for path generation
+ *
+ * @details computes the required prepare duration to be used for candidate path
+ * generation based on the elapsed time of turn signal activation, the minimum
+ * and maximum parameterized values for the prepare duration,
+ * and the minimum lane changing velocity.
+ *
+ * @param common_data_ptr Shared pointer to a CommonData structure, which includes
+ * lane change parameters and bpp common parameters.
+ * @param current_velocity current ego vehicle velocity.
+ * @param active_signal_duration elapsed time since turn signal activation.
+ * @return The calculated prepare duration value in seconds (s)
+ */
+double calc_actual_prepare_duration(
+  const CommonDataPtr & common_data_ptr, const double current_velocity,
+  const double active_signal_duration);
+
 std::vector<PhaseMetrics> calc_prepare_phase_metrics(
   const CommonDataPtr & common_data_ptr, const double current_velocity,
   const double max_path_velocity, const double min_length_threshold = 0.0,

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

@@ -222,6 +222,8 @@ struct TransientData
   size_t current_path_seg_idx;   // index of nearest segment to ego along current path
   double current_path_velocity;  // velocity of the current path at the ego position along the path
 
+  double lane_change_prepare_duration{0.0};
+
   bool is_ego_near_current_terminal_start{false};
   bool is_ego_stuck{false};
 

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

@@ -115,13 +115,15 @@ struct SafetyParameters
 
 struct TrajectoryParameters
 {
-  double prepare_duration{4.0};
+  double max_prepare_duration{4.0};
+  double min_prepare_duration{1.0};
   double lateral_jerk{0.5};
   double min_longitudinal_acc{-1.0};
   double max_longitudinal_acc{1.0};
   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/interface.cpp

@@ -78,7 +78,7 @@ void LaneChangeInterface::updateData()
   module_type_->setPreviousModuleOutput(getPreviousModuleOutput());
   module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING);
   module_type_->update_filtered_objects();
-  module_type_->update_transient_data();
+  module_type_->update_transient_data(getCurrentStatus() == ModuleStatus::RUNNING);
   module_type_->updateSpecialData();
 
   if (isWaitingApproval() || module_type_->isAbortState()) {
@@ -153,10 +153,10 @@ BehaviorModuleOutput LaneChangeInterface::plan()
 
 BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
 {
-  *prev_approved_path_ = getPreviousModuleOutput().path;
-
   BehaviorModuleOutput out = getPreviousModuleOutput();
 
+  *prev_approved_path_ = out.path;
+
   module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path);
   out.turn_signal_info = module_type_->get_current_turn_signal_info();
 
@@ -166,7 +166,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
     setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color);
   }
 
-  path_reference_ = std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);
+  path_reference_ = std::make_shared<PathWithLaneId>(out.reference_path);
   stop_pose_ = module_type_->getStopPose();
 
   if (!module_type_->isValidPath()) {

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

@@ -44,28 +44,32 @@
 
   // trajectory generation
   {
-    p.trajectory.prepare_duration =
-      getOrDeclareParameter<double>(*node, parameter("trajectory.prepare_duration"));
+    p.trajectory.max_prepare_duration =
+      getOrDeclareParameter<double>(*node, parameter("trajectory.max_prepare_duration"));
+    p.trajectory.min_prepare_duration =
+      getOrDeclareParameter<double>(*node, parameter("trajectory.min_prepare_duration"));
     p.trajectory.lateral_jerk =
       getOrDeclareParameter<double>(*node, parameter("trajectory.lateral_jerk"));
     p.trajectory.min_longitudinal_acc =
       getOrDeclareParameter<double>(*node, parameter("trajectory.min_longitudinal_acc"));
     p.trajectory.max_longitudinal_acc =
       getOrDeclareParameter<double>(*node, parameter("trajectory.max_longitudinal_acc"));
     p.trajectory.th_prepare_length_diff =
       getOrDeclareParameter<double>(*node, parameter("trajectory.th_prepare_length_diff"));
     p.trajectory.th_lane_changing_length_diff =
       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 =
       getOrDeclareParameter<int>(*node, parameter("trajectory.lat_acc_sampling_num"));
 
     const auto max_acc = getOrDeclareParameter<double>(*node, "normal.max_acc");
-    p.trajectory.min_lane_changing_velocity =
-      std::min(p.trajectory.min_lane_changing_velocity, max_acc * p.trajectory.prepare_duration);
+    p.trajectory.min_lane_changing_velocity = std::min(
+      p.trajectory.min_lane_changing_velocity, max_acc * p.trajectory.max_prepare_duration);
 
     // validation of trajectory parameters
     if (p.trajectory.lon_acc_sampling_num < 1 || p.trajectory.lat_acc_sampling_num < 1) {
@@ -308,15 +312,20 @@
 
   {
     const std::string ns = "lane_change.trajectory.";
-    updateParam<double>(parameters, ns + "prepare_duration", p->trajectory.prepare_duration);
+    updateParam<double>(
+      parameters, ns + "max_prepare_duration", p->trajectory.max_prepare_duration);
+    updateParam<double>(
+      parameters, ns + "min_prepare_duration", p->trajectory.min_prepare_duration);
     updateParam<double>(parameters, ns + "lateral_jerk", p->trajectory.lateral_jerk);
     updateParam<double>(
       parameters, ns + ".min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity);
     // longitudinal acceleration
     updateParam<double>(
       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/scene.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -130,7 +130,7 @@
   *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_);
 }
 
-void NormalLaneChange::update_transient_data()
+void NormalLaneChange::update_transient_data(const bool is_approved)
 {
   if (
     !common_data_ptr_ || !common_data_ptr_->is_data_available() ||
@@ -149,6 +149,13 @@
     prev_module_output_.path.points.at(nearest_seg_idx).point.longitudinal_velocity_mps;
   transient_data.current_path_seg_idx = nearest_seg_idx;
 
+  const auto active_signal_duration =
+    signal_activation_time_ ? (clock_.now() - signal_activation_time_.value()).seconds() : 0.0;
+  transient_data.lane_change_prepare_duration =
+    is_approved ? status_.lane_change_path.info.duration.prepare
+                : calculation::calc_actual_prepare_duration(
+                    common_data_ptr_, common_data_ptr_->get_ego_speed(), active_signal_duration);
+
   std::tie(transient_data.lane_changing_length, transient_data.current_dist_buffer) =
     calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, get_current_lanes());
 
@@ -327,6 +334,8 @@
     return get_terminal_turn_signal_info();
   }
 
+  set_signal_activation_time();
+
   return get_turn_signal(getEgoPose(), getLaneChangePath().info.lane_changing_end);
 }
 
@@ -355,9 +364,14 @@
   const auto nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold;
   const auto current_nearest_seg_idx = common_data_ptr_->transient_data.current_path_seg_idx;
 
-  return getTurnSignalDecider().overwrite_turn_signal(
+  const auto turn_signal_info = getTurnSignalDecider().overwrite_turn_signal(
     path, current_pose, current_nearest_seg_idx, original_turn_signal_info,
     terminal_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold);
+
+  set_signal_activation_time(
+    turn_signal_info.turn_signal.command != terminal_turn_signal_info.turn_signal.command);
+
+  return turn_signal_info;
 }
 
 LaneChangePath NormalLaneChange::getLaneChangePath() const
@@ -428,8 +442,6 @@
       output.path = utils::combinePath(output.path, *found_extended_path);
     }
     output.reference_path = getReferencePath();
-    output.turn_signal_info =
-      get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end);
 
     if (isStopState()) {
       const auto current_velocity = getEgoVelocity();
@@ -445,12 +457,17 @@
 
   extendOutputDrivableArea(output);
 
+  const auto turn_signal_info =
+    get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end);
   const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points);
   output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal(
     output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info,
-    output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold,
+    turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold,
     planner_data_->parameters.ego_nearest_yaw_threshold);
 
+  set_signal_activation_time(
+    output.turn_signal_info.turn_signal.command != turn_signal_info.turn_signal.command);
+
   return output;
 }