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

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

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

* docs(lane_change): fix document

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

* fix conflicts and refactor

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* fix conflict

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* style(pre-commit): autofix

---------

Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: 3 people

Diff for: planning/behavior_path_planner/autoware_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
@@ -823,7 +825,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
 

Diff for: planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml

@@ -120,6 +120,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]

Diff for: planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp

@@ -37,6 +37,7 @@ using geometry_msgs::msg::Twist;
 using lane_change::LanesPolygon;
 using tier4_planning_msgs::msg::PathWithLaneId;
 using utils::path_safety_checker::ExtendedPredictedObjects;
+using utils::path_safety_checker::RSSparams;
 
 class NormalLaneChange : public LaneChangeBase
 {
@@ -171,8 +172,18 @@ class NormalLaneChange : public LaneChangeBase
     const LaneChangePath & lane_change_path,
     const lane_change::TargetObjects & collision_check_objects,
     const utils::path_safety_checker::RSSparams & rss_params,
+    const size_t deceleration_sampling_num, CollisionCheckDebugMap & debug_data) const;
+
+  bool has_collision_with_decel_patterns(
+    const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects,
+    const size_t deceleration_sampling_num, const RSSparams & rss_param,
     CollisionCheckDebugMap & debug_data) const;
 
+  bool is_collided(
+    const PathWithLaneId & lane_change_path, const ExtendedPredictedObject & obj,
+    const std::vector<PoseWithVelocityStamped> & ego_predicted_path,
+    const RSSparams & selected_rss_param, 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
   //! obstructing ego path. It makes sense to use values like the maximum lane change distance.
@@ -220,6 +231,7 @@ class NormalLaneChange : public LaneChangeBase
   }
 
   double stop_time_{0.0};
+  static constexpr double floating_err_th{1e-3};
 };
 }  // namespace autoware::behavior_path_planner
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_

Diff for: planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/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

Diff for: planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp

@@ -160,7 +160,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(

Diff for: planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

@@ -238,6 +238,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 =

Diff for: planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -1685,14 +1685,15 @@ bool NormalLaneChange::getLaneChangePaths(
         }
 
         const auto is_safe = std::invoke([&]() {
+          constexpr size_t decel_sampling_num = 1;
           const auto safety_check_with_normal_rss = isLaneChangePathSafe(
             *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params,
-            lane_change_debug_.collision_check_objects);
+            decel_sampling_num, 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);
+              decel_sampling_num, lane_change_debug_.collision_check_objects);
             return safety_check_with_stuck_rss.is_safe;
           }
 
@@ -1881,7 +1882,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();
@@ -2119,7 +2121,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
 {
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
@@ -2131,82 +2133,140 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     return {is_safe, !is_object_behind_ego};
   }
 
+  const auto all_decel_pattern_has_collision =
+    [&](const utils::path_safety_checker::ExtendedPredictedObjects & objects) -> bool {
+    return has_collision_with_decel_patterns(
+      lane_change_path, objects, deceleration_sampling_num, rss_params, debug_data);
+  };
+
+  if (all_decel_pattern_has_collision(collision_check_objects.trailing)) {
+    return {!is_safe, is_object_behind_ego};
+  }
+
+  if (all_decel_pattern_has_collision(collision_check_objects.leading)) {
+    return {!is_safe, !is_object_behind_ego};
+  }
+
+  return {is_safe, !is_object_behind_ego};
+}
+
+bool NormalLaneChange::has_collision_with_decel_patterns(
+  const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects,
+  const size_t deceleration_sampling_num, const RSSparams & rss_param,
+  CollisionCheckDebugMap & debug_data) const
+{
+  if (objects.empty()) {
+    return false;
+  }
+
   const auto & path = lane_change_path.path;
-  const auto & common_parameters = planner_data_->parameters;
-  const auto current_pose = getEgoPose();
-  const auto current_twist = getEgoTwist();
 
   if (path.points.empty()) {
-    return {!is_safe, !is_object_behind_ego};
+    return false;
   }
 
+  const auto current_pose = common_data_ptr_->get_ego_pose();
+  const auto current_twist = common_data_ptr_->get_ego_twist();
+  const auto bpp_param = *common_data_ptr_->bpp_param_ptr;
+  const auto global_min_acc = bpp_param.min_acc;
+  const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing;
+
+  const auto min_acc = std::min(lane_changing_acc, global_min_acc);
+  const auto sampling_num =
+    std::abs(min_acc - lane_changing_acc) > floating_err_th ? deceleration_sampling_num : 1;
+  const auto acc_resolution = (min_acc - lane_changing_acc) / static_cast<double>(sampling_num);
+
+  std::vector<double> acceleration_values(sampling_num);
+  std::iota(acceleration_values.begin(), acceleration_values.end(), 0);
+
+  std::transform(
+    acceleration_values.begin(), acceleration_values.end(), acceleration_values.begin(),
+    [&](double n) { return lane_changing_acc + n * acc_resolution; });
+
   const auto time_resolution = lane_change_parameters_->prediction_time_resolution;
 
-  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);
-
-  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;
-
-  constexpr double collision_check_yaw_diff_threshold{M_PI};
-
-  const auto check_collision = [&](const ExtendedPredictedObject & obj) {
-    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_->stopped_object_velocity_threshold)
-                                      ? lane_change_parameters_->rss_params_for_parked
-                                      : rss_params;
-    for (const auto & obj_path : obj_predicted_paths) {
-      const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons(
-        path, ego_predicted_path, obj, obj_path, common_parameters, selected_rss_param, 1.0,
-        get_max_velocity_for_safety_check(), collision_check_yaw_diff_threshold,
-        current_debug_data.second);
-
-      if (collided_polygons.empty()) {
-        utils::path_safety_checker::updateCollisionCheckDebugMap(
-          debug_data, current_debug_data, is_safe);
-        continue;
-      }
+  const auto all_collided = 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, bpp_param,
+        *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) {
+        const auto selected_rss_param = (obj.initial_twist.twist.linear.x <=
+                                         lane_change_parameters_->stopped_object_velocity_threshold)
+                                          ? lane_change_parameters_->rss_params_for_parked
+                                          : rss_param;
+        return is_collided(
+          lane_change_path.path, obj, ego_predicted_path, selected_rss_param, debug_data);
+      });
+    });
 
