From 72d525448210942c3ad8a4b8ba7c1f62c621c9ae Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Fri, 19 Jul 2024 12:56:25 +0900 Subject: [PATCH 1/6] feat(static_obstacle_avoidance): enable force execution under unsafe conditions (#8094) * add force execution for static obstacle avoidance Signed-off-by: Go Sakayori * fix Signed-off-by: Go Sakayori * erase unused function in RTC interface Signed-off-by: Go Sakayori * refactor with lamda function Signed-off-by: Go Sakayori * fix rtc_interface Signed-off-by: Go Sakayori * add warn throtthle and move code block Signed-off-by: Go Sakayori * fix Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori Signed-off-by: Go Sakayori --- .../autoware/rtc_interface/rtc_interface.hpp | 1 + .../src/rtc_interface.cpp | 23 +++++++++++ .../scene.hpp | 23 +++++++++-- .../src/scene.cpp | 39 +++++++++++++++++++ 4 files changed, 82 insertions(+), 4 deletions(-) diff --git a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp index 23f7f0b4b36c5..be9b2cfc1ccf6 100644 --- a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp +++ b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp @@ -59,6 +59,7 @@ class RTCInterface void removeExpiredCooperateStatus(); void clearCooperateStatus(); bool isActivated(const UUID & uuid) const; + bool isForceActivated(const UUID & uuid) const; bool isRegistered(const UUID & uuid) const; bool isRTCEnabled(const UUID & uuid) const; bool isTerminated(const UUID & uuid) const; diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 44678077d5dd1..7dcdc4d5984a6 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -345,6 +345,29 @@ bool RTCInterface::isActivated(const UUID & uuid) const return false; } +bool RTCInterface::isForceActivated(const UUID & uuid) const +{ + std::lock_guard lock(mutex_); + const auto itr = std::find_if( + registered_status_.statuses.begin(), registered_status_.statuses.end(), + [uuid](const auto & s) { return s.uuid == uuid; }); + + if (itr != registered_status_.statuses.end()) { + if (itr->state.type != State::WAITING_FOR_EXECUTION && itr->state.type != State::RUNNING) { + return false; + } + if (itr->command_status.type == Command::ACTIVATE && !itr->safe) { + return true; + } else { + return false; + } + } + + RCLCPP_WARN_STREAM( + getLogger(), "[isForceActivated] uuid : " << to_string(uuid) << " is not found" << std::endl); + return false; +} + bool RTCInterface::isRegistered(const UUID & uuid) const { std::lock_guard lock(mutex_); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index 34d06a46d9ac8..87a4c91792649 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -118,8 +118,16 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface const double start_distance = autoware::motion_utils::calcSignedArcLength( path.points, ego_idx, left_shift.start_pose.position); const double finish_distance = start_distance + left_shift.relative_longitudinal; - rtc_interface_ptr_map_.at("left")->updateCooperateStatus( - left_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now()); + + // If force activated keep safety to false + if (rtc_interface_ptr_map_.at("left")->isForceActivated(left_shift.uuid)) { + rtc_interface_ptr_map_.at("left")->updateCooperateStatus( + left_shift.uuid, false, State::RUNNING, start_distance, finish_distance, clock_->now()); + } else { + rtc_interface_ptr_map_.at("left")->updateCooperateStatus( + left_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now()); + } + if (finish_distance > -1.0e-03) { steering_factor_interface_ptr_->updateSteeringFactor( {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance}, @@ -131,8 +139,15 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface const double start_distance = autoware::motion_utils::calcSignedArcLength( path.points, ego_idx, right_shift.start_pose.position); const double finish_distance = start_distance + right_shift.relative_longitudinal; - rtc_interface_ptr_map_.at("right")->updateCooperateStatus( - right_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now()); + + if (rtc_interface_ptr_map_.at("right")->isForceActivated(right_shift.uuid)) { + rtc_interface_ptr_map_.at("right")->updateCooperateStatus( + right_shift.uuid, false, State::RUNNING, start_distance, finish_distance, clock_->now()); + } else { + rtc_interface_ptr_map_.at("right")->updateCooperateStatus( + right_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now()); + } + if (finish_distance > -1.0e-03) { steering_factor_interface_ptr_->updateSteeringFactor( {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance}, diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 39a8d1b853e47..422c7b8ce0f26 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -579,6 +579,45 @@ void StaticObstacleAvoidanceModule::fillEgoStatus( return; } + auto candidate_sl_force_activated = [&](const std::string & direction) { + // If statement to avoid unnecessary warning occurring from isForceActivated function + if (candidate_uuid_ == uuid_map_.at(direction)) { + if (rtc_interface_ptr_map_.at(direction)->isForceActivated(candidate_uuid_)) { + return true; + } + } + return false; + }; + + auto registered_sl_force_activated = + [&](const std::string & direction, const RegisteredShiftLineArray shift_line_array) { + return std::any_of( + shift_line_array.begin(), shift_line_array.end(), [&](const auto & shift_line) { + return rtc_interface_ptr_map_.at(direction)->isForceActivated(shift_line.uuid); + }); + }; + + /** + * Check if the candidate avoidance path is force activated + */ + if (candidate_sl_force_activated("left") || candidate_sl_force_activated("right")) { + data.yield_required = false; + data.safe_shift_line = data.new_shift_line; + return; + } + + /** + * Check if any registered shift line is force activated + */ + if ( + registered_sl_force_activated("left", left_shift_array_) || + registered_sl_force_activated("right", right_shift_array_)) { + data.yield_required = false; + data.safe_shift_line = data.new_shift_line; + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe but force executed"); + return; + } + /** * If the yield maneuver is disabled, use unapproved_new_sl for avoidance path generation even if * the shift line is unsafe. From 5f8afec2313d3657bc9cca67d444765ba5c2c672 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Fri, 19 Jul 2024 18:47:47 +0900 Subject: [PATCH 2/6] fix(rtc_interface): fix build error (#8111) * fix Signed-off-by: Go Sakayori * fix format Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori Signed-off-by: Go Sakayori --- planning/autoware_rtc_interface/src/rtc_interface.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 7dcdc4d5984a6..1352ec10557f9 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -364,7 +364,8 @@ bool RTCInterface::isForceActivated(const UUID & uuid) const } RCLCPP_WARN_STREAM( - getLogger(), "[isForceActivated] uuid : " << to_string(uuid) << " is not found" << std::endl); + getLogger(), + "[isForceActivated] uuid : " << uuid_to_string(uuid) << " is not found" << std::endl); return false; } From 275d6d4262c01bd3781c4c12f3d6045225fa22f0 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Mon, 22 Jul 2024 23:00:31 +0900 Subject: [PATCH 3/6] feat(lane_change): enable force execution under unsafe conditions (#8131) add force execution conditions Signed-off-by: Go Sakayori --- .../src/interface.cpp | 24 ++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index b55b41828081e..34c4cbda75e11 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -123,9 +123,18 @@ BehaviorModuleOutput LaneChangeInterface::plan() } else { const auto path = assignToCandidate(module_type_->getLaneChangePath(), module_type_->getEgoPosition()); - updateRTCStatus( - path.start_distance_to_path_change, path.finish_distance_to_path_change, true, - State::RUNNING); + const auto force_activated = std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), + [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); }); + if (!force_activated) { + updateRTCStatus( + path.start_distance_to_path_change, path.finish_distance_to_path_change, true, + State::RUNNING); + } else { + updateRTCStatus( + path.start_distance_to_path_change, path.finish_distance_to_path_change, false, + State::RUNNING); + } } return output; @@ -226,6 +235,15 @@ bool LaneChangeInterface::canTransitFailureState() updateDebugMarker(); log_debug_throttled(__func__); + const auto force_activated = std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), + [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); }); + + if (force_activated) { + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe but force executed"); + return false; + } + if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) { log_debug_throttled("Abort process has on going."); return false; From e7d45e331f8a35b8ab9582b459bbcb4ce526ea86 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Mon, 29 Jul 2024 09:36:25 +0900 Subject: [PATCH 4/6] feat(rtc_inteface): add function to check force deactivate (#8221) add function to check for deactivation Signed-off-by: Go Sakayori --- .../autoware/rtc_interface/rtc_interface.hpp | 1 + .../src/rtc_interface.cpp | 24 +++++++++++++++++++ 2 files changed, 25 insertions(+) diff --git a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp index be9b2cfc1ccf6..705395c2b1741 100644 --- a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp +++ b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp @@ -60,6 +60,7 @@ class RTCInterface void clearCooperateStatus(); bool isActivated(const UUID & uuid) const; bool isForceActivated(const UUID & uuid) const; + bool isForceDeactivated(const UUID & UUID) const; bool isRegistered(const UUID & uuid) const; bool isRTCEnabled(const UUID & uuid) const; bool isTerminated(const UUID & uuid) const; diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 1352ec10557f9..f35134c9774c6 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -369,6 +369,30 @@ bool RTCInterface::isForceActivated(const UUID & uuid) const return false; } +bool RTCInterface::isForceDeactivated(const UUID & uuid) const +{ + std::lock_guard lock(mutex_); + const auto itr = std::find_if( + registered_status_.statuses.begin(), registered_status_.statuses.end(), + [uuid](const auto & s) { return s.uuid == uuid; }); + + if (itr != registered_status_.statuses.end()) { + if (itr->state.type != State::RUNNING) { + return false; + } + if (itr->command_status.type == Command::DEACTIVATE && itr->safe && !itr->auto_mode) { + return true; + } else { + return false; + } + } + + RCLCPP_WARN_STREAM( + getLogger(), + "[isForceDeactivated] uuid : " << uuid_to_string(uuid) << " is not found" << std::endl); + return false; +} + bool RTCInterface::isRegistered(const UUID & uuid) const { std::lock_guard lock(mutex_); From bbbeb38b6bbc5a54c2691a271b924f9bbad675f0 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Mon, 29 Jul 2024 14:46:33 +0900 Subject: [PATCH 5/6] feat(lane_change): force deactivation in prepare phase (#8235) transfer to cancel state when force deactivated Signed-off-by: Go Sakayori --- .../src/interface.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 34c4cbda75e11..09ac356f7088e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -274,6 +274,19 @@ bool LaneChangeInterface::canTransitFailureState() return true; } + const auto force_deactivated = std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), + [&](const auto & rtc) { return rtc.second->isForceDeactivated(uuid_map_.at(rtc.first)); }); + + if (force_deactivated && module_type_->isAbleToReturnCurrentLane()) { + log_debug_throttled("Cancel lane change due to force deactivation"); + module_type_->toCancelState(); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); + return true; + } + if (post_process_safety_status_.is_safe) { log_debug_throttled("Can't transit to failure state. Ego is on prepare, and it's safe."); return false; From d6b8c9cf0e34efafa76cbc57fdbeaad1f46d9374 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Wed, 31 Jul 2024 19:45:26 +0900 Subject: [PATCH 6/6] feat(static_obstacle_avoidance): force deactivation (#8288) * add force cancel function Signed-off-by: Go Sakayori * fix format Signed-off-by: Go Sakayori * fix json schema Signed-off-by: Go Sakayori * fix spelling Signed-off-by: Go Sakayori * fix Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori Signed-off-by: Go Sakayori --- .../static_obstacle_avoidance.param.yaml | 2 ++ .../data_structs.hpp | 4 +++ .../parameter_helper.hpp | 2 ++ .../scene.hpp | 3 ++ .../static_obstacle_avoidance.schema.json | 10 ++++++ .../src/manager.cpp | 5 +++ .../src/scene.cpp | 35 +++++++++++++++++++ 7 files changed, 61 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml index 0e60e3216361d..f10f871ca3cc0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml @@ -243,6 +243,8 @@ # For cancel maneuver cancel: enable: true # [-] + force: + duration_time: 2.0 # [s] # For yield maneuver yield: diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index 4e14dc4886768..35b547da1f9f6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -107,6 +107,8 @@ struct AvoidanceParameters // if this param is true, it reverts avoidance path when the path is no longer needed. bool enable_cancel_maneuver{false}; + double force_deactivate_duration_time{0.0}; + // enable avoidance for all parking vehicle std::string policy_ambiguous_vehicle{"ignore"}; @@ -581,6 +583,8 @@ struct AvoidancePlanningData bool found_avoidance_path{false}; + bool force_deactivated{false}; + double to_stop_line{std::numeric_limits::max()}; double to_start_point{std::numeric_limits::lowest()}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 56ac3d7f4f1bb..cc7254380cf0b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -301,6 +301,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) { const std::string ns = "avoidance.cancel."; p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable"); + p.force_deactivate_duration_time = + getOrDeclareParameter(*node, ns + "force.duration_time"); } // yield diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index 87a4c91792649..ea12c66859a36 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -477,6 +477,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface mutable std::vector debug_avoidance_initializer_for_shift_line_; mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_; + + bool force_deactivated_{false}; + rclcpp::Time last_deactivation_triggered_time_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index 38a91ac39525b..2a581f90ab255 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -1263,6 +1263,16 @@ "type": "boolean", "description": "Flag to enable cancel maneuver.", "default": "true" + }, + "force": { + "type": "object", + "properties": { + "duration_time": { + "type": "number", + "description": "force deactivate duration time", + "default": 2.0 + } + } } }, "required": ["enable"], diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index 5655696ff086d..015a82aea3013 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp @@ -182,6 +182,11 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( updateParam(parameters, ns + "consider_rear_overhang", p->consider_rear_overhang); } + { + const std::string ns = "avoidance.cancel."; + updateParam(parameters, ns + "force.duration_time", p->force_deactivate_duration_time); + } + { const std::string ns = "avoidance.stop."; updateParam(parameters, ns + "max_distance", p->stop_max_distance); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 422c7b8ce0f26..9e09a6ba0428e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -570,6 +570,24 @@ void StaticObstacleAvoidanceModule::fillEgoStatus( return; } + const auto registered_sl_force_deactivated = + [&](const std::string & direction, const RegisteredShiftLineArray shift_line_array) { + return std::any_of( + shift_line_array.begin(), shift_line_array.end(), [&](const auto & shift_line) { + return rtc_interface_ptr_map_.at(direction)->isForceDeactivated(shift_line.uuid); + }); + }; + + const auto is_force_deactivated = registered_sl_force_deactivated("left", left_shift_array_) || + registered_sl_force_deactivated("right", right_shift_array_); + if (is_force_deactivated && can_yield_maneuver) { + data.yield_required = true; + data.safe_shift_line = data.new_shift_line; + data.force_deactivated = true; + RCLCPP_INFO(getLogger(), "this module is force deactivated. wait until reactivation"); + return; + } + /** * If the avoidance path is safe, use unapproved_new_sl for avoidance path generation. */ @@ -755,6 +773,10 @@ bool StaticObstacleAvoidanceModule::isSafePath( universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & p = planner_data_->parameters; + if (force_deactivated_) { + return false; + } + if (!parameters_->enable_safety_check) { return true; // if safety check is disabled, it always return safe. } @@ -1366,6 +1388,19 @@ void StaticObstacleAvoidanceModule::updateData() } safe_ = avoid_data_.safe; + + if (!force_deactivated_) { + last_deactivation_triggered_time_ = clock_->now(); + force_deactivated_ = avoid_data_.force_deactivated; + return; + } + + if ( + (clock_->now() - last_deactivation_triggered_time_).seconds() > + parameters_->force_deactivate_duration_time) { + RCLCPP_INFO(getLogger(), "The force deactivation is released"); + force_deactivated_ = false; + } } void StaticObstacleAvoidanceModule::processOnEntry()