feat(lane_change): cancel hysteresis (autowarefoundation#6288)

* feat(lane_change): cancel hysteresis

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

* Update documentation

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

* fix the explanation of the hysteresis count

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

---------

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

Author: zulfaqar-azmi-t4

planning/behavior_path_lane_change_module/README.md

@@ -284,6 +284,29 @@ detach
 @enduml
 ```
 
+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
+@startuml
+skinparam defaultTextAlignment center
+skinparam backgroundColor #WHITE
+
+title Abort Lane Change
+
+if (Perform collision check?) then (<color:green><b>SAFE</b></color>)
+  :Reset unsafe_hysteresis_count_;
+else (<color:red><b>UNSAFE</b></color>)
+  :Increase unsafe_hysteresis_count_;
+  if (unsafe_hysteresis_count_ > unsafe_hysteresis_threshold?) then (<color:green><b>FALSE</b></color>)
+  else (<color:red><b>TRUE</b></color>)
+    #LightPink:Check abort condition;
+    stop
+  endif
+endif
+:Continue lane changing;
+@enduml
+```
+
 #### Cancel
 
 Suppose the lane change trajectory is evaluated as unsafe. In that case, if the ego vehicle has not departed from the current lane yet, the trajectory will be reset, and the ego vehicle will resume the lane following the maneuver.
@@ -433,6 +456,7 @@ 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            |
 
 ### Debug
 

planning/behavior_path_lane_change_module/config/lane_change.param.yaml

@@ -106,6 +106,7 @@
         duration: 5.0                       # [s]
         max_lateral_jerk: 1000.0            # [m/s3]
         overhang_tolerance: 0.0             # [m]
+        unsafe_hysteresis_threshold: 10     # [/]
 
       finish_judge_lateral_threshold: 0.2        # [m]
 

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -15,6 +15,7 @@
 #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_
 
 #include "behavior_path_lane_change_module/utils/base_class.hpp"
+#include "behavior_path_lane_change_module/utils/data_structs.hpp"
 
 #include <memory>
 #include <utility>
@@ -72,6 +73,8 @@ class NormalLaneChange : public LaneChangeBase
   PathSafetyStatus isApprovedPathSafe() const override;
 
   bool isRequiredStop(const bool is_object_coming_from_rear) const override;
+  PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis(
+    PathSafetyStatus approved_path_safety_status) override;
 
   bool isNearEndOfCurrentLanes(
     const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,

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

@@ -94,6 +94,9 @@ class LaneChangeBase
 
   virtual PathSafetyStatus isApprovedPathSafe() const = 0;
 
+  virtual PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis(
+    PathSafetyStatus approve_path_safety_status) = 0;
+
   virtual bool isNearEndOfCurrentLanes(
     const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
     const double threshold) const = 0;
@@ -248,6 +251,7 @@ class LaneChangeBase
 
   PathWithLaneId prev_approved_path_{};
 
+  int unsafe_hysteresis_count_{0};
   bool is_abort_path_approved_{false};
   bool is_abort_approval_requested_{false};
   bool is_activated_{false};

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

@@ -75,6 +75,11 @@ struct LaneChangeCancelParameters
   double duration{5.0};
   double max_lateral_jerk{10.0};
   double overhang_tolerance{0.0};
+
+  // unsafe_hysteresis_threshold will be compare with the number of detected unsafe instance. If the
+  // number of unsafe exceeds unsafe_hysteresis_threshold, the lane change will be cancelled or
+  // aborted.
+  int unsafe_hysteresis_threshold{2};
 };
 
 struct LaneChangeParameters

planning/behavior_path_lane_change_module/src/interface.cpp

@@ -116,7 +116,8 @@ ModuleStatus LaneChangeInterface::updateState()
     return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS;
   }
 
-  const auto [is_safe, is_object_coming_from_rear] = module_type_->isApprovedPathSafe();
+  const auto [is_safe, is_object_coming_from_rear] =
+    module_type_->evaluateApprovedPathWithUnsafeHysteresis(module_type_->isApprovedPathSafe());
 
   setObjectDebugVisualization();
   if (is_safe) {

planning/behavior_path_lane_change_module/src/manager.cpp

@@ -215,6 +215,8 @@ void LaneChangeModuleManager::init(rclcpp::Node * node)
     getOrDeclareParameter<double>(*node, parameter("cancel.max_lateral_jerk"));
   p.cancel.overhang_tolerance =
     getOrDeclareParameter<double>(*node, parameter("cancel.overhang_tolerance"));
+  p.cancel.unsafe_hysteresis_threshold =
+    getOrDeclareParameter<int>(*node, parameter("cancel.unsafe_hysteresis_threshold"));
 
   p.finish_judge_lateral_threshold =
     getOrDeclareParameter<double>(*node, parameter("finish_judge_lateral_threshold"));

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -420,6 +420,7 @@ void NormalLaneChange::resetParameters()
   is_abort_approval_requested_ = false;
   current_lane_change_state_ = LaneChangeStates::Normal;
   abort_path_ = nullptr;
+  unsafe_hysteresis_count_ = 0;
 
   object_debug_.clear();
 }
@@ -1451,6 +1452,28 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
   return safety_status;
 }
 
+PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis(
+  PathSafetyStatus approved_path_safety_status)
+{
+  if (!approved_path_safety_status.is_safe) {
+    ++unsafe_hysteresis_count_;
+    RCLCPP_DEBUG(
+      logger_, "%s: Increasing hysteresis count to %d.", __func__, unsafe_hysteresis_count_);
+  } else {
+    if (unsafe_hysteresis_count_ > 0) {
+      RCLCPP_DEBUG(logger_, "%s: Lane change is now SAFE. Resetting hysteresis count.", __func__);
+    }
+    unsafe_hysteresis_count_ = 0;
+  }
+  if (unsafe_hysteresis_count_ > lane_change_parameters_->cancel.unsafe_hysteresis_threshold) {
+    RCLCPP_DEBUG(
+      logger_, "%s: hysteresis count exceed threshold. lane change is now %s", __func__,
+      (approved_path_safety_status.is_safe ? "safe" : "UNSAFE"));
+    return approved_path_safety_status;
+  }
+  return {};
+}
+
 bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const
 {
   const auto & route_handler = planner_data_->route_handler;