From bf815cc600a0b7b19d426c729f6bf283bafff4cd Mon Sep 17 00:00:00 2001 From: Grant Date: Mon, 13 Jul 2026 09:33:52 +0200 Subject: [PATCH] feat(failsafe): gate-mode safe-zone-reached completion event closes RETURN_TO_SAFE trap MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds the missing exit from RETURN_TO_SAFE for gate-mode (AUV) committed recovery. Confirmed live on RPi5 12 Jul 2026: with the mode system now publishing a real gate profile, failsafe_monitor commits HOLD_AND_RECOVER -> RETURN_TO_SAFE on condition-clear as designed (2a3e577) but then never leaves, because the gate branch deliberately ignores further condition-clear so committed recovery runs to completion (DIR-7 addendum, "In-mission recovery is mode-dependent") — nothing signalled completion. New subscription: /rov/nav/safe_zone_reached (std_msgs/Bool), same plain QoS and latch-on-True callback style as the existing flag_manual_abort subscription. This topic and this subscription are the PERMANENT nav interface — the navigation subsystem will publish the real arrival event here (GPS at surface, EKF dead-reckoning underwater); only today's publisher is temporary bench-test/button scaffolding. failsafe_monitor only ever consumes this event, never computes position itself. health_role == HEALTH_GATE, fsm_state == RETURN_TO_SAFE, flag True -> transition to NORMAL, cause text "Safe zone reached — recovery complete" (DIR-7's cause-text MUST, via the existing message-prefix mechanism — no FailsafeStatus.msg change, no cockpit_bridge/W1 change needed). Any other combination (not in RETURN_TO_SAFE, or advisory mode) is ignored — advisory keeps its existing, untouched operator-resume mechanism. Gate's HOLD_AND_RECOVER->RETURN_TO_SAFE commit behaviour and the advisory branch are both unchanged — this only adds the missing exit. --- .../rov_control/failsafe_monitor.py | 112 +++++++++++++++++- 1 file changed, 108 insertions(+), 4 deletions(-) diff --git a/src/rov_control/rov_control/failsafe_monitor.py b/src/rov_control/rov_control/failsafe_monitor.py index 637309d..4538153 100644 --- a/src/rov_control/rov_control/failsafe_monitor.py +++ b/src/rov_control/rov_control/failsafe_monitor.py @@ -41,6 +41,16 @@ Topics subscribed: mode only, pending the proper rov_api resume endpoint (Parked Design Item). Lets an operator confirm "resume" out of a held HOLD_AND_RECOVER/RETURN_TO_SAFE recovery state. + /rov/nav/safe_zone_reached (std_msgs/Bool) — gate (AUV) mode only. This topic and this + subscription are the PERMANENT interface: the navigation + subsystem will publish the real arrival event here (derived + from GPS at surface, EKF dead-reckoning underwater). Today's + publisher of this topic is temporary bench-test/button + scaffolding, not this node — failsafe_monitor only ever + subscribes to and consumes this event, never computes + position itself. Lets a committed gate-mode RETURN_TO_SAFE + recovery exit back to NORMAL on arrival — see + _apply_failsafe_priority's health_role == HEALTH_GATE branch. Topics published: /rov/failsafe (rov_interfaces/FailsafeStatus) @@ -215,6 +225,20 @@ class FailsafeMonitor(Node): self.flag_manual_abort = False self.flag_thruster_anomaly = False + # Set True on receipt of a True message on /rov/nav/safe_zone_reached + # (see _safe_zone_reached_callback) — same latch style as + # flag_manual_abort above, for consistency, rather than something + # re-derived every cycle like the assessed condition flags. Gate-mode + # only: signals that a committed RETURN_TO_SAFE recovery has arrived + # and can exit to NORMAL (DIR-7 addendum, "In-mission recovery is + # mode-dependent"). UNLIKE flag_manual_abort (which is never reset), + # this flag IS consumed and reset back to False once acted on — see + # the health_role == HEALTH_GATE branch in _apply_failsafe_priority — + # because it authorises exiting THIS recovery episode only; leaving + # it True would silently auto-clear a future, unrelated + # RETURN_TO_SAFE without a fresh arrival event. + self.flag_safe_zone_reached = False + # ------------------------------------------------------------------ # Subscribers # ------------------------------------------------------------------ @@ -261,6 +285,28 @@ class FailsafeMonitor(Node): Bool, '/rov/failsafe/operator_resume', self._operator_resume_callback, 10) + # ------------------------------------------------------------------ + # PERMANENT interface — /rov/nav/safe_zone_reached. + # + # This topic and this subscription are the permanent nav-arrival + # interface for gate-mode (AUV) recovery completion; they are NOT a + # placeholder like the operator_resume subscription above. What IS + # temporary is today's PUBLISHER of this topic — until the + # navigation subsystem is built out, whatever publishes True here is + # bench-test or button scaffolding standing in for the real event + # (GPS-at-surface / EKF dead-reckoning-derived arrival). This node + # must only ever subscribe to and consume this event, never compute + # position itself — that responsibility belongs to navigation, not + # failsafe_monitor. Same plain QoS (depth 10, default reliability) + # as the /rov/mission/abort and /rov/failsafe/operator_resume + # subscriptions above, for consistency — this is an event topic, not + # a latched state topic like /rov/mode/profile, so TRANSIENT_LOCAL + # does not apply here. + # ------------------------------------------------------------------ + self.create_subscription( + Bool, '/rov/nav/safe_zone_reached', + self._safe_zone_reached_callback, 10) + # ------------------------------------------------------------------ # Publishers # ------------------------------------------------------------------ @@ -365,6 +411,24 @@ class FailsafeMonitor(Node): self._operator_resume_pending = True self.get_logger().info('Operator resume received from operator') + def _safe_zone_reached_callback(self, msg: Bool): + """ + Handle the safe-zone-arrival event (gate/AUV mode only). + + PERMANENT interface — see the module docstring and the subscription + comment in __init__ for why this is not a placeholder the way + _operator_resume_callback is: only the publisher of this topic is + temporary scaffolding today, not this subscription or this + callback. Same latch pattern as _abort_callback, for consistency: a + True message sets flag_safe_zone_reached; this callback does not + clear it again. _apply_failsafe_priority consumes and resets the + flag once it has acted on it (see the health_role == HEALTH_GATE + branch, RETURN_TO_SAFE case). + """ + if msg.data: + self.flag_safe_zone_reached = True + self.get_logger().info('Safe zone reached — arrival signal received') + # ------------------------------------------------------------------ # Main evaluation loop — rate adapts to assessment state # ------------------------------------------------------------------ @@ -621,10 +685,50 @@ class FailsafeMonitor(Node): elif self.fsm_state == FSMState.RETURN_TO_SAFE: # Already mid-recovery. Per the addendum, recovery once # committed runs to completion — the condition clearing - # partway through the return must NOT interrupt it. Leave - # fsm_state untouched (no _transition call at all — there is - # no state change to make or log). - self._recovery_cause = 'Recovering to safe zone' + # partway through the return must NOT interrupt it. This is + # existing, unchanged behaviour: condition-clear alone still + # does nothing here (no _transition call for that reason). + # + # The ONLY thing that is allowed to end a committed + # gate-mode recovery is the vehicle actually arriving at the + # safe zone. flag_safe_zone_reached is set by + # _safe_zone_reached_callback from /rov/nav/safe_zone_reached + # (the permanent nav-arrival interface — see module + # docstring and __init__ subscription comment). Without this + # check, RETURN_TO_SAFE was a terminal trap: nothing could + # ever move the FSM back out of it once entered, because the + # elif above deliberately ignores condition-clear by design. + if self.flag_safe_zone_reached: + # Arrived. Recovery is complete — exit to NORMAL. Uses + # _transition() (not a bare assignment) to match the + # sibling HOLD_AND_RECOVER->RETURN_TO_SAFE branch just + # above, which also uses it; this keeps the FSM-level + # transition log line consistent between the two halves + # of this same gate-mode branch. + self._transition(FSMState.NORMAL, + FailsafeStatus.ACTION_NONE, + 'Safe zone reached — recovery complete') + # Set (not appended via the elif in _publish_status) + # directly to the arrival-specific cause text so THIS + # cycle's published message reads "Safe zone reached — + # recovery complete" rather than the generic + # "Recovering to safe zone" that was true up to the + # previous cycle. DIR-7 MUST: the state must carry its + # cause — this is the arrival cause, not the in-progress + # one. + self._recovery_cause = 'Safe zone reached — recovery complete' + self.hold_recover_start = None + # Consume the arrival signal — same reasoning as + # _operator_resume_pending's reset in the advisory + # branch below: it authorises exiting RETURN_TO_SAFE for + # THIS recovery episode only. Leaving it True would + # silently auto-clear a future, unrelated RETURN_TO_SAFE + # without a fresh arrival event ever being published. + self.flag_safe_zone_reached = False + else: + # Not yet arrived — still recovering, unchanged from + # before this change. + self._recovery_cause = 'Recovering to safe zone' elif self.health_role == ModeProfile.HEALTH_ADVISORY: # ADVISORY (ROV): hold and hand the decision to the operator — # do not auto-return to NORMAL just because the condition