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

feat(failsafe): mode-dependent recovery per DIR-7 addendum (gate commits, advisory holds)

Browse Source 
Implements the DIR-7 addendum "In-mission recovery is mode-dependent" (11 Jul
2026, docs/design_intent_register.md). failsafe_monitor now subscribes to
/rov/mode/profile (latched) for health_role and branches condition-clear
recovery behaviour on it, replacing the mode-blind unconditional-NORMAL
recovery added in 1b0050d:

- gate (AUV): HOLD_AND_RECOVER commits to RETURN_TO_SAFE on condition-clear
  instead of returning to NORMAL; an in-progress RETURN_TO_SAFE is left
  alone (recovery runs to completion). Mission resumption stays a separate
  deliberate action (DIR-10) — not touched here.
- advisory (ROV): stays in its current recovery state on condition-clear;
  only an explicit operator resume (new placeholder topic
  /rov/failsafe/operator_resume, pending the real rov_api endpoint) returns
  it to NORMAL.

health_role defaults to gate (not advisory) until the first ModeProfile
message arrives, per DIR-7's absence-of-data-is-not-health guidance applied
to mode assumption. Cause text ("Recovering to safe zone" /
"Holding — awaiting operator decision") flows through the existing
message-prefix mechanism, so cockpit_bridge and W1 need no changes.

