administrator/rov-autonomy
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(failsafe): gate-mode safe-zone-reached completion event closes RETURN_TO_SAFE trap

Browse Source 
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.
This commit is contained in:
Grant 2026-07-13 09:33:52 +02:00
parent 464e17ed43
commit bf815cc600
1 changed files with 108 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
src/rov_control/rov_control/failsafe_monitor.py
View File

@ -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