Merge branch 'master' of ssh://symbytech:222/administrator/rov-autonomy
This commit is contained in:
commit
882768af01
@ -9,6 +9,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
|
|||||||
"msg/MissionStatus.msg"
|
"msg/MissionStatus.msg"
|
||||||
"msg/FailsafeStatus.msg"
|
"msg/FailsafeStatus.msg"
|
||||||
"msg/InspectionWaypoint.msg"
|
"msg/InspectionWaypoint.msg"
|
||||||
|
"msg/ModeProfile.msg"
|
||||||
"srv/MissionCommand.srv"
|
"srv/MissionCommand.srv"
|
||||||
DEPENDENCIES std_msgs geometry_msgs
|
DEPENDENCIES std_msgs geometry_msgs
|
||||||
)
|
)
|
||||||
|
|||||||
92
src/rov_interfaces/msg/ModeProfile.msg
Normal file
92
src/rov_interfaces/msg/ModeProfile.msg
Normal file
@ -0,0 +1,92 @@
|
|||||||
|
std_msgs/Header header
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# ModeProfile.msg — resolved operating-mode flag set.
|
||||||
|
#
|
||||||
|
# Published by mode_profile_loader on /rov/mode/profile whenever the active
|
||||||
|
# profile is loaded or changed. Carries the FULLY RESOLVED flags for the
|
||||||
|
# active profile (ROV / AUV / a saved custom-hybrid), so every consumer sees
|
||||||
|
# one atomic, self-consistent set. Widgets read these flags (via the Cockpit
|
||||||
|
# data lake) and adapt their own behaviour — they contain no mode logic.
|
||||||
|
#
|
||||||
|
# Source of truth is mode_profiles.yaml on the RPi5; this message is the
|
||||||
|
# runtime broadcast of the currently-active resolved profile.
|
||||||
|
# =============================================================================
|
||||||
|
|
||||||
|
# --- Active profile identity ------------------------------------------------
|
||||||
|
# Name of the active profile as it appears in mode_profiles.yaml.
|
||||||
|
# Standard values are "ROV" and "AUV"; a saved hybrid carries its custom name.
|
||||||
|
string profile_name
|
||||||
|
|
||||||
|
# Base mode this profile derives from — lets consumers reason about defaults
|
||||||
|
# without parsing the name. A hybrid reports the base it was saved from.
|
||||||
|
uint8 base_mode
|
||||||
|
uint8 BASE_ROV=0
|
||||||
|
uint8 BASE_AUV=1
|
||||||
|
|
||||||
|
# --- HILT: the dominant axis ------------------------------------------------
|
||||||
|
# Whether a live human-in-the-loop link is present for the whole run.
|
||||||
|
# ROV = present (widgets are live interfaces); AUV = absent once submerged
|
||||||
|
# (widgets serve pre-dive setup + post-dive replay only).
|
||||||
|
bool hilt_present
|
||||||
|
|
||||||
|
# --- System health role (W1) ------------------------------------------------
|
||||||
|
# advisory = display only, pilot decides (ROV).
|
||||||
|
# gate = pre-dive GO/NO-GO; RED blocks mission start (AUV).
|
||||||
|
uint8 health_role
|
||||||
|
uint8 HEALTH_ADVISORY=0
|
||||||
|
uint8 HEALTH_GATE=1
|
||||||
|
|
||||||
|
# --- Recording control (independent of HILT) --------------------------------
|
||||||
|
# manual = operator starts/stops recording (ROV reflex).
|
||||||
|
# auto = recording auto-starts on ARM, stops on DISARM (AUV reflex),
|
||||||
|
# bounded per armed run. Single ARM-bounded black-box model.
|
||||||
|
uint8 recording_control
|
||||||
|
uint8 REC_MANUAL=0
|
||||||
|
uint8 REC_AUTO=1
|
||||||
|
|
||||||
|
# No-go gate coupling for recording.
|
||||||
|
# relaxed = recording-off is not a hard no-go (ROV/dev).
|
||||||
|
# enforced = recording failure is a hard no-go (AUV, DIR-9 as amended).
|
||||||
|
uint8 nogo_gate
|
||||||
|
uint8 NOGO_RELAXED=0
|
||||||
|
uint8 NOGO_ENFORCED=1
|
||||||
|
|
||||||
|
# Reminder behaviour on the ARM edge, keyed to recording_control=manual.
|
||||||
|
# true = on transition to ARMED with manual control and recorder not active,
|
||||||
|
# the Recording Control widget prompts ("armed, not recording").
|
||||||
|
# false = no reminder (auto mode acts on ARM instead; nothing to prompt).
|
||||||
|
bool record_reminder_on_arm
|
||||||
|
|
||||||
|
# --- Return To Safe (W3) ----------------------------------------------------
|
||||||
|
# operator_selectable = operator chooses automated return OR switch to manual
|
||||||
|
# and fly out (ROV / HILT).
|
||||||
|
# autonomous = parameter-driven return, no operator in loop (AUV).
|
||||||
|
uint8 return_mode
|
||||||
|
uint8 RETURN_OPERATOR_SELECTABLE=0
|
||||||
|
uint8 RETURN_AUTONOMOUS=1
|
||||||
|
|
||||||
|
# --- Live telemetry (W2 Mission Status, W5 Return Budget, nav) ---------------
|
||||||
|
# on = live operator interfaces throughout (ROV).
|
||||||
|
# predive_only = useful on the surface up to launch, then dark — no link (AUV).
|
||||||
|
uint8 live_telemetry
|
||||||
|
uint8 TELEM_ON=0
|
||||||
|
uint8 TELEM_PREDIVE_ONLY=1
|
||||||
|
|
||||||
|
# --- Nav display (W6 Altitude + depth/heading/attitude) ---------------------
|
||||||
|
# true = shown to the pilot to fly on (ROV).
|
||||||
|
# false = feeds the autonomy stack, not shown as a live readout (AUV).
|
||||||
|
bool nav_display
|
||||||
|
|
||||||
|
# --- Camera role ------------------------------------------------------------
|
||||||
|
# pilot_primary = live view is the pilot's primary flight reference (ROV).
|
||||||
|
# record_only = recorded for dev/ML only (AUV).
|
||||||
|
# NOTE: recorded video is never a client deliverable in either mode until
|
||||||
|
# Class-approved devices are added — a separate constraint from this flag.
|
||||||
|
uint8 camera_role
|
||||||
|
uint8 CAMERA_PILOT_PRIMARY=0
|
||||||
|
uint8 CAMERA_RECORD_ONLY=1
|
||||||
|
|
||||||
|
# --- Human-readable status --------------------------------------------------
|
||||||
|
# e.g. "Loaded profile 'ROV'" — for logs and diagnostic widgets.
|
||||||
|
string message
|
||||||
90
src/rov_mission/config/mode_profiles.yaml
Normal file
90
src/rov_mission/config/mode_profiles.yaml
Normal file
@ -0,0 +1,90 @@
|
|||||||
|
# =============================================================================
|
||||||
|
# mode_profiles.yaml — Argonaut 3 operating-mode profiles
|
||||||
|
# -----------------------------------------------------------------------------
|
||||||
|
# SINGLE SOURCE OF TRUTH for mode-driven behaviour. Lives in rov_mission/config
|
||||||
|
# so it is version-controlled in Gitea and captured by the RPi5 workspace
|
||||||
|
# backup (CREATE THEN BACKUP satisfied with no extra step).
|
||||||
|
#
|
||||||
|
# HOW IT IS USED
|
||||||
|
# 1. mode_profile_loader (rov_mission) reads this file and the `active` key,
|
||||||
|
# resolves the chosen profile's flags into a ModeProfile message, and
|
||||||
|
# publishes it latched on /rov/mode/profile.
|
||||||
|
# 2. cockpit_bridge subscribes to that topic and surfaces the flags to the
|
||||||
|
# Cockpit data lake as external/rov-* variables (same road every other
|
||||||
|
# value already travels). Widgets read the flags; they hold no mode logic.
|
||||||
|
# 3. The setup wizard (from W4) selects a mode, shows the resolved flags,
|
||||||
|
# lets them be edited, writes back here via rov_api, then calls the
|
||||||
|
# loader's /rov/mode/reload service. Editing a flag off its mode default
|
||||||
|
# = a hybrid; "Save As" persists it under a new name for reuse.
|
||||||
|
#
|
||||||
|
# WHY THIS SHAPE
|
||||||
|
# One widget set, behaviour set by flags. Dominant flag is HILT:
|
||||||
|
# ROV = HILT present -> widgets are LIVE operator interfaces.
|
||||||
|
# AUV = HILT absent -> headless from parameters during the dive; widgets
|
||||||
|
# serve pre-dive setup + post-dive replay only.
|
||||||
|
# Recording is the one axis INDEPENDENT of HILT and has its own flags and
|
||||||
|
# its own ARM-bounded trigger (single black-box model).
|
||||||
|
#
|
||||||
|
# HYBRID is not a listed mode. It is any profile whose flags were hand-edited
|
||||||
|
# off a pure ROV/AUV default and saved under a custom name.
|
||||||
|
# =============================================================================
|
||||||
|
|
||||||
|
schema_version: 1
|
||||||
|
|
||||||
|
# Currently selected profile. Set by the setup wizard. The loader publishes
|
||||||
|
# this profile's resolved flags on startup and whenever reloaded.
|
||||||
|
active: ROV
|
||||||
|
|
||||||
|
|
||||||
|
profiles:
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
# ROV — HILT PRESENT (tethered, real-time human-in-the-loop)
|
||||||
|
# Widgets are live operator interfaces; the pilot reads them and acts.
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
ROV:
|
||||||
|
hilt: present # live link present for the whole dive
|
||||||
|
health_role: advisory # W1 informs the pilot; blocks nothing
|
||||||
|
recording_control: manual # operator starts/stops (ROV reflex)
|
||||||
|
nogo_gate: relaxed # recording-off is not a hard no-go here
|
||||||
|
record_reminder_on_arm: on # ARM prompts "armed, not recording"
|
||||||
|
return_mode: operator_selectable # auto-return OR switch to manual & fly out
|
||||||
|
live_telemetry: "on" # W2/W5 + nav are live operator interfaces
|
||||||
|
nav_display: on # W6/depth/heading shown to pilot to fly
|
||||||
|
camera_role: pilot_primary # live view is the primary flight reference
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
# AUV — HILT ABSENT once submerged (autonomous, parameter-driven)
|
||||||
|
# Widgets are pre-dive setup/verification + post-dive replay.
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
AUV:
|
||||||
|
hilt: absent # no live link during the dive
|
||||||
|
health_role: gate # pre-dive GO/NO-GO; RED blocks start
|
||||||
|
recording_control: auto # auto-record on ARM, stop on DISARM
|
||||||
|
nogo_gate: enforced # recording failure is a hard no-go (DIR-9)
|
||||||
|
record_reminder_on_arm: off # automatic; nothing to remind
|
||||||
|
return_mode: autonomous # parameter-driven return, no operator
|
||||||
|
live_telemetry: predive_only # useful on surface only; dark in dive
|
||||||
|
nav_display: off # nav feeds autonomy, not a live readout
|
||||||
|
camera_role: record_only # recorded for dev/ML, not a deliverable
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# CUSTOM / HYBRID PROFILES (written by the wizard's "Save As")
|
||||||
|
# -----------------------------------------------------------------------------
|
||||||
|
# Example only (commented out). A hybrid is a full flag set saved under a new
|
||||||
|
# name — e.g. an ROV dev session that auto-records like an AUV. 'base' records
|
||||||
|
# which standard mode it derived from so the loader can report base_mode.
|
||||||
|
#
|
||||||
|
# ROV_DEV_AUTORECORD:
|
||||||
|
# base: ROV
|
||||||
|
# hilt: present
|
||||||
|
# health_role: advisory
|
||||||
|
# recording_control: auto # borrowed AUV reflex (the override)
|
||||||
|
# nogo_gate: relaxed
|
||||||
|
# record_reminder_on_arm: off
|
||||||
|
# return_mode: operator_selectable
|
||||||
|
# live_telemetry: on
|
||||||
|
# nav_display: on
|
||||||
|
# camera_role: pilot_primary
|
||||||
|
# =============================================================================
|
||||||
@ -10,6 +10,9 @@
|
|||||||
<depend>std_msgs</depend>
|
<depend>std_msgs</depend>
|
||||||
<depend>geometry_msgs</depend>
|
<depend>geometry_msgs</depend>
|
||||||
<depend>rov_interfaces</depend>
|
<depend>rov_interfaces</depend>
|
||||||
|
<depend>std_srvs</depend>
|
||||||
|
<depend>ament_index_python</depend>
|
||||||
|
<exec_depend>python3-yaml</exec_depend>
|
||||||
<export>
|
<export>
|
||||||
<build_type>ament_python</build_type>
|
<build_type>ament_python</build_type>
|
||||||
</export>
|
</export>
|
||||||
|
|||||||
@ -24,10 +24,10 @@ import asyncio
|
|||||||
import threading
|
import threading
|
||||||
import rclpy
|
import rclpy
|
||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
from rclpy.qos import QoSProfile, ReliabilityPolicy
|
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
|
||||||
from std_msgs.msg import Bool, Float64
|
from std_msgs.msg import Bool, Float64
|
||||||
from sensor_msgs.msg import BatteryState, Range
|
from sensor_msgs.msg import BatteryState, Range
|
||||||
from rov_interfaces.msg import FailsafeStatus, MissionStatus
|
from rov_interfaces.msg import FailsafeStatus, MissionStatus, ModeProfile
|
||||||
import websockets
|
import websockets
|
||||||
|
|
||||||
# Port Cockpit connects to as a Generic WebSocket client
|
# Port Cockpit connects to as a Generic WebSocket client
|
||||||
@ -91,6 +91,47 @@ class CockpitBridge(Node):
|
|||||||
# Updated by _altitude_cb when /ping2/altitude messages arrive.
|
# Updated by _altitude_cb when /ping2/altitude messages arrive.
|
||||||
# Remains -1.0 until the Ping2 sonar is connected and publishing.
|
# Remains -1.0 until the Ping2 sonar is connected and publishing.
|
||||||
'rov-altitude': -1.0,
|
'rov-altitude': -1.0,
|
||||||
|
|
||||||
|
# --- Mode profile flags (from mode_profile_loader) ---------
|
||||||
|
# Resolved active operating profile, surfaced so widgets can
|
||||||
|
# adapt behaviour by flag rather than containing mode logic.
|
||||||
|
# Values mirror ModeProfile.msg enum constants (ints) and the
|
||||||
|
# profile name string. Defaults below apply until the first
|
||||||
|
# /rov/mode/profile message arrives (loader latches, so this
|
||||||
|
# is effectively immediate once both nodes are up).
|
||||||
|
|
||||||
|
# Active profile name, e.g. 'ROV', 'AUV', or a saved hybrid.
|
||||||
|
'rov-mode': 'ROV',
|
||||||
|
|
||||||
|
# base_mode: 0=ROV, 1=AUV (ModeProfile.BASE_*)
|
||||||
|
'rov-mode-base': 0,
|
||||||
|
|
||||||
|
# hilt_present: 1=live human-in-the-loop link, 0=absent
|
||||||
|
'rov-hilt': 1,
|
||||||
|
|
||||||
|
# health_role: 0=advisory (pilot decides), 1=gate (blocks start)
|
||||||
|
'rov-health-role': 0,
|
||||||
|
|
||||||
|
# recording_control: 0=manual (operator), 1=auto (ARM-bounded)
|
||||||
|
'rov-rec-control': 0,
|
||||||
|
|
||||||
|
# nogo_gate: 0=relaxed, 1=enforced (recording no-go coupling)
|
||||||
|
'rov-nogo-gate': 0,
|
||||||
|
|
||||||
|
# record_reminder_on_arm: 1=prompt on ARM when manual & idle
|
||||||
|
'rov-rec-reminder': 1,
|
||||||
|
|
||||||
|
# return_mode: 0=operator_selectable, 1=autonomous
|
||||||
|
'rov-return-mode': 0,
|
||||||
|
|
||||||
|
# live_telemetry: 0=on, 1=predive_only
|
||||||
|
'rov-live-telem': 0,
|
||||||
|
|
||||||
|
# nav_display: 1=shown to pilot, 0=feeds autonomy only
|
||||||
|
'rov-nav-display': 1,
|
||||||
|
|
||||||
|
# camera_role: 0=pilot_primary, 1=record_only
|
||||||
|
'rov-camera-role': 0,
|
||||||
}
|
}
|
||||||
|
|
||||||
# Set of currently connected Cockpit WebSocket clients.
|
# Set of currently connected Cockpit WebSocket clients.
|
||||||
@ -149,6 +190,20 @@ class CockpitBridge(Node):
|
|||||||
Range, '/ping2/altitude',
|
Range, '/ping2/altitude',
|
||||||
self._altitude_cb, 10)
|
self._altitude_cb, 10)
|
||||||
|
|
||||||
|
# Resolved operating-mode profile from mode_profile_loader.
|
||||||
|
# Latched (TRANSIENT_LOCAL, depth 1) to match the publisher, so we
|
||||||
|
# receive the current profile immediately even if the loader
|
||||||
|
# started first. Reliable + KEEP_LAST for a single latest value.
|
||||||
|
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_cb, mode_qos)
|
||||||
|
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
# Timers
|
# Timers
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
@ -226,6 +281,28 @@ class CockpitBridge(Node):
|
|||||||
# Round to 2dp for display — Ping2 is ±1% accuracy at range
|
# Round to 2dp for display — Ping2 is ±1% accuracy at range
|
||||||
self._values['rov-altitude'] = round(raw, 2)
|
self._values['rov-altitude'] = round(raw, 2)
|
||||||
|
|
||||||
|
def _mode_cb(self, msg: ModeProfile):
|
||||||
|
"""
|
||||||
|
Update mode-profile flags from mode_profile_loader.
|
||||||
|
|
||||||
|
Copies each resolved flag into _values so it is broadcast to
|
||||||
|
Cockpit as external/rov-*. Enum fields are already ints in the
|
||||||
|
message; the profile name is a string. Widgets read these to
|
||||||
|
adapt behaviour (they hold no mode logic themselves).
|
||||||
|
"""
|
||||||
|
with self._lock:
|
||||||
|
self._values['rov-mode'] = str(msg.profile_name)
|
||||||
|
self._values['rov-mode-base'] = int(msg.base_mode)
|
||||||
|
self._values['rov-hilt'] = 1 if msg.hilt_present else 0
|
||||||
|
self._values['rov-health-role'] = int(msg.health_role)
|
||||||
|
self._values['rov-rec-control'] = int(msg.recording_control)
|
||||||
|
self._values['rov-nogo-gate'] = int(msg.nogo_gate)
|
||||||
|
self._values['rov-rec-reminder'] = 1 if msg.record_reminder_on_arm else 0
|
||||||
|
self._values['rov-return-mode'] = int(msg.return_mode)
|
||||||
|
self._values['rov-live-telem'] = int(msg.live_telemetry)
|
||||||
|
self._values['rov-nav-display'] = 1 if msg.nav_display else 0
|
||||||
|
self._values['rov-camera-role'] = int(msg.camera_role)
|
||||||
|
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
# WebSocket broadcast — called from ROS2 timer at 2 Hz
|
# WebSocket broadcast — called from ROS2 timer at 2 Hz
|
||||||
# ------------------------------------------------------------------
|
# ------------------------------------------------------------------
|
||||||
|
|||||||
303
src/rov_mission/rov_mission/mode_profile_loader.py
Normal file
303
src/rov_mission/rov_mission/mode_profile_loader.py
Normal file
@ -0,0 +1,303 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
mode_profile_loader.py — publishes the active operating-mode profile.
|
||||||
|
|
||||||
|
Reads mode_profiles.yaml (the single source of truth on the RPi5), resolves
|
||||||
|
the active profile's flags into a ModeProfile message, and publishes it on
|
||||||
|
/rov/mode/profile with TRANSIENT_LOCAL durability so that any node or bridge
|
||||||
|
that starts later still receives the current profile (latched behaviour).
|
||||||
|
|
||||||
|
Design:
|
||||||
|
- Widgets contain no mode logic. They read the resolved flags (surfaced to
|
||||||
|
the Cockpit data lake by cockpit_bridge) and adapt their own behaviour.
|
||||||
|
- Flags always change together as a set, so they are published as ONE
|
||||||
|
atomic ModeProfile message rather than a scatter of single-value topics.
|
||||||
|
- The active profile can be changed at runtime by rewriting the YAML (via
|
||||||
|
the setup wizard / rov_api) and asking this node to reload — exposed here
|
||||||
|
as a std_srvs/Trigger service '/rov/mode/reload'.
|
||||||
|
|
||||||
|
This node deliberately holds no mode logic of its own beyond mapping the YAML
|
||||||
|
string values to the ModeProfile enum constants. If the YAML and the message
|
||||||
|
disagree on allowed values, that is surfaced as an error and the previous
|
||||||
|
valid profile is retained (fail-safe: never publish a malformed profile).
|
||||||
|
"""
|
||||||
|
|
||||||
|
import os
|
||||||
|
|
||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, HistoryPolicy
|
||||||
|
from std_srvs.srv import Trigger
|
||||||
|
|
||||||
|
import yaml
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
|
||||||
|
from rov_interfaces.msg import ModeProfile
|
||||||
|
|
||||||
|
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
# Mapping tables: YAML string values -> ModeProfile enum constants.
|
||||||
|
# Keeping these as explicit dicts (rather than clever lookups) makes the
|
||||||
|
# allowed value set obvious and makes an invalid YAML value easy to reject.
|
||||||
|
# ---------------------------------------------------------------------------
|
||||||
|
|
||||||
|
BASE_MODE_MAP = {
|
||||||
|
'ROV': ModeProfile.BASE_ROV,
|
||||||
|
'AUV': ModeProfile.BASE_AUV,
|
||||||
|
}
|
||||||
|
|
||||||
|
HEALTH_ROLE_MAP = {
|
||||||
|
'advisory': ModeProfile.HEALTH_ADVISORY,
|
||||||
|
'gate': ModeProfile.HEALTH_GATE,
|
||||||
|
}
|
||||||
|
|
||||||
|
RECORDING_CONTROL_MAP = {
|
||||||
|
'manual': ModeProfile.REC_MANUAL,
|
||||||
|
'auto': ModeProfile.REC_AUTO,
|
||||||
|
}
|
||||||
|
|
||||||
|
NOGO_GATE_MAP = {
|
||||||
|
'relaxed': ModeProfile.NOGO_RELAXED,
|
||||||
|
'enforced': ModeProfile.NOGO_ENFORCED,
|
||||||
|
}
|
||||||
|
|
||||||
|
RETURN_MODE_MAP = {
|
||||||
|
'operator_selectable': ModeProfile.RETURN_OPERATOR_SELECTABLE,
|
||||||
|
'autonomous': ModeProfile.RETURN_AUTONOMOUS,
|
||||||
|
}
|
||||||
|
|
||||||
|
LIVE_TELEMETRY_MAP = {
|
||||||
|
'on': ModeProfile.TELEM_ON,
|
||||||
|
'predive_only': ModeProfile.TELEM_PREDIVE_ONLY,
|
||||||
|
}
|
||||||
|
|
||||||
|
CAMERA_ROLE_MAP = {
|
||||||
|
'pilot_primary': ModeProfile.CAMERA_PILOT_PRIMARY,
|
||||||
|
'record_only': ModeProfile.CAMERA_RECORD_ONLY,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
class ModeProfileLoader(Node):
|
||||||
|
"""
|
||||||
|
ROS2 node that loads mode_profiles.yaml and publishes the resolved
|
||||||
|
active profile as a latched ModeProfile message.
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
super().__init__('mode_profile_loader')
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Parameter: path to mode_profiles.yaml.
|
||||||
|
# Defaults to the installed config dir of rov_mission, but can be
|
||||||
|
# overridden (e.g. for testing) via:
|
||||||
|
# ros2 run rov_mission mode_profile_loader \
|
||||||
|
# --ros-args -p profiles_path:=/abs/path/mode_profiles.yaml
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
default_path = os.path.join(
|
||||||
|
get_package_share_directory('rov_mission'),
|
||||||
|
'config', 'mode_profiles.yaml')
|
||||||
|
self.declare_parameter('profiles_path', default_path)
|
||||||
|
self._profiles_path = self.get_parameter(
|
||||||
|
'profiles_path').get_parameter_value().string_value
|
||||||
|
|
||||||
|
# Last successfully published profile message. Retained so that if a
|
||||||
|
# later reload fails validation we keep serving the last good profile
|
||||||
|
# rather than publishing something malformed.
|
||||||
|
self._last_good_msg = None
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Publisher: latched (TRANSIENT_LOCAL) so late subscribers — including
|
||||||
|
# cockpit_bridge if it starts after us — receive the current profile
|
||||||
|
# immediately on subscription. depth=1 because only the latest
|
||||||
|
# resolved profile matters.
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
latched_qos = QoSProfile(
|
||||||
|
depth=1,
|
||||||
|
history=HistoryPolicy.KEEP_LAST,
|
||||||
|
reliability=ReliabilityPolicy.RELIABLE,
|
||||||
|
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||||
|
)
|
||||||
|
self._pub = self.create_publisher(
|
||||||
|
ModeProfile, '/rov/mode/profile', latched_qos)
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Reload service: lets the setup wizard / rov_api trigger a re-read
|
||||||
|
# after it rewrites the YAML, without restarting the node.
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
self._reload_srv = self.create_service(
|
||||||
|
Trigger, '/rov/mode/reload', self._on_reload)
|
||||||
|
|
||||||
|
# Initial load at startup.
|
||||||
|
ok, detail = self._load_and_publish()
|
||||||
|
if ok:
|
||||||
|
self.get_logger().info(f'mode_profile_loader started: {detail}')
|
||||||
|
else:
|
||||||
|
# Startup load failed and there is no prior good profile to serve.
|
||||||
|
# Log loudly; the node stays up so a corrected YAML can be loaded
|
||||||
|
# via the reload service without a restart.
|
||||||
|
self.get_logger().error(
|
||||||
|
f'mode_profile_loader started but NO valid profile published: '
|
||||||
|
f'{detail}')
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Reload service handler
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
def _on_reload(self, request, response):
|
||||||
|
"""
|
||||||
|
std_srvs/Trigger handler — re-read the YAML and republish.
|
||||||
|
Returns success=False with a message if the new file is invalid;
|
||||||
|
in that case the previous good profile remains the published one.
|
||||||
|
"""
|
||||||
|
ok, detail = self._load_and_publish()
|
||||||
|
response.success = ok
|
||||||
|
response.message = detail
|
||||||
|
if ok:
|
||||||
|
self.get_logger().info(f'Reloaded profile: {detail}')
|
||||||
|
else:
|
||||||
|
self.get_logger().warn(f'Reload rejected: {detail}')
|
||||||
|
return response
|
||||||
|
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
# Core: load YAML, resolve active profile, validate, publish
|
||||||
|
# ------------------------------------------------------------------
|
||||||
|
|
||||||
|
def _load_and_publish(self):
|
||||||
|
"""
|
||||||
|
Read the YAML, build a ModeProfile from the active profile, and
|
||||||
|
publish it. Returns (ok: bool, detail: str).
|
||||||
|
|
||||||
|
On any error the previously published profile (if any) is left in
|
||||||
|
place — we never publish a partially-built or malformed profile.
|
||||||
|
"""
|
||||||
|
# --- Read + parse the file ------------------------------------------
|
||||||
|
try:
|
||||||
|
with open(self._profiles_path, 'r') as f:
|
||||||
|
data = yaml.safe_load(f)
|
||||||
|
except FileNotFoundError:
|
||||||
|
return False, f'profiles file not found: {self._profiles_path}'
|
||||||
|
except yaml.YAMLError as e:
|
||||||
|
return False, f'YAML parse error: {e}'
|
||||||
|
|
||||||
|
if not isinstance(data, dict):
|
||||||
|
return False, 'profiles file is empty or not a mapping'
|
||||||
|
|
||||||
|
# --- Resolve the active profile name --------------------------------
|
||||||
|
active = data.get('active')
|
||||||
|
if not active:
|
||||||
|
return False, "no 'active' profile specified"
|
||||||
|
|
||||||
|
profiles = data.get('profiles', {})
|
||||||
|
if active not in profiles:
|
||||||
|
return False, f"active profile '{active}' not found in profiles"
|
||||||
|
|
||||||
|
profile = profiles[active]
|
||||||
|
|
||||||
|
# --- Build the message (validates every field via the maps) ---------
|
||||||
|
try:
|
||||||
|
msg = self._build_msg(active, profile)
|
||||||
|
except KeyError as e:
|
||||||
|
# A flag held a value not present in the mapping table.
|
||||||
|
return False, f"invalid flag value in profile '{active}': {e}"
|
||||||
|
except Exception as e:
|
||||||
|
return False, f"error building profile '{active}': {e}"
|
||||||
|
|
||||||
|
# --- Publish + retain as last-good ----------------------------------
|
||||||
|
self._pub.publish(msg)
|
||||||
|
self._last_good_msg = msg
|
||||||
|
return True, f"'{active}' published"
|
||||||
|
|
||||||
|
def _build_msg(self, name, profile):
|
||||||
|
"""
|
||||||
|
Translate one YAML profile dict into a ModeProfile message.
|
||||||
|
|
||||||
|
Every enum lookup uses the mapping tables; a missing/invalid value
|
||||||
|
raises KeyError, which the caller turns into a clean rejection.
|
||||||
|
The base_mode is taken from an explicit 'base' key if present (saved
|
||||||
|
hybrids record the base they derive from); otherwise inferred from
|
||||||
|
the standard names, then from HILT presence as a last resort.
|
||||||
|
"""
|
||||||
|
msg = ModeProfile()
|
||||||
|
msg.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
|
||||||
|
# Active profile identity.
|
||||||
|
msg.profile_name = str(name)
|
||||||
|
|
||||||
|
# Base mode resolution (explicit 'base' -> standard name -> HILT).
|
||||||
|
base_key = str(profile.get('base', name))
|
||||||
|
if base_key in BASE_MODE_MAP:
|
||||||
|
msg.base_mode = BASE_MODE_MAP[base_key]
|
||||||
|
else:
|
||||||
|
msg.base_mode = (
|
||||||
|
ModeProfile.BASE_ROV
|
||||||
|
if profile.get('hilt') == 'present'
|
||||||
|
else ModeProfile.BASE_AUV)
|
||||||
|
|
||||||
|
# HILT axis.
|
||||||
|
msg.hilt_present = (profile['hilt'] == 'present')
|
||||||
|
|
||||||
|
# Enum flags — each raises KeyError on an unknown value.
|
||||||
|
msg.health_role = HEALTH_ROLE_MAP[self._enum_key(profile['health_role'])]
|
||||||
|
msg.recording_control = RECORDING_CONTROL_MAP[self._enum_key(profile['recording_control'])]
|
||||||
|
msg.nogo_gate = NOGO_GATE_MAP[self._enum_key(profile['nogo_gate'])]
|
||||||
|
msg.return_mode = RETURN_MODE_MAP[self._enum_key(profile['return_mode'])]
|
||||||
|
msg.live_telemetry = LIVE_TELEMETRY_MAP[self._enum_key(profile['live_telemetry'])]
|
||||||
|
msg.camera_role = CAMERA_ROLE_MAP[self._enum_key(profile['camera_role'])]
|
||||||
|
|
||||||
|
# Boolean flags — YAML 'on'/'off' or true/false both accepted.
|
||||||
|
msg.record_reminder_on_arm = self._as_bool(
|
||||||
|
profile['record_reminder_on_arm'])
|
||||||
|
msg.nav_display = self._as_bool(profile['nav_display'])
|
||||||
|
|
||||||
|
# Human-readable status for logs / diagnostic widgets.
|
||||||
|
msg.message = f"Loaded profile '{name}'"
|
||||||
|
return msg
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _enum_key(value):
|
||||||
|
"""
|
||||||
|
Normalise a YAML flag value to the lowercase string key used
|
||||||
|
by the enum mapping tables.
|
||||||
|
|
||||||
|
YAML 1.1 turns bare on/off/yes/no/true/false into Python
|
||||||
|
booleans, so an unquoted enum value like `on` arrives as True
|
||||||
|
and would miss a string-keyed map. Map booleans back to
|
||||||
|
'on'/'off'; stringify and lowercase everything else. Unknown
|
||||||
|
values still miss the map and are rejected upstream.
|
||||||
|
"""
|
||||||
|
if value is True:
|
||||||
|
return 'on'
|
||||||
|
if value is False:
|
||||||
|
return 'off'
|
||||||
|
return str(value).strip().lower()
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _as_bool(value):
|
||||||
|
"""
|
||||||
|
Coerce a YAML flag to bool. Accepts real booleans and the strings
|
||||||
|
'on'/'off'/'true'/'false' (case-insensitive). Anything else raises,
|
||||||
|
so a typo cannot silently become False.
|
||||||
|
"""
|
||||||
|
if isinstance(value, bool):
|
||||||
|
return value
|
||||||
|
s = str(value).strip().lower()
|
||||||
|
if s in ('on', 'true', 'yes', '1'):
|
||||||
|
return True
|
||||||
|
if s in ('off', 'false', 'no', '0'):
|
||||||
|
return False
|
||||||
|
raise ValueError(f'not a boolean flag: {value!r}')
|
||||||
|
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
"""Entry point — spin the loader node."""
|
||||||
|
rclpy.init(args=args)
|
||||||
|
node = ModeProfileLoader()
|
||||||
|
try:
|
||||||
|
rclpy.spin(node)
|
||||||
|
finally:
|
||||||
|
node.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
@ -27,6 +27,10 @@ setup(
|
|||||||
# Continuous MCAP recorder — DIR-9 foundational node
|
# Continuous MCAP recorder — DIR-9 foundational node
|
||||||
# Must be active before any mission can start (no-go gate)
|
# Must be active before any mission can start (no-go gate)
|
||||||
'recording_manager = rov_mission.recording_manager:main',
|
'recording_manager = rov_mission.recording_manager:main',
|
||||||
|
|
||||||
|
# Mode profile loader — reads mode_profiles.yaml, publishes the
|
||||||
|
# resolved active profile (ROV/AUV/hybrid) on /rov/mode/profile
|
||||||
|
'mode_profile_loader = rov_mission.mode_profile_loader:main',
|
||||||
],
|
],
|
||||||
},
|
},
|
||||||
)
|
)
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user