feat(nav-arrival): rov_api safe_zone_reached endpoint + cockpit_bridge failsafe_state variable
Both support the temporary gate-mode arrival-event operator button (Cockpit
side of the button is a separate later step):
- rov_api: new POST /nav/safe_zone_reached endpoint, mirroring the existing
POST /abort pattern exactly (same publisher-creation and thread-safe
publish mechanism). Publishes Bool(true) to /rov/nav/safe_zone_reached,
which failsafe_monitor already subscribes to and was verified consuming
on the bench (bf815cc). This endpoint and its future Cockpit button are
temporary operator scaffolding standing in for navigation's real
GPS/EKF-derived arrival event; the topic and failsafe_monitor's
subscription to it are permanent. /abort and all other endpoints
unchanged.
- cockpit_bridge: additive new data lake variable external/rov-failsafe-state,
reading msg.failsafe_state off the same FailsafeStatus message already
handled in _failsafe_cb, alongside the existing external/rov-failsafe
(assessment_state) and external/rov-failsafe-cause. Lets a widget key a
control on the vehicle being in RETURN_TO_SAFE (value 3, confirmed from
rov_interfaces/msg/FailsafeStatus.msg STATE_* constants) specifically.
Existing rov-failsafe / rov-failsafe-cause derivation unchanged.
Widget work itself (W1 button) is a separate later step, not part of this
commit. W1 widget file and its pre-edit .bak backup are untouched.
This commit is contained in:
parent
0eca8fef11
commit
2def2b1f41
@ -26,6 +26,10 @@ ARCHITECTURE
|
|||||||
(std_srvs/Trigger, after
|
(std_srvs/Trigger, after
|
||||||
rewriting mode_profiles.yaml)
|
rewriting mode_profiles.yaml)
|
||||||
|
|
||||||
|
Also: publish /rov/nav/safe_zone_reached (Bool) — TEMPORARY operator
|
||||||
|
scaffolding for the gate-mode arrival event; see POST /nav/safe_zone_reached
|
||||||
|
below and VERIFIED INTERFACES for what is permanent vs disposable here.
|
||||||
|
|
||||||
WHY THIS DESIGN
|
WHY THIS DESIGN
|
||||||
---------------
|
---------------
|
||||||
FastAPI is NOT a ROS2 node. rclpy needs its own executor spinning to
|
FastAPI is NOT a ROS2 node. rclpy needs its own executor spinning to
|
||||||
@ -43,6 +47,8 @@ ENDPOINTS (core only — Stage 2b adds /return_budget, /backup/*, /mission/uploa
|
|||||||
POST /mission/stop -> MissionCommand ABORT
|
POST /mission/stop -> MissionCommand ABORT
|
||||||
GET /mode -> currently active, fully-resolved mode profile
|
GET /mode -> currently active, fully-resolved mode profile
|
||||||
POST /mode -> switch active profile + trigger mode_profile_loader reload
|
POST /mode -> switch active profile + trigger mode_profile_loader reload
|
||||||
|
POST /nav/safe_zone_reached -> TEMPORARY operator trigger for the gate-mode arrival event
|
||||||
|
(publishes Bool true to /rov/nav/safe_zone_reached)
|
||||||
|
|
||||||
VERIFIED INTERFACES (read from source before writing — do not change without re-checking)
|
VERIFIED INTERFACES (read from source before writing — do not change without re-checking)
|
||||||
-----------------------------------------------------------------------------------------
|
-----------------------------------------------------------------------------------------
|
||||||
@ -69,6 +75,16 @@ VERIFIED INTERFACES (read from source before writing — do not change without r
|
|||||||
share directory. rov_api writes ONLY the top-level 'active:'
|
share directory. rov_api writes ONLY the top-level 'active:'
|
||||||
line (see RovApiNode._set_active_line) — never a full YAML
|
line (see RovApiNode._set_active_line) — never a full YAML
|
||||||
round-trip, which would strip the file's comments.
|
round-trip, which would strip the file's comments.
|
||||||
|
/rov/nav/safe_zone_reached : std_msgs/Bool. PERMANENT interface — failsafe_monitor
|
||||||
|
subscribes and, in gate mode while the FSM is in RETURN_TO_SAFE,
|
||||||
|
transitions to NORMAL on Bool(true) (verified on the bench). THIS
|
||||||
|
ENDPOINT and its future Cockpit button are TEMPORARY operator
|
||||||
|
scaffolding standing in for the real navigation-raised arrival
|
||||||
|
event (GPS at surface / EKF dead-reckoning underwater) — delete
|
||||||
|
both outright once navigation publishes to this topic itself. See
|
||||||
|
the mode-aware safe-zone-reached arrival event Parked Design Item,
|
||||||
|
docs/design_intent_register.md, and the DIR-7 "In-mission recovery
|
||||||
|
is mode-dependent" addendum.
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import os
|
import os
|
||||||
@ -224,6 +240,31 @@ class RovApiNode(Node):
|
|||||||
self._abort_pub = self.create_publisher(
|
self._abort_pub = self.create_publisher(
|
||||||
Bool, '/rov/mission/abort', 10)
|
Bool, '/rov/mission/abort', 10)
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Publisher — /rov/nav/safe_zone_reached (gate-mode arrival event)
|
||||||
|
#
|
||||||
|
# Created the exact same way as the /rov/mission/abort publisher
|
||||||
|
# directly above (create_publisher(MsgType, topic, queue_depth)) so
|
||||||
|
# publishing follows the same thread-safe pattern used there — see
|
||||||
|
# publish_safe_zone_reached() below and publish_abort() above.
|
||||||
|
#
|
||||||
|
# TEMPORARY operator scaffolding: this publisher, and the POST
|
||||||
|
# /nav/safe_zone_reached endpoint that calls it, stand in for the
|
||||||
|
# navigation subsystem, which will eventually publish this event
|
||||||
|
# itself (GPS at surface / EKF dead-reckoning underwater). The
|
||||||
|
# TOPIC itself, and failsafe_monitor's subscription to it, are
|
||||||
|
# PERMANENT — only this publisher is disposable. Publishing
|
||||||
|
# Bool(true) here releases a committed gate-mode RETURN_TO_SAFE
|
||||||
|
# recovery back to NORMAL (see failsafe_monitor's
|
||||||
|
# _safe_zone_reached_callback and the health_role == HEALTH_GATE
|
||||||
|
# branch in _apply_failsafe_priority — verified consuming this on
|
||||||
|
# the bench). See the mode-aware safe-zone-reached arrival event
|
||||||
|
# Parked Design Item, docs/design_intent_register.md, and DIR-7's
|
||||||
|
# "In-mission recovery is mode-dependent" addendum.
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
self._safe_zone_reached_pub = self.create_publisher(
|
||||||
|
Bool, '/rov/nav/safe_zone_reached', 10)
|
||||||
|
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
# Service client — /rov/mission/command
|
# Service client — /rov/mission/command
|
||||||
# Used for mission START and STOP. mission_executor enforces the
|
# Used for mission START and STOP. mission_executor enforces the
|
||||||
@ -337,6 +378,48 @@ class RovApiNode(Node):
|
|||||||
self._abort_pub.publish(msg)
|
self._abort_pub.publish(msg)
|
||||||
self.get_logger().warn('RETURN TO SAFE published to /rov/mission/abort (W3)')
|
self.get_logger().warn('RETURN TO SAFE published to /rov/mission/abort (W3)')
|
||||||
|
|
||||||
|
def publish_safe_zone_reached(self):
|
||||||
|
"""
|
||||||
|
Publish Bool(true) to /rov/nav/safe_zone_reached — gate-mode
|
||||||
|
arrival event that releases a committed RETURN_TO_SAFE recovery.
|
||||||
|
|
||||||
|
Same exact pattern as publish_abort() directly above: fire-and-
|
||||||
|
forget, because failsafe_monitor latches on the rising edge (Bool
|
||||||
|
true) rather than needing a continuous signal, so a single publish
|
||||||
|
is sufficient. Called from the FastAPI request thread; publishing
|
||||||
|
on an rclpy publisher created via create_publisher is thread-safe
|
||||||
|
with respect to the spinning executor (see the module docstring's
|
||||||
|
WHY THIS DESIGN section) — this is the same thread-safety
|
||||||
|
guarantee publish_abort() relies on, not a new mechanism.
|
||||||
|
|
||||||
|
TEMPORARY operator scaffolding: this method (and the POST
|
||||||
|
/nav/safe_zone_reached endpoint that calls it) stand in for the
|
||||||
|
navigation subsystem's future real GPS/EKF-derived arrival event.
|
||||||
|
The topic itself, and failsafe_monitor's subscription to it, are
|
||||||
|
PERMANENT — delete this method and its endpoint outright once
|
||||||
|
navigation publishes the real event on this same topic. See the
|
||||||
|
mode-aware safe-zone-reached arrival event Parked Design Item,
|
||||||
|
docs/design_intent_register.md.
|
||||||
|
"""
|
||||||
|
# Build the message the same way publish_abort() does: a fresh
|
||||||
|
# Bool with data=True. There is no reason to reuse a message
|
||||||
|
# instance across calls — this endpoint is called rarely (an
|
||||||
|
# operator button press), so the allocation cost is irrelevant.
|
||||||
|
msg = Bool()
|
||||||
|
msg.data = True
|
||||||
|
# Publish through the same publisher/executor thread-safety
|
||||||
|
# mechanism as publish_abort() — see that method's docstring and
|
||||||
|
# the module docstring's WHY THIS DESIGN section for why this is
|
||||||
|
# safe to call from the FastAPI request thread.
|
||||||
|
self._safe_zone_reached_pub.publish(msg)
|
||||||
|
# Warn (not info) so this shows up prominently in the service log,
|
||||||
|
# matching publish_abort()'s log level — this is an operator
|
||||||
|
# safety-relevant action, not routine chatter.
|
||||||
|
self.get_logger().warn(
|
||||||
|
'Safe zone reached published to /rov/nav/safe_zone_reached '
|
||||||
|
'(gate-mode recovery release — temporary operator scaffolding, '
|
||||||
|
'see mode-aware safe-zone-reached arrival event Parked Design Item)')
|
||||||
|
|
||||||
def call_mission_command(self, command: str, mission_id: str = '',
|
def call_mission_command(self, command: str, mission_id: str = '',
|
||||||
parameters: Optional[list] = None):
|
parameters: Optional[list] = None):
|
||||||
"""
|
"""
|
||||||
@ -631,6 +714,54 @@ def abort():
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# POST /nav/safe_zone_reached — TEMPORARY operator scaffolding
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
@app.post('/nav/safe_zone_reached')
|
||||||
|
def nav_safe_zone_reached():
|
||||||
|
"""
|
||||||
|
Publish the gate-mode safe-zone-arrival event.
|
||||||
|
|
||||||
|
Publishes Bool(true) to /rov/nav/safe_zone_reached, which
|
||||||
|
failsafe_monitor subscribes to and — in gate mode, while the FSM is in
|
||||||
|
RETURN_TO_SAFE — treats as the vehicle having reached its safe zone,
|
||||||
|
releasing a committed recovery back to NORMAL (verified consuming this
|
||||||
|
on the bench). Without this event, a committed gate-mode RETURN_TO_SAFE
|
||||||
|
stays committed indefinitely by design (see the DIR-7 "In-mission
|
||||||
|
recovery is mode-dependent" addendum — recovery, once committed, must
|
||||||
|
run to completion, not be abandoned just because the triggering
|
||||||
|
condition cleared).
|
||||||
|
|
||||||
|
/rov/nav/safe_zone_reached and failsafe_monitor's subscription to it
|
||||||
|
are the PERMANENT navigation interface — that is not going away.
|
||||||
|
**THIS ENDPOINT**, however, and the Cockpit button that will call it,
|
||||||
|
are **TEMPORARY operator scaffolding**: a manual stand-in for the
|
||||||
|
navigation subsystem, which will eventually raise this same event
|
||||||
|
itself — from GPS at the surface and EKF dead-reckoning underwater.
|
||||||
|
Delete this endpoint and its button outright once navigation publishes
|
||||||
|
the real event on this topic; do not keep this around afterwards as a
|
||||||
|
manual override. See the mode-aware safe-zone-reached arrival event
|
||||||
|
Parked Design Item, docs/design_intent_register.md.
|
||||||
|
"""
|
||||||
|
# Same None-guard pattern as every other endpoint in this file — the
|
||||||
|
# node is created before uvicorn starts, so this should never trigger,
|
||||||
|
# but it keeps the failure mode a clean JSON error instead of a 500 if
|
||||||
|
# it somehow does.
|
||||||
|
if _node is None:
|
||||||
|
return {'ok': False, 'error': 'ROS2 node not initialised'}
|
||||||
|
|
||||||
|
# Delegate to the node method that owns the actual publish call — see
|
||||||
|
# RovApiNode.publish_safe_zone_reached() for the thread-safe publish
|
||||||
|
# mechanism (same one publish_abort() uses).
|
||||||
|
_node.publish_safe_zone_reached()
|
||||||
|
return {
|
||||||
|
'ok': True,
|
||||||
|
'action': 'safe_zone_reached',
|
||||||
|
'message': 'Safe zone reached — recovery release sent to vehicle.',
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
# POST /mission/start
|
# POST /mission/start
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
|
|||||||
@ -9,7 +9,18 @@ Cockpit setup: Settings -> General -> Generic WebSocket Connections
|
|||||||
Add: ws://192.168.1.101:9001
|
Add: ws://192.168.1.101:9001
|
||||||
|
|
||||||
Variables in the Cockpit data lake (all prefixed 'external/' by Cockpit):
|
Variables in the Cockpit data lake (all prefixed 'external/' by Cockpit):
|
||||||
rov-failsafe — failsafe state: 0=GREEN, 1=AMBER, 2=RED
|
rov-failsafe — failsafe ASSESSMENT: 0=GREEN, 1=AMBER, 2=RED (hazard level)
|
||||||
|
rov-failsafe-cause — cause text for the W1 status line, e.g. "Heartbeat Lost"
|
||||||
|
rov-failsafe-state — failsafe FSM STATE: 0=NORMAL, 1=ASSESSING,
|
||||||
|
2=HOLD_AND_RECOVER, 3=RETURN_TO_SAFE, 4=EMERGENCY_SURFACE,
|
||||||
|
5=MISSION_COMPLETE, 6=ABORTED (see rov_interfaces/msg/
|
||||||
|
FailsafeStatus.msg STATE_* constants). Distinct from
|
||||||
|
rov-failsafe above: assessment is the GREEN/AMBER/RED
|
||||||
|
hazard level, this is which FSM state the vehicle is
|
||||||
|
currently in as a result of that (or a prior) assessment.
|
||||||
|
Added so a widget can key a control on the vehicle being
|
||||||
|
in RETURN_TO_SAFE (3) specifically — pairs with the
|
||||||
|
temporary gate-mode safe-zone-reached operator button.
|
||||||
rov-depth — depth in metres (float, 3 dp)
|
rov-depth — depth in metres (float, 3 dp)
|
||||||
rov-voltage — battery voltage (float, 2 dp)
|
rov-voltage — battery voltage (float, 2 dp)
|
||||||
rov-heading — compass heading in degrees (float, 1 dp)
|
rov-heading — compass heading in degrees (float, 1 dp)
|
||||||
@ -67,6 +78,12 @@ class CockpitBridge(Node):
|
|||||||
# Cause text for W1 status line, e.g. "Heartbeat Lost"
|
# Cause text for W1 status line, e.g. "Heartbeat Lost"
|
||||||
'rov-failsafe-cause': '',
|
'rov-failsafe-cause': '',
|
||||||
|
|
||||||
|
# Failsafe FSM state integer (STATE_NORMAL=0 .. STATE_ABORTED=6),
|
||||||
|
# NOT the assessment above — see the module docstring and
|
||||||
|
# _failsafe_cb for the assessment-vs-FSM-state distinction.
|
||||||
|
# -1 initial value indicates no data received yet.
|
||||||
|
'rov-failsafe-state': -1,
|
||||||
|
|
||||||
# Depth in metres below surface (positive = deeper)
|
# Depth in metres below surface (positive = deeper)
|
||||||
'rov-depth': 0.0,
|
'rov-depth': 0.0,
|
||||||
|
|
||||||
@ -263,6 +280,35 @@ class CockpitBridge(Node):
|
|||||||
else:
|
else:
|
||||||
self._values['rov-failsafe-cause'] = leading_segment
|
self._values['rov-failsafe-cause'] = leading_segment
|
||||||
|
|
||||||
|
# ------------------------------------------------------------
|
||||||
|
# rov-failsafe-state — the failsafe FSM state, read off the
|
||||||
|
# SAME msg object already being handled in this callback (no
|
||||||
|
# new subscription; purely additive alongside rov-failsafe and
|
||||||
|
# rov-failsafe-cause above).
|
||||||
|
#
|
||||||
|
# This is a DIFFERENT integer from rov-failsafe (assessment_state)
|
||||||
|
# above, and the two must not be confused:
|
||||||
|
# rov-failsafe <- msg.assessment_state -- GREEN/AMBER/RED
|
||||||
|
# hazard level (0/1/2).
|
||||||
|
# rov-failsafe-state <- msg.failsafe_state -- which FSM state
|
||||||
|
# the vehicle is actually in as a
|
||||||
|
# consequence of that assessment: 0=NORMAL,
|
||||||
|
# 1=ASSESSING, 2=HOLD_AND_RECOVER,
|
||||||
|
# 3=RETURN_TO_SAFE, 4=EMERGENCY_SURFACE,
|
||||||
|
# 5=MISSION_COMPLETE, 6=ABORTED (see
|
||||||
|
# rov_interfaces/msg/FailsafeStatus.msg
|
||||||
|
# STATE_* constants, and failsafe_monitor's
|
||||||
|
# FSMState class which mirrors them).
|
||||||
|
#
|
||||||
|
# Exists so a widget can show/enable a control conditionally on
|
||||||
|
# the vehicle currently being in RETURN_TO_SAFE (3) specifically
|
||||||
|
# — e.g. only while a gate-mode committed recovery is under way.
|
||||||
|
# Pairs with the temporary gate-mode safe-zone-reached operator
|
||||||
|
# button (POST /nav/safe_zone_reached on rov_api): the button
|
||||||
|
# should only be relevant/shown while rov-failsafe-state == 3.
|
||||||
|
# ------------------------------------------------------------
|
||||||
|
self._values['rov-failsafe-state'] = int(msg.failsafe_state)
|
||||||
|
|
||||||
def _depth_cb(self, msg: Float64):
|
def _depth_cb(self, msg: Float64):
|
||||||
"""Update depth in metres from Float64 message."""
|
"""Update depth in metres from Float64 message."""
|
||||||
with self._lock:
|
with self._lock:
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user