FailsafeStatus.msg, mission_executor, and mission-resume logic are
out of scope and untouched.
This commit is contained in:
Grant 2026-07-12 13:33:18 +02:00
parent 5551173b7a
commit 2a3e5777af
1 changed files with 207 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -32,6 +32,15 @@ Topics subscribed:
/mavros/state (mavros_msgs/State)
/mavros/mavros/rel_alt (std_msgs/Float64) — altitude/depth proxy
/rov/mission/abort (std_msgs/Bool)
/rov/mode/profile (rov_interfaces/ModeProfile) — latched (TRANSIENT_LOCAL),
published by mode_profile_loader. Read here for health_role
(gate/advisory) so recovery behaviour can be mode-dependent
per the DIR-7 addendum "In-mission recovery is mode-dependent"
(11 Jul 2026) — see _apply_failsafe_priority.
/rov/failsafe/operator_resume (std_msgs/Bool) — PLACEHOLDER interface, advisory (ROV)
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.
Topics published:
/rov/failsafe (rov_interfaces/FailsafeStatus)
@ -40,11 +49,17 @@ Topics published:
import rclpy
from rclpy.node import Node
from rclpy.duration import Duration
from rclpy.qos import qos_profile_sensor_data
from rclpy.qos import (
qos_profile_sensor_data,
QoSProfile,
DurabilityPolicy,
ReliabilityPolicy,
HistoryPolicy,
)
from std_msgs.msg import Bool, Float64
from sensor_msgs.msg import BatteryState
from mavros_msgs.msg import State
from rov_interfaces.msg import FailsafeStatus
from rov_interfaces.msg import FailsafeStatus, ModeProfile
# ---------------------------------------------------------------------------
@ -140,6 +155,50 @@ class FailsafeMonitor(Node):
self.fcu_ever_connected = False
self.current_depth_m = 0.0
# ------------------------------------------------------------------
# Mode profile — health_role (DIR-7: gate vs advisory)
#
# DIR-7 addendum "In-mission recovery is mode-dependent" (11 Jul
# 2026): what the FSM does when a triggering condition clears is not
# one behaviour — AUV (gate) commits to completing the recovery it
# already started; ROV (advisory) holds and hands the decision to
# the operator. This monitor needs to know which mode is active to
# implement that — see _mode_profile_cb and the health_role branch
# in _apply_failsafe_priority.
#
# Default is HEALTH_GATE (the stricter, no-operator-assumed
# behaviour), not HEALTH_ADVISORY, and this is intentional rather
# than an arbitrary pick. DIR-7's own "absence of data is not
# health" guidance (the MUST that a monitor with no input must not
# report GREEN) generalises here: before the first ModeProfile
# message arrives, we do not positively know an operator is present
# (advisory), so we must not assume the more permissive mode is
# correct just because we haven't heard otherwise. Defaulting to
# gate — commit to recovery, never silently return to NORMAL
# without a known operator to hand the decision to — is the
# fail-safe assumption for the same reason absence of FC data
# defaults to "not healthy" rather than "probably fine".
# ------------------------------------------------------------------
self.health_role = ModeProfile.HEALTH_GATE
# PLACEHOLDER: set True on receipt of a True message on
# /rov/failsafe/operator_resume (advisory mode only). Consumed and
# reset to False by _apply_failsafe_priority once acted upon — see
# the health_role == HEALTH_ADVISORY branch. Pending the proper
# rov_api resume endpoint (Parked Design Item); this topic is a
# stand-in so the mode-dependent recovery logic has something
# concrete to gate on today.
self._operator_resume_pending = False
# Cause text override for the "conditions cleared, now in a
# mode-dependent recovery hold/return" state (see the health_role
# branch in _apply_failsafe_priority). Empty string means no
# override is active — _publish_status falls back to the generic
# Assessment/State/Battery text. Reset to '' at the top of every
# _apply_failsafe_priority cycle so it never goes stale once the
# condition it described is no longer true.
self._recovery_cause = ''
# Timing
self.last_heartbeat = self.get_clock().now()
self.comms_loss_start = None # When comms loss began
@ -176,6 +235,32 @@ class FailsafeMonitor(Node):
Bool, '/rov/mission/abort',
self._abort_callback, 10)
# ------------------------------------------------------------------
# Mode profile — /rov/mode/profile, published LATCHED (TRANSIENT_LOCAL,
# depth=1) by mode_profile_loader (rov_mission). Our subscriber QoS
# must match durability (TRANSIENT_LOCAL) or the latched message is
# never delivered — a VOLATILE subscriber only sees messages
# published AFTER it subscribes, and mode_profile_loader typically
# publishes once at its own startup, which may well be before this
# node exists. Same QoS construction already used by rov_api and
# cockpit_bridge for the same topic, for consistency.
# ------------------------------------------------------------------
mode_qos = QoSProfile(
depth=1,
history=HistoryPolicy.KEEP_LAST,
reliability=ReliabilityPolicy.RELIABLE,
durability=DurabilityPolicy.TRANSIENT_LOCAL,
)
self.create_subscription(
ModeProfile, '/rov/mode/profile',
self._mode_profile_cb, mode_qos)
# PLACEHOLDER interface — see module docstring and the
# _operator_resume_pending comment above for scope/rationale.
self.create_subscription(
Bool, '/rov/failsafe/operator_resume',
self._operator_resume_callback, 10)
# ------------------------------------------------------------------
# Publishers
# ------------------------------------------------------------------
@ -252,6 +337,34 @@ class FailsafeMonitor(Node):
self.flag_manual_abort = True
self.get_logger().warn('Manual abort received from operator')
def _mode_profile_cb(self, msg: ModeProfile):
"""
Cache health_role (gate/advisory) from the latched mode profile.
DIR-7 addendum "In-mission recovery is mode-dependent": recovery
behaviour on condition-clear must be derived from health_role, never
assumed — see the health_role branch in _apply_failsafe_priority.
Only health_role is read here; the rest of ModeProfile's fields are
not this node's concern.
"""
self.health_role = msg.health_role
def _operator_resume_callback(self, msg: Bool):
"""
Handle operator resume confirmation (advisory/ROV mode only).
PLACEHOLDER interface pending the proper rov_api resume endpoint
(Parked Design Item, docs/design_intent_register.md) — a stand-in so
the mode-dependent recovery logic has something concrete to gate on
today. Same latch pattern as _abort_callback above, for consistency:
a True message sets the pending flag; 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_ADVISORY branch).
"""
if msg.data:
self._operator_resume_pending = True
self.get_logger().info('Operator resume received from operator')
# ------------------------------------------------------------------
# Main evaluation loop — rate adapts to assessment state
# ------------------------------------------------------------------
@ -407,9 +520,21 @@ class FailsafeMonitor(Node):
RETURN_TO_SAFE). Its GREEN/AMBER hold-and-retry branches are dead
code for that case — retained as a structural fallback rather than
deleted, but not expected to execute.
DIR-7 addendum "In-mission recovery is mode-dependent" (11 Jul
2026): what happens at the very end of this method, once every
condition above has cleared, now branches on health_role instead of
unconditionally returning to NORMAL — see that branch below.
"""
now = self.get_clock().now()
# Reset every cycle so a stale cause from a PREVIOUS cycle's
# mode-dependent recovery branch (below) never lingers into a cycle
# where some higher-priority condition is active instead — only the
# branch itself sets this to a non-empty value, and only when it
# actually applies this cycle.
self._recovery_cause = ''
# Priority 1: Manual emergency surface (hardcoded, no assessment)
# (Handled separately — no current ROS trigger for P1 emergency.
# Manual abort at P6 covers operator intent. Reserved for future.)
@ -465,20 +590,65 @@ class FailsafeMonitor(Node):
'complete current task, no new panels'
)
# 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
# All clear — this block is only reached when every higher-priority
# condition above has cleared (each returns early while active), so
# reaching here means the triggering condition is gone. What happens
# next is NOT the same in both modes (DIR-7 addendum, "In-mission
# recovery is mode-dependent", 11 Jul 2026) — in my own words: a
# gate-mode (AUV) vehicle has no operator to ask, so once it has
# committed to a recovery response it has to see that recovery
# through rather than guessing that "condition cleared" means "carry
# on as if nothing happened" — actually resuming the survey mission
# afterwards is a separate, deliberate mission start (DIR-10), never
# an automatic side effect of this monitor. An advisory-mode (ROV)
# vehicle has a HILT operator who is the one who should decide
# whether to accept the recovery or override and continue (DIR-5 /
# DIR-5b) — this monitor must not auto-decide on the operator's
# behalf, so it holds until told to resume.
if self.health_role == ModeProfile.HEALTH_GATE:
# GATE (AUV): commit to the recovery already under way.
if self.fsm_state == FSMState.HOLD_AND_RECOVER:
# Was only holding station waiting for the condition to
# clear. Now that it has, commit to actually completing the
# return to the designated safe place (DIR-3) — do NOT slip
# back to NORMAL as if the trigger never happened.
self._transition(FSMState.RETURN_TO_SAFE,
FailsafeStatus.ACTION_RETURN_TO_SAFE,
'Conditions cleared — gate mode committing '
'to recovery, returning to safe zone')
self._recovery_cause = 'Recovering to safe zone'
self.hold_recover_start = None
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'
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
# cleared. The only thing allowed to clear this hold is an
# explicit operator resume (see _operator_resume_callback).
if self.fsm_state in (FSMState.HOLD_AND_RECOVER, FSMState.RETURN_TO_SAFE):
if self._operator_resume_pending:
self.get_logger().info(
'Operator resume received — returning to NORMAL')
self.fsm_state = FSMState.NORMAL
self.hold_recover_start = None
# Consume the resume signal — it authorises returning to
# NORMAL from THIS recovery episode only. Leaving it True
# would silently auto-clear a future, unrelated recovery
# without a fresh operator decision, defeating the whole
# point of the advisory-mode handoff above.
self._operator_resume_pending = False
else:
self._recovery_cause = 'Holding — awaiting operator decision'
# health_role is only ever HEALTH_GATE or HEALTH_ADVISORY (the only
# two values ModeProfile.msg defines), so no other branch is
# reachable — if it somehow were, the safe default is to do nothing
# here, which is exactly what falling through both branches above
# already does.
def _handle_comms_loss(self, now):
"""
@ -606,12 +776,33 @@ class FailsafeMonitor(Node):
# connected (fcu_ever_connected False — likely wiring/power/board)
# from one that connected and then dropped (heartbeat aged out past
# comms_timeout_s).
#
# An active comms loss takes priority over the mode-dependent
# recovery cause below — flag_comms_loss True means the CONDITION is
# still active right now, which is more specific and more urgent
# than "why is the FSM still in a recovery state after the condition
# cleared". self._recovery_cause is only ever non-empty when
# flag_comms_loss (and every other condition flag) is already clear
# — see the reset/set logic in _apply_failsafe_priority — so the
# two never actually describe the same moment, but the explicit
# elif keeps that precedence obvious rather than relying on that
# invariant silently.
cause_prefix = ''
if self.flag_comms_loss:
cause_prefix = (
'Vehicle Disconnected | ' if not self.fcu_ever_connected
else 'Heartbeat Lost | '
)
elif self._recovery_cause:
# DIR-7 addendum "In-mission recovery is mode-dependent": surface
# which of the two mode-dependent recovery branches is active
# ("Recovering to safe zone" for gate/AUV, "Holding — awaiting
# operator decision" for advisory/ROV) as cause text, same
# shortcut mechanism (substring of msg.message, no FailsafeStatus
# field change) as the comms-loss cause above. cockpit_bridge and
# the W1 widget already just display whatever string is
# published here, so no changes are needed on that end.
cause_prefix = f'{self._recovery_cause} | '
msg.message = (
f'{cause_prefix}'