From 1b0050dd6f560448453733ce30edcd73f50f8d8d Mon Sep 17 00:00:00 2001 From: Grant Date: Sat, 11 Jul 2026 14:14:30 +0200 Subject: [PATCH] fix(failsafe): allow recovery to NORMAL from RETURN_TO_SAFE, not just HOLD_AND_RECOVER --- src/rov_control/rov_control/failsafe_monitor.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/rov_control/rov_control/failsafe_monitor.py b/src/rov_control/rov_control/failsafe_monitor.py index e35ca89..1053b5d 100644 --- a/src/rov_control/rov_control/failsafe_monitor.py +++ b/src/rov_control/rov_control/failsafe_monitor.py @@ -465,8 +465,17 @@ class FailsafeMonitor(Node): 'complete current task, no new panels' ) - # All clear — return to NORMAL if previously in HOLD_AND_RECOVER - if self.fsm_state == FSMState.HOLD_AND_RECOVER: + # All clear — recover to NORMAL from any transient failsafe state. + # This block is only reached when every higher-priority condition + # above has cleared (each returns early while active), so reaching + # here means the vehicle is once again capable. Both HOLD_AND_RECOVER + # and RETURN_TO_SAFE are recoverable: without RETURN_TO_SAFE here it + # becomes a terminal trap (the FSM never leaves it even after the + # triggering condition clears). NOTE: this is the simple mode-blind + # recovery. Mode-dependent recovery (AUV commits to recovery, ROV + # hands to operator at AMBER) is a separate future change once the + # monitor reads the mode profile — see DIR. + if self.fsm_state in (FSMState.HOLD_AND_RECOVER, FSMState.RETURN_TO_SAFE): self.get_logger().info('Conditions cleared — returning to NORMAL') self.fsm_state = FSMState.NORMAL self.hold_recover_start = None