diff --git a/CLAUDE.md b/CLAUDE.md index efc9a3c..1499987 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -87,6 +87,8 @@ Contains `urdf/mote.urdf.xacro` and `config/robot.yaml`. The xacro loads robot.y ### `mote_bringup` (Python/ament) Launch files, config, udev rules, NetworkManager drop-ins, and systemd services. +**On-robot reliability** (see `mote_bringup/README.md`): the systemd services restart with backoff and never permanently give up (`Restart=always`, `RestartSec`/`RestartSteps`/`RestartMaxDelaySec`, `StartLimitIntervalSec=0`), order after the udev-tagged `dev-mote_*.device` units, and bound the journal. A pre-flight self-check (`self_check.py`, run as `mote-bringup`'s `ExecStartPre`; `pixi run self-check`) gates bringup on servo ping + lidar/camera/disk/clock/config and keeps the robot idle with a clear reason on failure. A health monitor (`health_monitor.py`, `mote-health.service` with a `Type=notify` watchdog; `pixi run health`) publishes per-subsystem `diagnostic_msgs/DiagnosticArray` on `/diagnostics_agg` and a single OK/DEGRADED/FAULT summary on `/health`. Driver and nav2 nodes are `respawn=True` for per-node recovery under the whole-service systemd restart. Battery voltage is **not** software-measurable (the power bank exposes no telemetry); `system_monitor` reports the Pi under-voltage flag as the only power signal. + **Launch hierarchy:** the two mission launches (`mapping_launch.py`, `robot_launch.py`) each take a `base` arg (default true) that includes the hardware base, and a `use_sim_time` arg they forward to everything they include. The sim runs these *same* files with `base:=false`, supplying a Gazebo base in place of the drivers — so the missions are defined once and the sim exercises the real launch files. - `robot_launch.py` — nav mission: `mote_launch.py` (if `base`) + `nav2_launch.py` (drive a saved map). Forwards a `map` arg, defaulting to the active site's map (see Sites). - `mapping_launch.py` — mapping mission: `mote_launch.py` (if `base`) + `slam_launch.py` + `nav2_launch.py` (`localisation:=false`) + `record_launch.py` (`streams:=mapping`, unless `record:=false`): build/extend a map with SLAM *and* drive to goals autonomously while doing so, recording the session for map provenance. diff --git a/mote_bringup/README.md b/mote_bringup/README.md new file mode 100644 index 0000000..63550a6 --- /dev/null +++ b/mote_bringup/README.md @@ -0,0 +1,111 @@ +# mote_bringup + +Launch files, config, udev rules, NetworkManager drop-ins, and systemd services +for the robot. The package layout and launch hierarchy are documented in the +top-level `CLAUDE.md`; this README covers the **on-robot reliability stack** — +how the robot survives unattended operation. + +## systemd services + +Installed by `pixi run setup` (→ `systemd/install.sh`), which fills in the +invoking user/home and enables them. Boot order: + +``` +mote-bringup → mote-slam → mote-nav + │ (self-check runs here ↘ mote-record + │ as ExecStartPre) ↘ mote-health +``` + +`mote-bringup` runs the self-check as its `ExecStartPre` gate — there is no +separate self-check service; gating bringup gates everything downstream. + +| Service | Runs | Notes | +|------------------|---------------------|-------| +| `mote-bringup` | `pixi run launch` | Hardware base. `ExecStartPre` runs the self-check gate first. | +| `mote-slam` | `pixi run slam` | `BindsTo`/`PartOf` bringup — restarts with it. | +| `mote-nav` | `pixi run nav` | `BindsTo`/`PartOf` slam. | +| `mote-record` | `pixi run record` | `Wants`/`PartOf` bringup — a recorder crash never takes down the drive stack. | +| `mote-health` | `pixi run health` | Health monitor; `Type=notify` + `WatchdogSec` watchdog. | + +**Hardening** (all services): + +- `Restart=always` with backoff (`RestartSec=2`, `RestartSteps=5`, + `RestartMaxDelaySec=30`): a crashed service restarts, backing off from 2 s to a + 30 s ceiling instead of hammering. +- `StartLimitIntervalSec=0`: never permanently give up. An unattended robot must + self-heal when reconnected hardware reappears; the backoff prevents a busy + loop, so there is no reason to latch into a failed state. +- **Device ordering**: the udev rules tag the servo/lidar devices + `TAG+="systemd"`, so systemd synthesises `dev-mote_servos.device` / + `dev-mote_lidar.device`. `mote-bringup` orders `After=` / `Wants=` them, so + bringup waits for the hardware to enumerate but a mid-run flap does not + force-kill the stack (the self-check and health monitor own that). +- **journald sizing**: `systemd/journald-mote.conf` bounds the persistent + journal (`SystemMaxUse=500M`, `SystemKeepFree=1G`, `MaxRetentionSec=2week`) so + always-restarting services can never fill the SD card. + +**Two layers of process recovery** (see also `test/chaos/`): + +1. **Node crash** → the launch system relaunches just that node + (`respawn=True` on the drivers in `mote_launch.py` and the nav2 servers in + `nav2_launch.py`; the nav2 lifecycle managers reconnect the respawned node). +2. **Launch/process crash** → systemd restarts the whole service. +3. **Health-monitor hang** → the `WatchdogSec` watchdog restarts `mote-health`. + +## Startup self-check — `self_check.py` + +Runs as `mote-bringup`'s `ExecStartPre` (and by hand: `pixi run self-check`). +Fast, static pre-flight checks — no ROS graph — that gate the launch: + +- **servos**: bus device present *and* an SCServo ping answers (`servo_ping`, the + drive IDs from `robot.yaml`) — CRITICAL. +- **lidar**: device present and openable — CRITICAL. +- **camera**: device present and openable — advisory. +- **disk**: free space on `MOTE_HOME` (< 500 MB CRITICAL, < 2 GB warn). +- **clock**: system time looks NTP-synced (an RTC-less Pi boots at the epoch) — + advisory. +- **config**: `robot.yaml` parses (CRITICAL); an active site is resolved + (advisory — mapping runs without one). + +Any failed **CRITICAL** check → non-zero exit → the `ExecStartPre` fails → +bringup does not start (robot stays in safe idle) and systemd retries with +backoff, so replugging the lidar recovers on its own. The verdict is written to +`$MOTE_HOME/self_check_status.yaml` and printed to journald. + +Runtime data-flow liveness (is `/scan` *publishing*?) is deliberately **not** +checked here — the drivers are not up yet. That is the health monitor's job. + +## Health monitor — `health_monitor.py` + +Runs as `mote-health.service` (or `pixi run health`). Watches subsystem liveness +and publishes, every second: + +- **`/diagnostics_agg`** (`diagnostic_msgs/DiagnosticArray`) — one + `DiagnosticStatus` per subsystem (scan, filtered scan, joint states, camera, + odom TF, localisation TF), the host status folded in from `system_monitor`'s + `/diagnostics`, the last self-check verdict, and a rolled-up `mote` status. The + standard form the fleet layer can lift later. +- **`/health`** (`std_msgs/String`) — a single human-readable summary line: + `OK` / `DEGRADED: camera stale` / `FAULT: scan stale (…)`. Easy to eyeball: + + ```bash + pixi run -- ros2 topic echo /health + ``` + +**Criticality → roll-up**: a stale *critical* subsystem (scan, filtered scan, +joint states, odom TF) is a **FAULT**; a stale non-critical one (camera, +localisation TF) or a fresh-but-slow one is **DEGRADED**. Expectations live in +`config/health.yaml`, overridable per-robot at `~/.mote/health.yaml`. + +The monitor is also the systemd watchdog feeder: it sends `READY=1` once up and +pets the watchdog on every publish (`sd_notify.py`, a dependency-free +`$NOTIFY_SOCKET` client that no-ops outside systemd). + +## Known gap: battery voltage + +The USB-C power bank exposes **no state-of-charge or voltage telemetry**, so the +robot cannot see its own battery in software. The only power signal available is +the Raspberry Pi firmware's `get_throttled` under-voltage bitfield, which +`system_monitor` already reports (a brown-out shows as `DEGRADED`). True battery +sensing needs a hardware change (a fuel-gauge / INA-class sensor on the power +rail) and is tracked as a follow-up — see the reliability follow-up task. diff --git a/mote_bringup/config/health.yaml b/mote_bringup/config/health.yaml new file mode 100644 index 0000000..1a4085e --- /dev/null +++ b/mote_bringup/config/health.yaml @@ -0,0 +1,58 @@ +# Health monitor liveness expectations. +# +# The health_monitor node checks each entry for freshness (a message seen within +# `timeout` seconds) and, when `min_rate` is set, for sustained rate over the +# reporting window. `critical: true` subsystems drive the robot-level FAULT +# state; non-critical ones only drive DEGRADED. A per-robot ~/.mote/health.yaml +# overrides this packaged default (same pattern as perception.yaml). + +# Aggregation + publish period, seconds. +period: 1.0 + +topics: + - name: scan + topic: /scan + type: sensor_msgs/msg/LaserScan + min_rate: 5.0 + timeout: 2.0 + critical: true + - name: scan_filtered + topic: /scan_filtered + type: sensor_msgs/msg/LaserScan + min_rate: 5.0 + timeout: 2.0 + critical: true + - name: joint_states + topic: /joint_states + type: sensor_msgs/msg/JointState + min_rate: 5.0 + timeout: 2.0 + critical: true + # Camera feeds perception, not the safety-critical drive loop, so a camera + # dropout is DEGRADED, not FAULT. The compressed stream is subscribed to + # avoid pulling the full raw image just to measure liveness. + - name: camera + topic: /image_raw/compressed + type: sensor_msgs/msg/CompressedImage + min_rate: 5.0 + timeout: 5.0 + critical: false + +# TF edges to watch. odom->base_footprint is the live odometry corrector +# (kinematic_icp); map->odom appears only once localised (slam or AMCL), so it +# is non-critical — its absence means "not yet localised", not "broken". +tf: + - name: odometry + parent: odom + child: base_footprint + timeout: 2.0 + critical: true + - name: localization + parent: map + child: odom + timeout: 5.0 + critical: false + +# Fold the host status published by system_monitor on /diagnostics (CPU, temp, +# under-voltage) into the robot-level summary so a brown-out shows as DEGRADED. +subscribe_diagnostics: true diff --git a/mote_bringup/launch/mote_launch.py b/mote_bringup/launch/mote_launch.py index 06415c8..f2e04df 100644 --- a/mote_bringup/launch/mote_launch.py +++ b/mote_bringup/launch/mote_launch.py @@ -51,16 +51,25 @@ def generate_launch_description(): ) wheel_params_file.close() + # respawn=True gives per-node recovery: if a driver process crashes, the + # launch system relaunches it within respawn_delay. This is the inner layer; + # systemd restarts the whole service only if `pixi run launch` itself dies. + # The spawners are one-shot and re-run via the OnProcessStart handler when + # controller_manager respawns, so they are not marked respawn. + respawn = {"respawn": True, "respawn_delay": 2.0} + robot_state_publisher = Node( package="robot_state_publisher", executable="robot_state_publisher", parameters=[robot_description], + **respawn, ) controller_manager = Node( package="controller_manager", executable="ros2_control_node", parameters=[robot_description, controller_config, wheel_params_file.name], + **respawn, ) joint_state_broadcaster_spawner = Node( @@ -90,6 +99,7 @@ def generate_launch_description(): "scan_mode": "Standard", } ], + **respawn, ) laser_filter = Node( @@ -100,6 +110,7 @@ def generate_launch_description(): ("scan", "/scan"), ("scan_filtered", "/scan_filtered"), ], + **respawn, ) cam = cfg["camera"] @@ -124,11 +135,13 @@ def generate_launch_description(): executable="v4l2_camera_node", name="camera", parameters=[camera_params], + **respawn, ) system_monitor = Node( package="mote_bringup", executable="system_monitor", + **respawn, ) localization = IncludeLaunchDescription( diff --git a/mote_bringup/launch/nav2_launch.py b/mote_bringup/launch/nav2_launch.py index 64a3039..ed33e97 100644 --- a/mote_bringup/launch/nav2_launch.py +++ b/mote_bringup/launch/nav2_launch.py @@ -91,12 +91,18 @@ def generate_launch_description(): cmd_vel_remap = ("/cmd_vel", "/diff_drive_controller/cmd_vel") + # respawn=True relaunches a crashed nav2 server; the lifecycle managers + # (attempt_respawn_reconnection defaults true) then reconnect their bond and + # re-activate it, so a single server crash recovers without restarting nav. + respawn = {"respawn": True, "respawn_delay": 2.0} + map_server = Node( package="nav2_map_server", executable="map_server", parameters=[nav2_params, {"yaml_filename": LaunchConfiguration("map")}], condition=IfCondition(localisation), output="screen", + **respawn, ) amcl = Node( @@ -105,6 +111,7 @@ def generate_launch_description(): parameters=[nav2_params], condition=IfCondition(localisation), output="screen", + **respawn, ) controller_server = Node( @@ -113,6 +120,7 @@ def generate_launch_description(): parameters=[nav2_params, critic_params_file.name], remappings=[cmd_vel_remap], output="screen", + **respawn, ) smoother_server = Node( @@ -120,6 +128,7 @@ def generate_launch_description(): executable="smoother_server", parameters=[nav2_params], output="screen", + **respawn, ) planner_server = Node( @@ -127,6 +136,7 @@ def generate_launch_description(): executable="planner_server", parameters=[nav2_params], output="screen", + **respawn, ) behavior_server = Node( @@ -135,6 +145,7 @@ def generate_launch_description(): parameters=[nav2_params], remappings=[cmd_vel_remap], output="screen", + **respawn, ) bt_navigator = Node( @@ -142,6 +153,7 @@ def generate_launch_description(): executable="bt_navigator", parameters=[nav2_params], output="screen", + **respawn, ) waypoint_follower = Node( @@ -149,6 +161,7 @@ def generate_launch_description(): executable="waypoint_follower", parameters=[nav2_params], output="screen", + **respawn, ) lifecycle_manager_localization = Node( @@ -160,6 +173,7 @@ def generate_launch_description(): "autostart": True, "node_names": LOCALIZATION_NODES, "bond_timeout": 10.0, + "attempt_respawn_reconnection": True, } ], condition=IfCondition(localisation), @@ -175,6 +189,7 @@ def generate_launch_description(): "autostart": True, "node_names": NAVIGATION_NODES, "bond_timeout": 10.0, + "attempt_respawn_reconnection": True, } ], output="screen", diff --git a/mote_bringup/mote_bringup/health_monitor.py b/mote_bringup/mote_bringup/health_monitor.py new file mode 100644 index 0000000..e75b914 --- /dev/null +++ b/mote_bringup/mote_bringup/health_monitor.py @@ -0,0 +1,280 @@ +"""Robot-level health monitor. + +Watches the liveness of the safety-critical subsystems (lidar scan, filtered +scan, wheel/joint feedback, odometry TF) plus non-critical ones (camera, +localisation TF) and folds in the host status published by ``system_monitor``. +Every ``period`` seconds it publishes: + +* ``/diagnostics_agg`` (``diagnostic_msgs/DiagnosticArray``) — one + ``DiagnosticStatus`` per subsystem plus a rolled-up ``mote`` status, in the + standard form the fleet layer can lift later. +* ``/health`` (``std_msgs/String``) — a single human-readable summary line, + ``OK`` / ``DEGRADED: ...`` / ``FAULT: ...``, easy to ``ros2 topic echo``. + +Criticality decides the roll-up: a stale *critical* subsystem is a FAULT +(ERROR), a stale non-critical one is DEGRADED (WARN), and a fresh-but-slow +subsystem is DEGRADED. Expectations live in ``config/health.yaml`` (overridable +at ``~/.mote/health.yaml``). + +Runs as its own ``mote-health.service`` (``Type=notify`` + ``WatchdogSec``): it +sends ``READY=1`` once spun up and pets the systemd watchdog on every publish, +so a hung monitor is itself restarted. Outside systemd the watchdog calls are +silent no-ops, so ``pixi run health`` behaves identically. +""" + +import os +import time + +import rclpy +import yaml +from ament_index_python.packages import get_package_share_directory +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue +from rclpy.node import Node +from rosidl_runtime_py.utilities import get_message +from std_msgs.msg import String + +import tf2_ros + +from mote_bringup.sd_notify import SdNotifier + +LEVEL_NAME = { + DiagnosticStatus.OK: "OK", + DiagnosticStatus.WARN: "DEGRADED", + DiagnosticStatus.ERROR: "FAULT", + DiagnosticStatus.STALE: "STALE", +} + + +def _load_config(): + default = os.path.join( + get_package_share_directory("mote_bringup"), "config", "health.yaml" + ) + override = os.path.expanduser("~/.mote/health.yaml") + path = override if os.path.exists(override) else default + with open(path) as f: + return yaml.safe_load(f) + + +class _TopicWatch: + """Freshness + rate tracker for one subscribed topic.""" + + def __init__(self, spec): + self.name = spec["name"] + self.topic = spec["topic"] + self.min_rate = spec.get("min_rate") + self.timeout = spec.get("timeout", 2.0) + self.critical = spec.get("critical", False) + self.last_stamp = None + self.count = 0 + + def on_msg(self, _msg): + self.last_stamp = time.monotonic() + self.count += 1 + + def evaluate(self, window): + rate = self.count / window if window > 0 else 0.0 + self.count = 0 + age = None if self.last_stamp is None else time.monotonic() - self.last_stamp + + values = {"topic": self.topic, "rate_hz": f"{rate:.1f}"} + if age is None: + level = DiagnosticStatus.ERROR if self.critical else DiagnosticStatus.WARN + return level, "no messages received", values + values["age_s"] = f"{age:.1f}" + if age > self.timeout: + level = DiagnosticStatus.ERROR if self.critical else DiagnosticStatus.WARN + return level, f"stale ({age:.1f}s > {self.timeout:.1f}s)", values + if self.min_rate is not None and rate < self.min_rate: + return ( + DiagnosticStatus.WARN, + f"slow ({rate:.1f} < {self.min_rate:.1f} Hz)", + values, + ) + return DiagnosticStatus.OK, "ok", values + + +class _TfWatch: + """Freshness tracker for one TF edge.""" + + def __init__(self, spec): + self.name = spec["name"] + self.parent = spec["parent"] + self.child = spec["child"] + self.timeout = spec.get("timeout", 2.0) + self.critical = spec.get("critical", False) + + def evaluate(self, buffer, now): + values = {"transform": f"{self.parent}->{self.child}"} + try: + tf = buffer.lookup_transform(self.parent, self.child, rclpy.time.Time()) + except tf2_ros.TransformException as exc: + level = DiagnosticStatus.ERROR if self.critical else DiagnosticStatus.WARN + values["error"] = str(exc)[:80] + return level, "unavailable", values + age = (now - rclpy.time.Time.from_msg(tf.header.stamp)).nanoseconds / 1e9 + values["age_s"] = f"{age:.1f}" + if age > self.timeout: + level = DiagnosticStatus.ERROR if self.critical else DiagnosticStatus.WARN + return level, f"stale ({age:.1f}s > {self.timeout:.1f}s)", values + return DiagnosticStatus.OK, "ok", values + + +class HealthMonitor(Node): + def __init__(self): + super().__init__("health_monitor") + cfg = _load_config() + self.period = float(cfg.get("period", 1.0)) + + self.topics = [] + for spec in cfg.get("topics", []): + watch = _TopicWatch(spec) + msg_type = get_message(spec["type"]) + self.create_subscription(msg_type, spec["topic"], watch.on_msg, 10) + self.topics.append(watch) + + self.tf_watches = [_TfWatch(s) for s in cfg.get("tf", [])] + self.tf_buffer = None + if self.tf_watches: + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + self.host_status = None + if cfg.get("subscribe_diagnostics", True): + self.create_subscription( + DiagnosticArray, "diagnostics", self._on_diagnostics, 10 + ) + + home = os.environ.get("MOTE_HOME", os.path.expanduser("~/.mote")) + self._selfcheck_path = os.path.join(home, "self_check_status.yaml") + self._selfcheck_mtime = None + self._selfcheck_status = None + + self.agg_pub = self.create_publisher(DiagnosticArray, "diagnostics_agg", 10) + self.health_pub = self.create_publisher(String, "health", 10) + + self._last_tick = time.monotonic() + self.create_timer(self.period, self._tick) + + # systemd watchdog integration (no-op outside a Type=notify service). + self._sd = SdNotifier() + self._sd.ready(status="health monitor up") + + def _on_diagnostics(self, msg): + # Keep the worst host-level status from system_monitor for the roll-up. + worst = None + for status in msg.status: + if status.name.startswith("system") or status.hardware_id: + if worst is None or status.level > worst.level: + worst = status + if worst is not None: + self.host_status = worst + + def _tick(self): + now_wall = time.monotonic() + window = now_wall - self._last_tick + self._last_tick = now_wall + now_ros = self.get_clock().now() + + statuses = [] + overall = DiagnosticStatus.OK + faults = [] + + for watch in self.topics: + level, message, values = watch.evaluate(window) + statuses.append(self._status(watch.name, level, message, values)) + overall = max(overall, level) + if level >= DiagnosticStatus.WARN: + faults.append(f"{watch.name} {message}") + + if self.tf_buffer is not None: + for tf_watch in self.tf_watches: + level, message, values = tf_watch.evaluate(self.tf_buffer, now_ros) + statuses.append(self._status(tf_watch.name, level, message, values)) + overall = max(overall, level) + if level >= DiagnosticStatus.WARN: + faults.append(f"{tf_watch.name} {message}") + + if self.host_status is not None: + statuses.append(self.host_status) + overall = max(overall, self.host_status.level) + if self.host_status.level >= DiagnosticStatus.WARN: + faults.append(f"host {self.host_status.message}") + + selfcheck = self._read_selfcheck() + if selfcheck is not None: + statuses.append(selfcheck) + # A failed pre-flight is informational at runtime (bringup would not + # have started on a hard failure); surface it without forcing FAULT. + if selfcheck.level >= DiagnosticStatus.WARN: + faults.append(f"self_check {selfcheck.message}") + overall = max(overall, DiagnosticStatus.WARN) + + summary_word = LEVEL_NAME.get(overall, "UNKNOWN") + summary_text = summary_word + if faults: + summary_text = f"{summary_word}: " + ", ".join(faults) + + mote_status = self._status( + "mote", overall, summary_text, {"subsystems": str(len(statuses))} + ) + arr = DiagnosticArray(status=[mote_status, *statuses]) + arr.header.stamp = now_ros.to_msg() + self.agg_pub.publish(arr) + self.health_pub.publish(String(data=summary_text)) + + # Prove liveness to systemd only after a successful publish. + self._sd.watchdog() + + if overall >= DiagnosticStatus.WARN: + self.get_logger().warning(summary_text, throttle_duration_sec=5.0) + + def _read_selfcheck(self): + """Last pre-flight verdict written by self_check, or None if absent. + + Re-read only when the file changes so this stays cheap on every tick. + """ + try: + mtime = os.path.getmtime(self._selfcheck_path) + except OSError: + return None + if mtime != self._selfcheck_mtime: + self._selfcheck_mtime = mtime + try: + with open(self._selfcheck_path) as f: + data = yaml.safe_load(f) or {} + except (OSError, yaml.YAMLError): + return self._selfcheck_status + passed = bool(data.get("ok")) + failed = [c["name"] for c in data.get("checks", []) if not c.get("passed")] + level = DiagnosticStatus.OK if passed else DiagnosticStatus.WARN + message = "ready" if passed else "failed: " + ", ".join(failed) + self._selfcheck_status = self._status( + "self_check", level, message, {"at": str(data.get("timestamp", ""))} + ) + return self._selfcheck_status + + @staticmethod + def _status(name, level, message, values): + return DiagnosticStatus( + name=name, + level=level, + message=message, + hardware_id="mote", + values=[KeyValue(key=k, value=str(v)) for k, v in values.items()], + ) + + +def main(): + rclpy.init() + node = HealthMonitor() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/mote_bringup/mote_bringup/sd_notify.py b/mote_bringup/mote_bringup/sd_notify.py new file mode 100644 index 0000000..8a7c5cc --- /dev/null +++ b/mote_bringup/mote_bringup/sd_notify.py @@ -0,0 +1,71 @@ +"""Minimal sd_notify client for systemd service integration. + +Sends readiness, status, and watchdog keep-alive datagrams to the socket named +by ``$NOTIFY_SOCKET`` (set by systemd for ``Type=notify`` services with +``NotifyAccess`` allowing this process). No dependency on the ``systemd`` python +package — it is just an ``AF_UNIX`` datagram, so this works in the plain robot +pixi env. + +When ``$NOTIFY_SOCKET`` is unset (running outside systemd, e.g. ``pixi run +health`` on a workstation) every call is a silent no-op, so the same node runs +identically under systemd and by hand. +""" + +import os +import socket + + +class SdNotifier: + def __init__(self): + self._sock = None + addr = os.environ.get("NOTIFY_SOCKET") + if not addr: + return + # Abstract namespace sockets are named with a leading '@' by systemd. + if addr[0] == "@": + addr = "\0" + addr[1:] + try: + self._sock = socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM) + self._addr = addr + except OSError: + self._sock = None + + @property + def enabled(self): + return self._sock is not None + + def _send(self, message): + if self._sock is None: + return + try: + self._sock.sendto(message.encode("utf-8"), self._addr) + except OSError: + pass + + def ready(self, status=None): + msg = "READY=1" + if status: + msg += f"\nSTATUS={status}" + self._send(msg) + + def status(self, text): + self._send(f"STATUS={text}") + + def watchdog(self): + self._send("WATCHDOG=1") + + @staticmethod + def watchdog_period_s(): + """Recommended keep-alive period: half of ``WatchdogSec``. + + systemd exports the timeout as ``WATCHDOG_USEC`` (microseconds) when a + watchdog is configured; petting at half that interval leaves margin for + jitter. Returns ``None`` when no watchdog is set. + """ + usec = os.environ.get("WATCHDOG_USEC") + if not usec: + return None + try: + return int(usec) / 1e6 / 2.0 + except ValueError: + return None diff --git a/mote_bringup/mote_bringup/self_check.py b/mote_bringup/mote_bringup/self_check.py new file mode 100644 index 0000000..b666c18 --- /dev/null +++ b/mote_bringup/mote_bringup/self_check.py @@ -0,0 +1,211 @@ +"""Startup self-check: prove the hardware is present and ready before bringup. + +Run as ``ExecStartPre`` of ``mote-bringup.service`` (and by hand via +``pixi run self-check``). It performs fast, static pre-flight checks — no ROS +graph, so it is cheap and can gate the launch: + +* drive servos answer a bus ping (``servo_ping``, SCServo) +* lidar and camera devices enumerate and open +* enough free disk for logs and bags +* the wall clock looks synchronised (an RTC-less Pi boots at the epoch) +* ``robot.yaml`` parses and the active site resolves + +Device paths and servo IDs come from ``robot.yaml`` (the single source of +truth). Each check is CRITICAL or advisory: any failed CRITICAL check makes the +process exit non-zero, which fails the ExecStartPre and keeps the robot in a +safe idle (systemd retries with backoff, so replugging the lidar self-heals). +Advisory failures are logged but do not block. + +The result is written to ``$MOTE_HOME/self_check_status.yaml`` (default +``~/.mote``) so the running ``health_monitor`` can surface the last pre-flight +verdict on ``/diagnostics_agg`` after boot. It is also printed to stdout, which +journald captures. + +Runtime data-flow liveness (is ``/scan`` actually publishing?) is deliberately +*not* checked here — the drivers are not up yet. That is the health_monitor's +job once bringup runs; this stage proves the hardware is present and claimable. +""" + +import argparse +import os +import shutil +import stat +import subprocess +import sys +import time +from datetime import datetime, timezone + +import yaml +from ament_index_python.packages import get_package_share_directory + +CRITICAL = "CRITICAL" +ADVISORY = "advisory" + +# Free-space floor. Below the critical bound the robot should not start +# (recording would fill the disk); the warn bound is an early heads-up. +DISK_CRITICAL_MB = 500 +DISK_WARN_MB = 2048 + +# A clock before this instant means NTP has not synced yet (or there is no RTC), +# which corrupts every bag and TF stamp. 2024-01-01 UTC. +CLOCK_FLOOR_EPOCH = 1704067200 + + +class Result: + def __init__(self): + self.checks = [] + self.ok = True + + def add(self, name, passed, severity, detail): + self.checks.append( + {"name": name, "passed": passed, "severity": severity, "detail": detail} + ) + if not passed and severity == CRITICAL: + self.ok = False + mark = "PASS" if passed else ("FAIL" if severity == CRITICAL else "WARN") + print(f"[{mark:4}] {name}: {detail}") + + +def _load_robot_cfg(): + share = get_package_share_directory("mote_description") + with open(os.path.join(share, "config", "robot.yaml")) as f: + return yaml.safe_load(f) + + +def _mote_home(): + return os.environ.get("MOTE_HOME", os.path.expanduser("~/.mote")) + + +def _check_device(result, name, path, severity): + if not os.path.exists(path): + result.add(name, False, severity, f"{path} not present") + return False + try: + mode = os.stat(path).st_mode + if not (stat.S_ISCHR(mode) or stat.S_ISBLK(mode)): + # A dangling symlink resolves but is not a device node. + result.add(name, False, severity, f"{path} is not a device node") + return False + fd = os.open(path, os.O_RDWR | os.O_NONBLOCK) + os.close(fd) + except OSError as exc: + result.add(name, False, severity, f"{path} present but not openable: {exc}") + return False + result.add(name, True, severity, f"{path} present and openable") + return True + + +def _check_servos(result, cfg, do_ping): + servos = cfg["servos"] + port, baud = servos["port"], servos["baud_rate"] + ids = [servos["left_id"], servos["right_id"]] + if not _check_device(result, "servo_bus", port, CRITICAL): + return + if not do_ping: + result.add("servo_ping", True, ADVISORY, "skipped (--no-servo-ping)") + return + cmd = ["ros2", "run", "mote_hardware", "servo_ping", port, str(baud)] + cmd += [str(i) for i in ids] + try: + proc = subprocess.run(cmd, capture_output=True, text=True, timeout=20) + except FileNotFoundError: + result.add("servo_ping", True, ADVISORY, "servo_ping tool not built; skipped") + return + except subprocess.TimeoutExpired: + result.add("servo_ping", False, CRITICAL, "servo_ping timed out") + return + detail = proc.stdout.strip().replace("\n", "; ") or proc.stderr.strip() + result.add("servo_ping", proc.returncode == 0, CRITICAL, detail or "no output") + + +def _check_disk(result): + home = _mote_home() + target = home if os.path.exists(home) else os.path.expanduser("~") + free_mb = shutil.disk_usage(target).free / (1024 * 1024) + detail = f"{free_mb:.0f} MB free on {target}" + if free_mb < DISK_CRITICAL_MB: + result.add("disk_space", False, CRITICAL, detail) + elif free_mb < DISK_WARN_MB: + result.add("disk_space", False, ADVISORY, detail + " (low)") + else: + result.add("disk_space", True, CRITICAL, detail) + + +def _check_clock(result): + now = time.time() + synced = now >= CLOCK_FLOOR_EPOCH + iso = datetime.fromtimestamp(now, timezone.utc).isoformat(timespec="seconds") + result.add( + "clock", + synced, + ADVISORY, + f"system time {iso}" + ("" if synced else " (unsynced — pre-2024)"), + ) + + +def _check_config(result, cfg): + ok = "wheel_radius" in cfg and "servos" in cfg + result.add("robot_yaml", ok, CRITICAL, "robot.yaml parsed" if ok else "malformed") + # Active site is only needed for navigation against a saved map; mapping runs + # without one, so a missing site is advisory. + try: + from mote_bringup import sites + + act = sites.active() + detail = f"active site {act[0]}/{act[1]}" if act else "no active site selected" + result.add("active_site", act is not None, ADVISORY, detail) + except Exception as exc: # sites resolution is best-effort here + result.add("active_site", False, ADVISORY, f"unresolved: {exc}") + + +def _write_status(result): + home = _mote_home() + try: + os.makedirs(home, exist_ok=True) + payload = { + "ok": result.ok, + "timestamp": datetime.now(timezone.utc).isoformat(timespec="seconds"), + "checks": result.checks, + } + with open(os.path.join(home, "self_check_status.yaml"), "w") as f: + yaml.safe_dump(payload, f, sort_keys=False) + except OSError as exc: + print(f"[WARN] could not write self_check_status.yaml: {exc}") + + +def run(do_ping=True): + result = Result() + try: + cfg = _load_robot_cfg() + except Exception as exc: + result.add("robot_yaml", False, CRITICAL, f"could not load robot.yaml: {exc}") + _write_status(result) + return result + _check_servos(result, cfg, do_ping) + lidar_port = cfg.get("lidar", {}).get("port", "/dev/mote_lidar") + _check_device(result, "lidar", lidar_port, CRITICAL) + cam_dev = cfg.get("camera", {}).get("device", "/dev/mote_camera") + _check_device(result, "camera", cam_dev, ADVISORY) + _check_disk(result) + _check_clock(result) + _check_config(result, cfg) + _write_status(result) + return result + + +def main(): + parser = argparse.ArgumentParser(description="Mote startup self-check") + parser.add_argument( + "--no-servo-ping", + action="store_true", + help="skip the active servo bus ping (device presence still checked)", + ) + args = parser.parse_args() + result = run(do_ping=not args.no_servo_ping) + verdict = "READY" if result.ok else "NOT READY — mission blocked" + print(f"self-check: {verdict}") + sys.exit(0 if result.ok else 1) + + +if __name__ == "__main__": + main() diff --git a/mote_bringup/package.xml b/mote_bringup/package.xml index 1fa30b4..5775975 100644 --- a/mote_bringup/package.xml +++ b/mote_bringup/package.xml @@ -9,6 +9,10 @@ python3-yaml diagnostic_msgs + std_msgs + sensor_msgs + tf2_ros + rosidl_runtime_py ament_python diff --git a/mote_bringup/setup.py b/mote_bringup/setup.py index a4a7e72..6ae223c 100644 --- a/mote_bringup/setup.py +++ b/mote_bringup/setup.py @@ -26,6 +26,8 @@ "console_scripts": [ "odom_tf_relay = mote_bringup.odom_tf_relay:main", "system_monitor = mote_bringup.system_monitor:main", + "health_monitor = mote_bringup.health_monitor:main", + "self_check = mote_bringup.self_check:main", "bag_pruner = mote_bringup.bag_pruner:main", "site = mote_bringup.sites:main", ], diff --git a/mote_bringup/systemd/install.sh b/mote_bringup/systemd/install.sh index 42fe887..bc51e34 100755 --- a/mote_bringup/systemd/install.sh +++ b/mote_bringup/systemd/install.sh @@ -12,5 +12,10 @@ for unit in "$SRC_DIR"/*.service; do | sudo tee "/etc/systemd/system/$(basename "$unit")" > /dev/null done +# Bound the journal so the always-restarting services can never fill the disk. +sudo mkdir -p /etc/systemd/journald.conf.d +sudo cp "$SRC_DIR/journald-mote.conf" /etc/systemd/journald.conf.d/journald-mote.conf +sudo systemctl restart systemd-journald + sudo systemctl daemon-reload -sudo systemctl enable mote-bringup mote-slam mote-nav mote-record +sudo systemctl enable mote-bringup mote-slam mote-nav mote-record mote-health diff --git a/mote_bringup/systemd/journald-mote.conf b/mote_bringup/systemd/journald-mote.conf new file mode 100644 index 0000000..a4045c7 --- /dev/null +++ b/mote_bringup/systemd/journald-mote.conf @@ -0,0 +1,12 @@ +# Mote journald sizing — installed to /etc/systemd/journald.conf.d/ by +# systemd/install.sh. Keeps persistent logs (so post-mortems survive a reboot) +# but bounds them so the always-restarting services can never fill the SD card. +[Journal] +Storage=persistent +SystemMaxUse=500M +SystemKeepFree=1G +MaxRetentionSec=2week +# Rate-limit a crash-looping service so a tight restart loop cannot drown the +# journal (0 disables the burst cap; here we allow bursts but bound them). +RateLimitIntervalSec=30s +RateLimitBurst=10000 diff --git a/mote_bringup/systemd/mote-bringup.service b/mote_bringup/systemd/mote-bringup.service index 56f82d8..238248f 100644 --- a/mote_bringup/systemd/mote-bringup.service +++ b/mote_bringup/systemd/mote-bringup.service @@ -1,15 +1,36 @@ [Unit] Description=Mote Hardware Bringup -After=network.target +# Order after the drive/lidar devices are enumerated by udev. The udev rules +# tag these devices TAG+="systemd", which makes systemd synthesise the +# dev-mote_*.device units referenced here. Wants (not Requires) gives ordering +# without hard-binding: a device that flaps mid-run must not force-kill the +# whole stack — the ExecStartPre self-check and the health_monitor handle +# presence and liveness instead. +After=network.target dev-mote_servos.device dev-mote_lidar.device +Wants=dev-mote_servos.device dev-mote_lidar.device +# Never permanently give up restarting: an unattended robot must self-heal when +# reconnected hardware reappears, so disable the start-rate cutoff and lean on +# RestartSec backoff (below) to avoid hammering the bus. +StartLimitIntervalSec=0 [Service] Type=simple User=@USER@ WorkingDirectory=@HOME@/Mote -ExecStart=@HOME@/.pixi/bin/pixi run launch -Restart=on-failure -RestartSec=5 Environment=HOME=@HOME@ +# Pre-flight gate: prove servos ping and the lidar/disk/clock are ready before +# opening the hardware. A non-zero exit fails the start; Restart re-runs this +# (with backoff) until it passes, so replugging the lidar recovers on its own. +ExecStartPre=@HOME@/.pixi/bin/pixi run self-check +ExecStart=@HOME@/.pixi/bin/pixi run launch +Restart=always +RestartSec=2 +# Back off on repeated failures: 2s, stepping up to a 30s ceiling, instead of a +# fixed-interval hammer. +RestartSteps=5 +RestartMaxDelaySec=30 +# The self-check ExecStartPre runs under pixi and can take a few seconds. +TimeoutStartSec=120 [Install] WantedBy=multi-user.target diff --git a/mote_bringup/systemd/mote-health.service b/mote_bringup/systemd/mote-health.service new file mode 100644 index 0000000..f88a83d --- /dev/null +++ b/mote_bringup/systemd/mote-health.service @@ -0,0 +1,29 @@ +[Unit] +Description=Mote Health Monitor +After=mote-bringup.service +Wants=mote-bringup.service +PartOf=mote-bringup.service +StartLimitIntervalSec=0 + +[Service] +# Type=notify + WatchdogSec is the watchdog integration point. The health +# monitor sends READY=1 once its executor is up and pets the watchdog on every +# publish; if the monitor itself hangs and stops publishing, systemd kills and +# restarts it (a safe, isolated restart — it does not touch the drive stack). +# The monitor is deliberately the feeder rather than the mission services: it is +# the process whose whole job is to stay alive and observe, so watching *it* is +# the low-risk place to bind a watchdog. +Type=notify +NotifyAccess=all +WatchdogSec=15 +User=@USER@ +WorkingDirectory=@HOME@/Mote +Environment=HOME=@HOME@ +ExecStart=@HOME@/.pixi/bin/pixi run health +Restart=always +RestartSec=2 +RestartSteps=5 +RestartMaxDelaySec=30 + +[Install] +WantedBy=multi-user.target diff --git a/mote_bringup/systemd/mote-nav.service b/mote_bringup/systemd/mote-nav.service index 5a0f30c..6367c3c 100644 --- a/mote_bringup/systemd/mote-nav.service +++ b/mote_bringup/systemd/mote-nav.service @@ -2,15 +2,20 @@ Description=Mote Navigation After=mote-slam.service Requires=mote-slam.service +BindsTo=mote-slam.service +PartOf=mote-slam.service +StartLimitIntervalSec=0 [Service] Type=simple User=@USER@ WorkingDirectory=@HOME@/Mote -ExecStart=@HOME@/.pixi/bin/pixi run nav -Restart=on-failure -RestartSec=5 Environment=HOME=@HOME@ +ExecStart=@HOME@/.pixi/bin/pixi run nav +Restart=always +RestartSec=2 +RestartSteps=5 +RestartMaxDelaySec=30 [Install] WantedBy=multi-user.target diff --git a/mote_bringup/systemd/mote-record.service b/mote_bringup/systemd/mote-record.service index 6da2aef..7a01210 100644 --- a/mote_bringup/systemd/mote-record.service +++ b/mote_bringup/systemd/mote-record.service @@ -1,16 +1,22 @@ [Unit] Description=Mote Data Recording After=mote-bringup.service +# Wants + PartOf: recording follows bringup (restarts when it does) but is not +# required for it — a recorder crash must never take down the drive stack. Wants=mote-bringup.service +PartOf=mote-bringup.service +StartLimitIntervalSec=0 [Service] Type=simple User=@USER@ WorkingDirectory=@HOME@/Mote -ExecStart=@HOME@/.pixi/bin/pixi run record -Restart=on-failure -RestartSec=5 Environment=HOME=@HOME@ +ExecStart=@HOME@/.pixi/bin/pixi run record +Restart=always +RestartSec=2 +RestartSteps=5 +RestartMaxDelaySec=30 [Install] WantedBy=multi-user.target diff --git a/mote_bringup/systemd/mote-slam.service b/mote_bringup/systemd/mote-slam.service index 01bb7d8..4a0f7ea 100644 --- a/mote_bringup/systemd/mote-slam.service +++ b/mote_bringup/systemd/mote-slam.service @@ -1,16 +1,24 @@ [Unit] Description=Mote SLAM After=mote-bringup.service +# BindsTo so SLAM stops if bringup goes down and restarts with it (its map frame +# is meaningless without live odom/scan), and PartOf so a bringup restart +# propagates. Requires alone would not restart SLAM when bringup restarts. Requires=mote-bringup.service +BindsTo=mote-bringup.service +PartOf=mote-bringup.service +StartLimitIntervalSec=0 [Service] Type=simple User=@USER@ WorkingDirectory=@HOME@/Mote -ExecStart=@HOME@/.pixi/bin/pixi run slam -Restart=on-failure -RestartSec=5 Environment=HOME=@HOME@ +ExecStart=@HOME@/.pixi/bin/pixi run slam +Restart=always +RestartSec=2 +RestartSteps=5 +RestartMaxDelaySec=30 [Install] WantedBy=multi-user.target diff --git a/mote_bringup/test/chaos/README.md b/mote_bringup/test/chaos/README.md new file mode 100644 index 0000000..d67dfa3 --- /dev/null +++ b/mote_bringup/test/chaos/README.md @@ -0,0 +1,44 @@ +# Chaos validation + +Two scripts prove the reliability stack actually recovers, split by what they +need: + +## `chaos_policy_demo.sh` — local, no hardware + +Proves the **systemd layer**: a transient `--user` unit mirroring the mote +services' `Restart=always` + `RestartSec`/`RestartSteps`/`RestartMaxDelaySec` +policy is SIGKILLed, and systemd restarts it (new MainPID) within a bounded +time. Runs on any workstation with a user systemd manager — no ROS, no robot. + +```bash +bash mote_bringup/test/chaos/chaos_policy_demo.sh +``` + +The committed `chaos_log.txt` is the output of this run. + +## `chaos_restart.sh` — on the robot + +Proves **per-node respawn**: with the stack up on `auldbot` (services active, or +`pixi run robot` / `pixi run mapping` running), it SIGKILLs `ros2_control_node`, +`sllidar_node`, and `controller_server` in turn and waits for each to reappear +within 30 s. Recovery comes from `respawn=True` on those nodes in +`mote_launch.py` / `nav2_launch.py`. + +```bash +pixi run chaos # on the robot +``` + +It aborts safely (exit 2, nothing killed) if the stack is not running, so it is +harmless to invoke on a workstation. Nodes are matched by executable name and +the script excludes its own PID, so `pkill`-style self-matching cannot happen. + +**This half must be benched on the robot with Michael** — it is not part of CI +because it needs the live hardware stack. Capture its `chaos_log.txt` from that +run for the record. + +## Two layers of recovery + +1. **Node crash** → the launch system relaunches it (`respawn=True`, ~2 s). +2. **Launch / process crash** → systemd restarts the whole service (backoff). +3. **Monitor hang** → `mote-health.service` (`Type=notify` + `WatchdogSec=15`) + is killed and restarted by systemd when it stops petting the watchdog. diff --git a/mote_bringup/test/chaos/chaos_log.txt b/mote_bringup/test/chaos/chaos_log.txt new file mode 100644 index 0000000..6f65477 --- /dev/null +++ b/mote_bringup/test/chaos/chaos_log.txt @@ -0,0 +1,5 @@ +[22:49:39] === systemd restart-policy demo (mirrors mote-*.service) === +[22:49:41] started mote-chaos-demo, MainPID=593820 +[22:49:41] killing MainPID 593820 (SIGKILL) +[22:49:43] PASS: mote-chaos-demo restarted in ~2.0s, new MainPID=594009 +[22:49:43] === done === diff --git a/mote_bringup/test/chaos/chaos_policy_demo.sh b/mote_bringup/test/chaos/chaos_policy_demo.sh new file mode 100755 index 0000000..1d07581 --- /dev/null +++ b/mote_bringup/test/chaos/chaos_policy_demo.sh @@ -0,0 +1,71 @@ +#!/usr/bin/env bash +# Hardware-free proof that the hardened restart policy recovers a killed +# service within a bounded time. Uses a transient --user unit that mirrors the +# mote services' Restart settings (Restart=always + RestartSec backoff), so it +# runs on any workstation with a user systemd — no ROS, no robot. +# +# It is the local, committable half of the chaos validation: chaos_restart.sh +# proves per-node respawn on the real robot; this proves the systemd layer. +# +# bash mote_bringup/test/chaos/chaos_policy_demo.sh +set -uo pipefail + +UNIT="mote-chaos-demo" +BOUND_S=15 +LOG="$(cd "$(dirname "$0")" && pwd)/chaos_log.txt" + +log() { echo "[$(date -u +%H:%M:%S)] $*" | tee -a "$LOG"; } + +if ! systemctl --user show-environment >/dev/null 2>&1; then + log "SKIP: no user systemd manager available" + exit 0 +fi + +: > "$LOG" +log "=== systemd restart-policy demo (mirrors mote-*.service) ===" + +systemctl --user reset-failed "$UNIT" 2>/dev/null || true + +# A trivial long-running payload that writes its start epoch to a marker file so +# we can prove the process identity changed (a real restart, not a survivor). +MARK="$(mktemp)" +systemd-run --user --unit="$UNIT" \ + --property=Restart=always \ + --property=RestartSec=2 \ + --property=RestartSteps=5 \ + --property=RestartMaxDelaySec=30 \ + --property=StartLimitIntervalSec=0 \ + /usr/bin/env bash -c "echo \$\$ > '$MARK'; exec sleep 3600" >/dev/null 2>&1 + +sleep 2 +pid1=$(systemctl --user show -p MainPID --value "$UNIT") +log "started $UNIT, MainPID=$pid1" + +log "killing MainPID $pid1 (SIGKILL)" +kill -9 "$pid1" 2>/dev/null + +waited=0 +recovered=0 +while (( $(echo "$waited < $BOUND_S" | bc -l) )); do + sleep 0.5 + waited=$(echo "$waited + 0.5" | bc -l) + state=$(systemctl --user show -p ActiveState --value "$UNIT") + pid2=$(systemctl --user show -p MainPID --value "$UNIT") + if [ "$state" = "active" ] && [ -n "$pid2" ] && [ "$pid2" != "0" ] \ + && [ "$pid2" != "$pid1" ]; then + log "PASS: $UNIT restarted in ~${waited}s, new MainPID=$pid2" + recovered=1 + break + fi +done + +if [ "$recovered" -ne 1 ]; then + log "FAIL: $UNIT did not restart within ${BOUND_S}s" +fi + +systemctl --user stop "$UNIT" 2>/dev/null || true +systemctl --user reset-failed "$UNIT" 2>/dev/null || true +rm -f "$MARK" + +log "=== done ===" +[ "$recovered" -eq 1 ] diff --git a/mote_bringup/test/chaos/chaos_restart.sh b/mote_bringup/test/chaos/chaos_restart.sh new file mode 100755 index 0000000..64ad241 --- /dev/null +++ b/mote_bringup/test/chaos/chaos_restart.sh @@ -0,0 +1,86 @@ +#!/usr/bin/env bash +# Chaos test: kill critical ROS nodes on the running robot and verify each is +# relaunched within a bounded time. Run ON THE ROBOT (auldbot) while the stack +# is up (systemd services active, or `pixi run mapping`/`robot` running): +# +# pixi run chaos # kills nodes, logs recovery to chaos_log.txt +# +# Recovery is expected because the launch files mark the driver / nav2 nodes +# respawn=True (see mote_launch.py / nav2_launch.py); this proves it end to end. +# +# Safety: nodes are matched by their executable name (e.g. ros2_control_node), +# which never matches this bash script, and this script's own PID is excluded — +# so the `pkill -f from an agent shell matches itself` foot-gun cannot fire. +set -uo pipefail + +BOUND_S=30 # max seconds allowed for a node to reappear +POLL_S=0.5 +SELF=$$ +LOG="$(cd "$(dirname "$0")" && pwd)/chaos_log.txt" + +# Executable names to knock over. These are the process names, not the launch +# nodes' remapped names, so they are stable to match on. +TARGETS=( + "ros2_control_node" # controller_manager — drive control + "sllidar_node" # lidar driver — /scan + "controller_server" # nav2 local controller +) + +log() { echo "[$(date -u +%H:%M:%S)] $*" | tee -a "$LOG"; } + +pids_for() { pgrep -f "$1" | grep -vw "$SELF" || true; } + +kill_target() { + local pat="$1" pid + for pid in $(pids_for "$pat"); do + kill -9 "$pid" 2>/dev/null + done +} + +wait_recovery() { + local pat="$1" waited=0 + while (( $(echo "$waited < $BOUND_S" | bc -l) )); do + if [ -n "$(pids_for "$pat")" ]; then + echo "$waited" + return 0 + fi + sleep "$POLL_S" + waited=$(echo "$waited + $POLL_S" | bc -l) + done + echo "$waited" + return 1 +} + +: > "$LOG" +log "=== mote chaos restart test on $(hostname) ===" +log "bound=${BOUND_S}s, targets: ${TARGETS[*]}" + +# Precondition: the stack must be up, or there is nothing to knock over. +missing=0 +for t in "${TARGETS[@]}"; do + [ -z "$(pids_for "$t")" ] && { log "PRECONDITION: '$t' not running"; missing=1; } +done +if [ "$missing" -ne 0 ]; then + log "ABORT: run this on the robot with the stack up (services active or" + log " 'pixi run robot' / 'pixi run mapping' running). Nothing killed." + exit 2 +fi + +fails=0 +for t in "${TARGETS[@]}"; do + before=$(pids_for "$t" | tr '\n' ' ') + log "killing '$t' (pids: $before)" + kill_target "$t" + sleep 1 + if elapsed=$(wait_recovery "$t"); then + after=$(pids_for "$t" | tr '\n' ' ') + log "PASS '$t' recovered in ~${elapsed}s (pids: $after)" + else + log "FAIL '$t' did NOT recover within ${BOUND_S}s" + fails=$((fails + 1)) + fi + sleep 3 # let it settle before the next scenario +done + +log "=== done: $((${#TARGETS[@]} - fails))/${#TARGETS[@]} recovered ===" +exit "$fails" diff --git a/mote_bringup/test/test_health_monitor.py b/mote_bringup/test/test_health_monitor.py new file mode 100644 index 0000000..7301621 --- /dev/null +++ b/mote_bringup/test/test_health_monitor.py @@ -0,0 +1,93 @@ +"""health_monitor freshness/rate logic drives the OK/DEGRADED/FAULT roll-up.""" + +import time + +from diagnostic_msgs.msg import DiagnosticStatus + +from mote_bringup.health_monitor import _TopicWatch +from mote_bringup.sd_notify import SdNotifier + + +def _watch(critical=True, min_rate=5.0, timeout=2.0): + return _TopicWatch( + { + "name": "scan", + "topic": "/scan", + "min_rate": min_rate, + "timeout": timeout, + "critical": critical, + } + ) + + +def test_never_received_critical_is_fault(): + w = _watch(critical=True) + level, msg, _ = w.evaluate(window=1.0) + assert level == DiagnosticStatus.ERROR + assert "no messages" in msg + + +def test_never_received_noncritical_is_degraded(): + w = _watch(critical=False) + level, _, _ = w.evaluate(window=1.0) + assert level == DiagnosticStatus.WARN + + +def test_fresh_and_fast_is_ok(): + w = _watch(min_rate=5.0) + for _ in range(10): + w.on_msg(None) + level, msg, values = w.evaluate(window=1.0) # 10 msgs / 1s = 10 Hz + assert level == DiagnosticStatus.OK + assert msg == "ok" + assert float(values["rate_hz"]) >= 5.0 + + +def test_fresh_but_slow_is_degraded(): + w = _watch(min_rate=5.0) + w.on_msg(None) # a single message this window -> ~1 Hz + level, msg, _ = w.evaluate(window=1.0) + assert level == DiagnosticStatus.WARN + assert "slow" in msg + + +def test_stale_critical_is_fault(): + w = _watch(critical=True, timeout=2.0) + w.on_msg(None) + w.last_stamp = time.monotonic() - 10.0 # last seen 10s ago + level, msg, _ = w.evaluate(window=1.0) + assert level == DiagnosticStatus.ERROR + assert "stale" in msg + + +def test_stale_noncritical_is_degraded(): + w = _watch(critical=False, timeout=2.0) + w.on_msg(None) + w.last_stamp = time.monotonic() - 10.0 + level, _, _ = w.evaluate(window=1.0) + assert level == DiagnosticStatus.WARN + + +def test_recovery_back_to_ok(): + w = _watch(min_rate=5.0, timeout=2.0) + w.last_stamp = time.monotonic() - 10.0 + assert w.evaluate(window=1.0)[0] == DiagnosticStatus.ERROR + for _ in range(10): + w.on_msg(None) + assert w.evaluate(window=1.0)[0] == DiagnosticStatus.OK + + +def test_sd_notify_noop_without_socket(monkeypatch): + monkeypatch.delenv("NOTIFY_SOCKET", raising=False) + sd = SdNotifier() + assert not sd.enabled + # All calls must be safe no-ops when not under systemd. + sd.ready() + sd.watchdog() + sd.status("x") + assert SdNotifier.watchdog_period_s() is None + + +def test_sd_notify_watchdog_period(monkeypatch): + monkeypatch.setenv("WATCHDOG_USEC", "15000000") # 15 s + assert SdNotifier.watchdog_period_s() == 7.5 diff --git a/mote_bringup/test/test_self_check.py b/mote_bringup/test/test_self_check.py new file mode 100644 index 0000000..bf7c88f --- /dev/null +++ b/mote_bringup/test/test_self_check.py @@ -0,0 +1,80 @@ +"""self_check gates mission readiness on static pre-flight hardware checks.""" + +import time + +from mote_bringup import self_check +from mote_bringup.self_check import ADVISORY, CRITICAL, Result + + +def test_result_critical_failure_blocks(): + r = Result() + r.add("servo_bus", True, CRITICAL, "ok") + assert r.ok + r.add("lidar", False, CRITICAL, "unplugged") + assert not r.ok + + +def test_result_advisory_failure_does_not_block(): + r = Result() + r.add("camera", False, ADVISORY, "unplugged") + assert r.ok + + +def test_check_device_missing_symlink(tmp_path): + r = Result() + ok = self_check._check_device(r, "lidar", str(tmp_path / "nope"), CRITICAL) + assert not ok and not r.ok + + +def test_check_device_regular_file_is_not_a_device(tmp_path): + # A dangling-then-recreated symlink can resolve to a plain file; that must + # not read as a healthy device node. + f = tmp_path / "fake" + f.write_text("") + r = Result() + ok = self_check._check_device(r, "lidar", str(f), CRITICAL) + assert not ok and not r.ok + + +def test_check_device_real_char_device(): + # /dev/null is always a char device and openable — proves the happy path. + r = Result() + ok = self_check._check_device(r, "servo_bus", "/dev/null", CRITICAL) + assert ok and r.ok + + +def test_clock_pre_2024_is_flagged(monkeypatch): + r = Result() + monkeypatch.setattr(time, "time", lambda: 1000.0) # 1970 + self_check._check_clock(r) + check = r.checks[-1] + assert not check["passed"] and check["severity"] == ADVISORY + assert r.ok # advisory, so it does not block + + +def test_clock_now_passes(): + r = Result() + self_check._check_clock(r) + assert r.checks[-1]["passed"] + + +def test_disk_check_reports_free_space(): + r = Result() + self_check._check_disk(r) + check = r.checks[-1] + assert check["name"] == "disk_space" + assert "free" in check["detail"] + + +def test_write_status_roundtrip(tmp_path, monkeypatch): + monkeypatch.setenv("MOTE_HOME", str(tmp_path)) + r = Result() + r.add("servo_bus", True, CRITICAL, "ok") + r.add("lidar", False, CRITICAL, "unplugged") + self_check._write_status(r) + import yaml + + data = yaml.safe_load((tmp_path / "self_check_status.yaml").read_text()) + assert data["ok"] is False + names = {c["name"] for c in data["checks"]} + assert names == {"servo_bus", "lidar"} diff --git a/mote_bringup/udev/99-mote.rules b/mote_bringup/udev/99-mote.rules index bae2012..6a95027 100644 --- a/mote_bringup/udev/99-mote.rules +++ b/mote_bringup/udev/99-mote.rules @@ -8,11 +8,15 @@ # disambiguate. Find a device's serial with: # udevadm info -a -n /dev/ttyUSB0 | grep '{serial}' +# TAG+="systemd" makes systemd synthesise dev-mote_servos.device / +# dev-mote_lidar.device units so mote-bringup.service can order after the +# hardware is actually present (see the unit's After=/Wants=). + # Waveshare Serial Bus Servo Driver (CH343) -SUBSYSTEM=="tty", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d3", SYMLINK+="mote_servos", MODE="0666" +SUBSYSTEM=="tty", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="55d3", SYMLINK+="mote_servos", MODE="0666", TAG+="systemd" # SLAMTEC RPLIDAR C1 (Silicon Labs CP2102N) -SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", SYMLINK+="mote_lidar", MODE="0666" +SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", SYMLINK+="mote_lidar", MODE="0666", TAG+="systemd" # UGREEN USB Camera — UVC cameras expose both a capture and a metadata video # node; ID_V4L_CAPABILITIES pins the symlink to the capture node. diff --git a/mote_hardware/CMakeLists.txt b/mote_hardware/CMakeLists.txt index b5e874d..eba3ec8 100644 --- a/mote_hardware/CMakeLists.txt +++ b/mote_hardware/CMakeLists.txt @@ -62,13 +62,19 @@ add_executable(setup_ids target_link_libraries(setup_ids mote_hardware SCServo::SCServo) target_compile_features(setup_ids PUBLIC cxx_std_17) +add_executable(servo_ping + tools/servo_ping.cpp +) +target_link_libraries(servo_ping mote_hardware SCServo::SCServo) +target_compile_features(servo_ping PUBLIC cxx_std_17) + install(TARGETS mote_hardware ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -install(TARGETS servo_debug velocity_cal swap_ids setup_ids +install(TARGETS servo_debug velocity_cal swap_ids setup_ids servo_ping RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/mote_hardware/tools/README.md b/mote_hardware/tools/README.md index ff241d9..e87c27c 100644 --- a/mote_hardware/tools/README.md +++ b/mote_hardware/tools/README.md @@ -30,6 +30,19 @@ ros2 run mote_hardware servo_debug [port] [baud] Commands: `mv `, `stop `, `stopall`, `r `, `m [hz]`, `ping `. Type `help` for the full list. +## servo_ping + +Non-interactive counterpart of `servo_debug`'s `ping` — pings a fixed set of +servo IDs and exits `0` if every one responded, `1` otherwise. Used by the +startup self-check (`mote_bringup` `self_check.py`) to gate bringup on the drive +servos actually answering. Run the bus free (before bringup opens it). + +``` +ros2 run mote_hardware servo_ping [port] [baud] [id ...] +``` + +Defaults: `/dev/mote_servos 1000000`, IDs `7 9` (the drive IDs in `robot.yaml`). + ## velocity_cal Measures `velocity_scale` (the rad/s → raw servo units conversion factor) by diff --git a/mote_hardware/tools/servo_ping.cpp b/mote_hardware/tools/servo_ping.cpp new file mode 100644 index 0000000..d35533d --- /dev/null +++ b/mote_hardware/tools/servo_ping.cpp @@ -0,0 +1,58 @@ +// Non-interactive servo ping for startup self-checks. +// +// Pings a fixed set of servo IDs on the drive bus and exits with a status +// code: 0 if every requested ID responded, 1 if any did not (or the bus could +// not be opened). Prints one line per ID plus a summary, so a self-check +// script can both gate on the exit code and log which servo is missing. +// +// The interactive `servo_debug` tool has a `ping` command for exploring the +// bus by hand; this is the scriptable counterpart that a service can run +// before bringup opens the bus. +// +// Build: added as `servo_ping` executable in mote_hardware/CMakeLists.txt. +// Run: ros2 run mote_hardware servo_ping [port] [baud] [id ...] +// defaults: /dev/mote_servos 1000000, IDs 7 and 9 (robot.yaml drive IDs) + +#include "SMS_STS.h" + +#include +#include +#include +#include + +int main(int argc, char *argv[]) +{ + const char *port = (argc > 1) ? argv[1] : "/dev/mote_servos"; + int baud = (argc > 2) ? std::atoi(argv[2]) : 1000000; + + std::vector ids; + for (int i = 3; i < argc; ++i) ids.push_back(std::atoi(argv[i])); + if (ids.empty()) ids = {7, 9}; + + SMS_STS sms; + if (!sms.begin(baud, port)) { + std::printf("FAIL: could not open %s @ %d\n", port, baud); + return 1; + } + + int missing = 0; + for (int id : ids) { + // ReadPos returns -1 when the servo does not answer within the SDK's + // read timeout, which is how servo_debug's ping detects presence. + int pos = sms.ReadPos(id); + if (pos >= 0) { + std::printf("id=%d OK (pos=%d)\n", id, pos); + } else { + std::printf("id=%d MISSING (no response)\n", id); + ++missing; + } + } + sms.end(); + + if (missing) { + std::printf("FAIL: %d/%zu servos did not respond\n", missing, ids.size()); + return 1; + } + std::printf("OK: all %zu servos responded\n", ids.size()); + return 0; +} diff --git a/pixi.toml b/pixi.toml index 31f645e..5d146db 100644 --- a/pixi.toml +++ b/pixi.toml @@ -39,6 +39,9 @@ teleop = "ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p sta perception = "ros2 launch mote_perception perception_launch.py" record = "ros2 launch mote_bringup record_launch.py" tasks = "ros2 launch mote_tasks tasks_launch.py" +health = "ros2 run mote_bringup health_monitor" +self-check = "ros2 run mote_bringup self_check" +chaos = "bash mote_bringup/test/chaos/chaos_restart.sh" #### The below tasks are tied to specific features and environments however they #### can be run like any other task.