feat(lane_change): consider deceleration in safety check for cancel

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>

Author: rej55

planning/behavior_path_lane_change_module/README.md

@@ -573,6 +573,8 @@ detach
 @enduml
 ```
 
+During a lane change, a safety check is made in consideration of the deceleration of the ego vehicle, and a safety check is made for `cancel.deceleration_sampling_num` deceleration patterns, and the lane change will be canceled if the abort condition is satisfied for all deceleration patterns.
+
 To preventive measure for lane change path oscillations caused by alternating safe and unsafe conditions, an additional hysteresis count check is implemented before executing an abort or cancel maneuver. If unsafe, the `unsafe_hysteresis_count_` is incremented and compared against `unsafe_hysteresis_threshold`; exceeding it prompts an abort condition check, ensuring decisions are made with consideration to recent safety assessments as shown in flow chart above. This mechanism stabilizes decision-making, preventing abrupt changes due to transient unsafe conditions.
 
 ```plantuml
@@ -822,7 +824,8 @@ The following parameters are configurable in `lane_change.param.yaml`.
 | `cancel.duration`                      | [s]     | double  | The time taken to complete returning to the center line.                                                         | 3.0           |
 | `cancel.max_lateral_jerk`              | [m/sss] | double  | The maximum lateral jerk for abort path                                                                          | 1000.0        |
 | `cancel.overhang_tolerance`            | [m]     | double  | Lane change cancel is prohibited if the vehicle head exceeds the lane boundary more than this tolerance distance | 0.0           |
-| `unsafe_hysteresis_threshold`          | [-]     | int     | threshold that helps prevent frequent switching between safe and unsafe decisions                                | 10            |
+| `cancel.unsafe_hysteresis_threshold`   | [-]     | int     | threshold that helps prevent frequent switching between safe and unsafe decisions                                | 10            |
+| `cancel.deceleration_sampling_num`     | [-]     | int     | Number of deceleration patterns to check safety to cancel lane change                                            | 5             |
 
 ### Debug
 

planning/behavior_path_lane_change_module/config/lane_change.param.yaml

@@ -119,6 +119,7 @@
         max_lateral_jerk: 1000.0            # [m/s3]
         overhang_tolerance: 0.0             # [m]
         unsafe_hysteresis_threshold: 10     # [/]
+        deceleration_sampling_num: 5 # [/]
 
       lane_change_finish_judge_buffer: 2.0      # [m]
       finish_judge_lateral_threshold: 0.1        # [m]

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -168,7 +168,7 @@ class NormalLaneChange : public LaneChangeBase
     const LaneChangePath & lane_change_path,
     const lane_change::TargetObjects & collision_check_objects,
     const utils::path_safety_checker::RSSparams & rss_params,
-    CollisionCheckDebugMap & debug_data) const;
+    const size_t deceleration_sampling_num, CollisionCheckDebugMap & debug_data) const;
 
   //! @brief Check if the ego vehicle is in stuck by a stationary obstacle.
   //! @param obstacle_check_distance Distance to check ahead for any objects that might be

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp

@@ -94,6 +94,8 @@ struct CancelParameters
   // number of unsafe exceeds unsafe_hysteresis_threshold, the lane change will be cancelled or
   // aborted.
   int unsafe_hysteresis_threshold{2};
+
+  int deceleration_sampling_num{5};
 };
 
 struct Parameters

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp

