diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md index 9ff8939862c65..37d44cae32da3 100644 --- a/planning/behavior_path_lane_change_module/README.md +++ b/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 (SAFE) + :Reset unsafe_hysteresis_count_; +else (UNSAFE) + :Increase unsafe_hysteresis_count_; + if (unsafe_hysteresis_count_ > unsafe_hysteresis_threshold?) then (FALSE) + else (TRUE) + #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. @@ -430,6 +453,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 diff --git a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml index 0cb7f3f1a7a92..74c6ad0893b23 100644 --- a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/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] diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index e4af48a71c8f3..454282d4eacd3 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/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 #include @@ -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( diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 35d018ad3d58c..3bb250c5affd8 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/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}; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp index dd7760d31eaa7..e48f687a89035 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/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 diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 0c6656fda3247..472870e2f36fc 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/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() diff --git a/planning/behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp index d5d4bc9627114..9f1c3c13bfadc 100644 --- a/planning/behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_lane_change_module/src/manager.cpp @@ -215,6 +215,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) getOrDeclareParameter(*node, parameter("cancel.max_lateral_jerk")); p.cancel.overhang_tolerance = getOrDeclareParameter(*node, parameter("cancel.overhang_tolerance")); + p.cancel.unsafe_hysteresis_threshold = + getOrDeclareParameter(*node, parameter("cancel.unsafe_hysteresis_threshold")); p.finish_judge_lateral_threshold = getOrDeclareParameter(*node, parameter("finish_judge_lateral_threshold")); @@ -376,6 +378,8 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "duration", p->cancel.duration); updateParam(parameters, ns + "max_lateral_jerk", p->cancel.max_lateral_jerk); updateParam(parameters, ns + "overhang_tolerance", p->cancel.overhang_tolerance); + updateParam( + 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); diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index a5c49c32e0eb6..6c4552de83ac1 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/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) {