diff --git a/src/rov_control/rov_control/failsafe_monitor.py b/src/rov_control/rov_control/failsafe_monitor.py index 1053b5d..637309d 100644 --- a/src/rov_control/rov_control/failsafe_monitor.py +++ b/src/rov_control/rov_control/failsafe_monitor.py @@ -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}'