From 6394817636de85f6c9866485f88a6390188068a6 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 1 Feb 2024 19:29:11 +0900 Subject: [PATCH] feat(lane_change): cancel hysteresis Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/lane_change.param.yaml | 1 + .../scene.hpp | 4 ++++ .../utils/base_class.hpp | 4 ++++ .../utils/data_structs.hpp | 5 ++++ .../src/interface.cpp | 7 +++++- .../src/manager.cpp | 7 ++++++ .../src/scene.cpp | 23 +++++++++++++++++++ 7 files changed, 50 insertions(+), 1 deletion(-) 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 e3763720c2d8a..23ff5d822e518 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 0a56c17d89fd0..9f1cba622be1c 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")); @@ -283,6 +285,11 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector( parameters, ns + "finish_judge_lateral_threshold", p->finish_judge_lateral_threshold); + { + const std::string ns = "lane_change.cancel."; + 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) {