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