@@ -158,7 +158,7 @@ std::optional<lanelet::ConstLanelet> getLaneChangeTargetLane(
 
 std::vector<PoseWithVelocityStamped> convertToPredictedPath(
   const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose,
-  const BehaviorPathPlannerParameters & common_parameters,
+  const double lane_changing_acceleration, const BehaviorPathPlannerParameters & common_parameters,
   const LaneChangeParameters & lane_change_parameters, const double resolution);
 
 bool isParkedObject(

planning/behavior_path_lane_change_module/src/manager.cpp

@@ -236,6 +236,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
     getOrDeclareParameter<double>(*node, parameter("cancel.overhang_tolerance"));
   p.cancel.unsafe_hysteresis_threshold =
     getOrDeclareParameter<int>(*node, parameter("cancel.unsafe_hysteresis_threshold"));
+  p.cancel.deceleration_sampling_num =
+    getOrDeclareParameter<int>(*node, parameter("cancel.deceleration_sampling_num"));
 
   // finish judge parameters
   p.lane_change_finish_judge_buffer =

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -1679,13 +1679,13 @@ bool NormalLaneChange::getLaneChangePaths(
 
         const auto is_safe = std::invoke([&]() {
           const auto safety_check_with_normal_rss = isLaneChangePathSafe(
-            *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params,
+            *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, 1,
             lane_change_debug_.collision_check_objects);
 
           if (!safety_check_with_normal_rss.is_safe && is_stuck) {
             const auto safety_check_with_stuck_rss = isLaneChangePathSafe(
               *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params_for_stuck,
-              lane_change_debug_.collision_check_objects);
+              1, lane_change_debug_.collision_check_objects);
             return safety_check_with_stuck_rss.is_safe;
           }
 
@@ -1875,7 +1875,8 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
   }
 
   const auto safety_status = isLaneChangePathSafe(
-    path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data);
+    path, target_objects, lane_change_parameters_->rss_params_for_abort,
+    static_cast<size_t>(lane_change_parameters_->cancel.deceleration_sampling_num), debug_data);
   {
     // only for debug purpose
     lane_change_debug_.collision_check_objects.clear();
@@ -2109,7 +2110,7 @@ bool NormalLaneChange::calcAbortPath()
 PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
   const LaneChangePath & lane_change_path,
   const lane_change::TargetObjects & collision_check_objects,
-  const utils::path_safety_checker::RSSparams & rss_params,
+  const utils::path_safety_checker::RSSparams & rss_params, const size_t deceleration_sampling_num,
   CollisionCheckDebugMap & debug_data) const
 {
   constexpr auto is_safe = true;
@@ -2129,22 +2130,31 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     return {!is_safe, !is_object_behind_ego};
   }
 
-  const auto time_resolution = lane_change_parameters_->prediction_time_resolution;
+  const double current_acc = lane_change_path.info.longitudinal_acceleration.lane_changing;
+  const double min_acc = std::min(current_acc, common_parameters.min_acc);
+  const auto sampling_num =
+    std::abs(min_acc - current_acc) > 1.0E-03 ? deceleration_sampling_num : 1;
+  const double acc_resolution = (min_acc - current_acc) / static_cast<double>(sampling_num);
+
+  std::vector<double> acceleration_values(sampling_num);
+  std::iota(acceleration_values.begin(), acceleration_values.end(), 0);
 
-  const auto ego_predicted_path = utils::lane_change::convertToPredictedPath(
-    lane_change_path, current_twist, current_pose, common_parameters, *lane_change_parameters_,
-    time_resolution);
-  const auto debug_predicted_path =
-    utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution);
+  std::transform(
+    acceleration_values.begin(), acceleration_values.end(), acceleration_values.begin(),
+    [&](double n) { return current_acc + n * acc_resolution; });
+
+  const auto time_resolution = lane_change_parameters_->prediction_time_resolution;
 
   const auto & current_lanes_polygon = common_data_ptr_->lanes_polygon_ptr->current;
   const auto & expanded_target_polygon = common_data_ptr_->lanes_polygon_ptr->expanded_target;
 
-  const auto check_collision = [&](const ExtendedPredictedObject & obj) {
+  const auto has_collision = [&](
+                               const ExtendedPredictedObject & obj,
+                               const std::vector<PoseWithVelocityStamped> & ego_predicted_path) {
+    constexpr bool is_collided = true;
     auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj);
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
       obj, lane_change_parameters_->use_all_predicted_path);
-    auto is_safe = true;
     const auto selected_rss_param =
       (obj.initial_twist.twist.linear.x <=
        lane_change_parameters_->prepare_segment_ignore_object_velocity_thresh)
@@ -2174,23 +2184,35 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
 
       utils::path_safety_checker::updateCollisionCheckDebugMap(
         debug_data, current_debug_data, !is_safe);
-      return !is_safe;
+      return is_collided;
     }
     utils::path_safety_checker::updateCollisionCheckDebugMap(
       debug_data, current_debug_data, is_safe);
