feat(lane_change): cancel hysteresis

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

Author: zulfaqar-azmi-t4

planning/behavior_path_lane_change_module/config/lane_change.param.yaml

@@ -103,6 +103,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>
@@ -73,6 +74,9 @@ class NormalLaneChange : public LaneChangeBase
 
   PathSafetyStatus isApprovedPathSafe() const override;
 
+  PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis(
+    PathSafetyStatus approved_path_safety_status) override;
+
   bool isRequiredStop(const bool is_object_coming_from_rear) override;
 
   bool isNearEndOfCurrentLanes(

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

@@ -96,6 +96,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;
@@ -240,6 +243,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

@@ -56,6 +56,7 @@ void LaneChangeInterface::processOnExit()
 {
   module_type_->resetParameters();
   debug_marker_.markers.clear();
+  post_process_safety_status_ = {};
   resetPathCandidate();
 }
 
@@ -91,7 +92,11 @@ void LaneChangeInterface::updateData()
 
 void LaneChangeInterface::postProcess()
 {
-  post_process_safety_status_ = module_type_->isApprovedPathSafe();
+  if (getCurrentStatus() == ModuleStatus::RUNNING) {
+    const auto safety_status = module_type_->isApprovedPathSafe();
+    post_process_safety_status_ =
+      module_type_->evaluateApprovedPathWithUnsafeHysteresis(safety_status);
+  }
 }
 
 BehaviorModuleOutput LaneChangeInterface::plan()

planning/behavior_path_lane_change_module/src/manager.cpp

@@ -215,6 +215,8 @@ void LaneChangeModuleManager::initParams(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"));
@@ -283,6 +285,11 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector<rclcpp::Param
   updateParam<double>(
     parameters, ns + "finish_judge_lateral_threshold", p->finish_judge_lateral_threshold);
 
+  {
+    const std::string ns = "lane_change.cancel.";
+    updateParam<int>(
+      parameters, ns + "unsafe_hysteresis_threshold", p->cancel.unsafe_hysteresis_threshold);
+  }
   std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) {
     if (!observer.expired()) observer.lock()->updateModuleParams(p);
   });

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -474,6 +474,7 @@ void NormalLaneChange::resetParameters()
   current_lane_change_state_ = LaneChangeStates::Normal;
   abort_path_ = nullptr;
   status_ = {};
+  unsafe_hysteresis_count_ = 0;
   lane_change_debug_.reset();
 
   RCLCPP_DEBUG(logger_, "reset all flags and debug information.");
@@ -1665,6 +1666,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 {};
+}
+
 TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const
 {
   const auto get_blinker_pose = [](const PathWithLaneId & path, const double length) {