-      const auto collision_in_current_lanes =
-        utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, current_lanes_polygon);
-      const auto collision_in_target_lanes =
-        utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_polygon);
+  return all_collided;
+}
 
-      if (!collision_in_current_lanes && !collision_in_target_lanes) {
-        utils::path_safety_checker::updateCollisionCheckDebugMap(
-          debug_data, current_debug_data, is_safe);
-        continue;
-      }
+bool NormalLaneChange::is_collided(
+  const PathWithLaneId & lane_change_path, const ExtendedPredictedObject & obj,
+  const std::vector<PoseWithVelocityStamped> & ego_predicted_path,
+  const RSSparams & selected_rss_param, CollisionCheckDebugMap & debug_data) const
+{
+  constexpr auto is_collided{true};
+
+  if (lane_change_path.points.empty()) {
+    return !is_collided;
+  }
+
+  if (ego_predicted_path.empty()) {
+    return !is_collided;
+  }
+
+  const auto & lanes_polygon_ptr = common_data_ptr_->lanes_polygon_ptr;
+  const auto & current_polygon = lanes_polygon_ptr->current;
+  const auto & expanded_target_polygon = lanes_polygon_ptr->target;
+
+  if (!current_polygon.has_value() || !expanded_target_polygon.has_value()) {
+    return !is_collided;
+  }
+
+  constexpr auto is_safe{true};
+  auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj);
+  constexpr auto collision_check_yaw_diff_threshold{M_PI};
+  constexpr auto hysteresis_factor{1.0};
+  const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
+    obj, lane_change_parameters_->use_all_predicted_path);
+  const auto safety_check_max_vel = get_max_velocity_for_safety_check();
+  const auto & bpp_param = *common_data_ptr_->bpp_param_ptr;
 
+  for (const auto & obj_path : obj_predicted_paths) {
+    const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons(
+      lane_change_path, ego_predicted_path, obj, obj_path, bpp_param, selected_rss_param,
+      hysteresis_factor, safety_check_max_vel, collision_check_yaw_diff_threshold,
+      current_debug_data.second);
+
+    if (collided_polygons.empty()) {
       utils::path_safety_checker::updateCollisionCheckDebugMap(
-        debug_data, current_debug_data, !is_safe);
-      return !is_safe;
+        debug_data, current_debug_data, is_safe);
+      continue;
     }
-    utils::path_safety_checker::updateCollisionCheckDebugMap(
-      debug_data, current_debug_data, is_safe);
-    return is_safe;
-  };
 
-  for (const auto & obj : collision_check_objects.trailing) {
-    if (!check_collision(obj)) {
-      return {!is_safe, is_object_behind_ego};
-    }
-  }
+    const auto collision_in_current_lanes =
+      utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, current_polygon);
+    const auto collision_in_target_lanes =
+      utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_polygon);
 
-  for (const auto & obj : collision_check_objects.leading) {
-    if (!check_collision(obj)) {
-      return {!is_safe, !is_object_behind_ego};
+    if (!collision_in_current_lanes && !collision_in_target_lanes) {
+      utils::path_safety_checker::updateCollisionCheckDebugMap(
+        debug_data, current_debug_data, is_safe);
+      continue;
     }
-  }
 
-  return {is_safe, !is_object_behind_ego};
+    utils::path_safety_checker::updateCollisionCheckDebugMap(
+      debug_data, current_debug_data, !is_safe);
+    return is_collided;
+  }
+  utils::path_safety_checker::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe);
+  return !is_collided;
 }
 
 // Check if the ego vehicle is in stuck by a stationary obstacle or by the terminal of current lanes

Diff for: planning/behavior_path_planner/autoware_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 =
@@ -820,9 +819,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 = autoware::motion_utils::calcInterpolatedPose(
       path.points, vehicle_pose_frenet.length + length);
     predicted_path.emplace_back(t, pose, velocity);