-    return is_safe;
+    return !is_collided;
   };
 
-  for (const auto & obj : collision_check_objects.trailing) {
-    if (!check_collision(obj)) {
-      return {!is_safe, is_object_behind_ego};
-    }
+  const auto has_collision_with_decel_pattern =
+    [&](const utils::path_safety_checker::ExtendedPredictedObjects & objects) {
+      return std::all_of(
+        acceleration_values.begin(), acceleration_values.end(), [&](const auto acceleration) {
+          const auto ego_predicted_path = utils::lane_change::convertToPredictedPath(
+            lane_change_path, current_twist, current_pose, acceleration, common_parameters,
+            *lane_change_parameters_, time_resolution);
+          const auto debug_predicted_path =
+            utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution);
+
+          return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) {
+            return has_collision(obj, ego_predicted_path);
+          });
+        });
+    };
+
+  if (has_collision_with_decel_pattern(collision_check_objects.trailing)) {
+    return {!is_safe, is_object_behind_ego};
   }
 
-  for (const auto & obj : collision_check_objects.leading) {
-    if (!check_collision(obj)) {
-      return {!is_safe, !is_object_behind_ego};
-    }
+  if (has_collision_with_decel_pattern(collision_check_objects.leading)) {
+    return {!is_safe, !is_object_behind_ego};
   }
 
   return {is_safe, !is_object_behind_ego};

planning/behavior_path_lane_change_module/src/utils/utils.cpp

@@ -778,7 +778,7 @@ std::optional<lanelet::ConstLanelet> getLaneChangeTargetLane(
 
 std::vector<PoseWithVelocityStamped> convertToPredictedPath(
   const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & vehicle_pose,
-  const BehaviorPathPlannerParameters & common_parameters,
+  const double lane_changing_acceleration, const BehaviorPathPlannerParameters & common_parameters,
   const LaneChangeParameters & lane_change_parameters, const double resolution)
 {
   if (lane_change_path.path.points.empty()) {
@@ -787,7 +787,6 @@ std::vector<PoseWithVelocityStamped> convertToPredictedPath(
 
   const auto & path = lane_change_path.path;
   const auto prepare_acc = lane_change_path.info.longitudinal_acceleration.prepare;
-  const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing;
   const auto duration = lane_change_path.info.duration.sum();
   const auto prepare_time = lane_change_path.info.duration.prepare;
   const auto & minimum_lane_changing_velocity =
@@ -819,9 +818,9 @@ std::vector<PoseWithVelocityStamped> convertToPredictedPath(
     initial_velocity * prepare_time + 0.5 * prepare_acc * prepare_time * prepare_time;
   for (double t = prepare_time; t < duration; t += resolution) {
     const double delta_t = t - prepare_time;
-    const double velocity = lane_changing_velocity + lane_changing_acc * delta_t;
-    const double length =
-      lane_changing_velocity * delta_t + 0.5 * lane_changing_acc * delta_t * delta_t + offset;
+    const double velocity = lane_changing_velocity + lane_changing_acceleration * delta_t;
+    const double length = lane_changing_velocity * delta_t +
+                          0.5 * lane_changing_acceleration * delta_t * delta_t + offset;
     const auto pose =
       motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length);
     predicted_path.emplace_back(t, pose, velocity);