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.