feat: Jetson Orin system monitor ROS2 node (Issue #631)

New package saltybot_system_monitor:
- jetson_stats.py: pure-Python data layer (JetsonStats, CpuCore,
  TegrastatsParser, JtopReader, TegrastatsReader, MockReader,
  AlertChecker, AlertThresholds) — no ROS2 dependency
- system_monitor_node.py: ROS2 node publishing /saltybot/system/stats
  (JSON) and /saltybot/diagnostics (DiagnosticArray) at 1 Hz
- Alerts: CPU/GPU >85% WARN (+10% ERROR), temp >80°C, disk/RAM >90%,
  power >30 W; each alert produces a DiagnosticStatus entry
- Stats source priority: jtop > tegrastats > mock (auto-detected)
- config/system_monitor.yaml: all thresholds and rate tunable via params
- launch/system_monitor.launch.py: single-node launch with config arg
- test/test_system_monitor.py: 50+ pytest tests, ROS2-free

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

chassis/cable_tray.scad

@@ -0,0 +1,410 @@
+// ============================================================
+// Cable Management Tray — Issue #628
+// Agent   : sl-mechanical
+// Date    : 2026-03-15
+// Part catalogue:
+//   1. tray_body     — under-plate tray with snap-in cable channels, Velcro
+//                      tie-down slots every 40 mm, pass-through holes, label slots
+//   2. tnut_bracket  — 2020 T-nut rail mount bracket (×2, slide under tray)
+//   3. channel_clip  — snap-in divider clip separating power / signal / servo zones
+//   4. cover_panel   — hinged snap-on lid (living-hinge PETG flexure strip)
+//   5. cable_saddle  — individual cable saddle / strain-relief clip (×n)
+//
+// BOM:
+//   4 × M5×10 BHCS + M5 T-nuts    (tnut_bracket × 2 to rail)
+//   4 × M3×8  SHCS                (tnut_bracket to tray body)
+//   n × 100 mm Velcro tie-down strips (through 6×2 mm slots, every 40 mm)
+//
+// Cable channel layout (X axis, inside tray):
+//   Zone A — Power  (2S–6S LiPo, XT60/XT30): 20 mm wide, 14 mm deep
+//   Zone B — Signal (JST-SH, PWM, I2C, UART): 14 mm wide, 10 mm deep
+//   Zone C — Servo  (JST-PH, thick servo leads): 14 mm wide, 12 mm deep
+//   Divider walls: 2.5 mm thick between zones
+//
+// Print settings (PETG):
+//   tray_body / tnut_bracket / channel_clip : 5 perimeters, 40 % gyroid, no supports
+//   cover_panel                             : 3 perimeters, 20 % gyroid, no supports
+//                                             (living-hinge — print flat, thin strip flexes)
+//   cable_saddle                            : 3 perimeters, 30 % gyroid, no supports
+//
+// Export commands:
+//   openscad -D 'RENDER="tray_body"'     -o tray_body.stl     cable_tray.scad
+//   openscad -D 'RENDER="tnut_bracket"'  -o tnut_bracket.stl  cable_tray.scad
+//   openscad -D 'RENDER="channel_clip"'  -o channel_clip.stl  cable_tray.scad
+//   openscad -D 'RENDER="cover_panel"'   -o cover_panel.stl   cable_tray.scad
+//   openscad -D 'RENDER="cable_saddle"'  -o cable_saddle.stl  cable_tray.scad
+//   openscad -D 'RENDER="assembly"'      -o assembly.png      cable_tray.scad
+// ============================================================
+
+RENDER = "assembly"; // tray_body | tnut_bracket | channel_clip | cover_panel | cable_saddle | assembly
+
+$fn = 48;
+EPS = 0.01;
+
+// ── 2020 rail constants ──────────────────────────────────────
+RAIL_W      = 20.0;
+TNUT_W      =  9.8;
+TNUT_H      =  5.5;
+TNUT_L      = 12.0;
+SLOT_NECK_H =  3.2;
+M5_D        =  5.2;
+M5_HEAD_D   =  9.5;
+M5_HEAD_H   =  4.0;
+
+// ── Tray geometry ────────────────────────────────────────────
+TRAY_L      = 280.0;  // length along rail (7 × 40 mm tie-down pitch)
+TRAY_W      =  60.0;  // width across rail (covers standard 40 mm rail pair)
+TRAY_WALL   =   2.5;  // side / floor wall thickness
+TRAY_DEPTH  =  18.0;  // interior depth (tallest zone + wall)
+
+// Cable channel zones (widths must sum to TRAY_W - 2*TRAY_WALL - 2*DIV_T)
+DIV_T       =   2.5;  // divider wall thickness
+ZONE_A_W    =  20.0;  // Power
+ZONE_A_D    =  14.0;
+ZONE_B_W    =  14.0;  // Signal
+ZONE_B_D    =  10.0;
+ZONE_C_W    =  14.0;  // Servo
+ZONE_C_D    =  12.0;
+// Total inner width used: ZONE_A_W + ZONE_B_W + ZONE_C_W + 2*DIV_T = 55 mm < TRAY_W - 2*TRAY_WALL = 55 mm ✓
+
+// Tie-down slots (Velcro strips)
+TIEDOWN_PITCH = 40.0;
+TIEDOWN_W   =   6.0;  // slot width (fits 6 mm wide Velcro)
+TIEDOWN_T   =   2.2;  // slot through-thickness (floor)
+TIEDOWN_CNT = 7;      // 7 positions along tray
+
+// Pass-through holes in floor
+PASSTHRU_D  =  12.0;  // circular grommet-compatible pass-through
+PASSTHRU_CNT = 3;     // one per zone, at tray mid-length
+
+// Label slots (rear outer wall)
+LABEL_W     =  24.0;
+LABEL_H     =   8.0;
+LABEL_T     =   1.0;  // depth from outer face
+
+// Snap ledge for cover
+SNAP_LEDGE_H =  2.5;
+SNAP_LEDGE_D =  1.5;
+
+// ── T-nut bracket ────────────────────────────────────────────
+BKT_L       =  60.0;
+BKT_W       =  TRAY_W;
+BKT_T       =   6.0;
+BOLT_PITCH  =  40.0;
+M3_D        =   3.2;
+M3_HEAD_D   =   6.0;
+M3_HEAD_H   =   3.0;
+M3_NUT_W    =   5.5;
+M3_NUT_H    =   2.4;
+
+// ── Cover panel ──────────────────────────────────────────────
+CVR_T       =   1.8;  // panel thickness
+HINGE_T     =   0.6;  // living-hinge strip thickness (printed in PETG)
+HINGE_W     =   3.0;  // hinge strip width (flexes easily)
+SNAP_HOOK_H =   3.5;  // snap hook height
+SNAP_HOOK_T =   2.2;
+
+// ── Cable saddle ─────────────────────────────────────────────
+SAD_W       =  12.0;
+SAD_H       =   8.0;
+SAD_T       =   2.5;
+SAD_BORE_D  =   7.0;  // cable bundle bore
+SAD_CLIP_T  =   1.6;  // snap arm thickness
+
+// ── Utilities ────────────────────────────────────────────────
+module chamfer_cube(size, ch=1.0) {
+    hull() {
+        translate([ch, ch, 0])   cube([size[0]-2*ch, size[1]-2*ch, EPS]);
+        translate([0, 0, ch])    cube(size - [0, 0, ch]);
+    }
+}
+
+module hex_pocket(af, depth) {
+    cylinder(d=af/cos(30), h=depth, $fn=6);
+}
+
+// ── Part 1: tray_body ───────────────────────────────────────
+module tray_body() {
+    difference() {
+        // Outer shell
+        union() {
+            chamfer_cube([TRAY_L, TRAY_W, TRAY_DEPTH + TRAY_WALL], ch=1.5);
+
+            // Snap ledge along top of both long walls (for cover_panel)
+            for (y = [-SNAP_LEDGE_D, TRAY_W])
+                translate([0, y, TRAY_DEPTH])
+                    cube([TRAY_L, TRAY_WALL + SNAP_LEDGE_D, SNAP_LEDGE_H]);
+        }
+
+        // Interior cavity
+        translate([TRAY_WALL, TRAY_WALL, TRAY_WALL])
+            cube([TRAY_L - 2*TRAY_WALL, TRAY_W - 2*TRAY_WALL,
+                  TRAY_DEPTH + EPS]);
+
+        // ── Zone dividers (subtract from solid to leave walls) ──
+        // Zone A (Power) inner floor cut — full depth A
+        translate([TRAY_WALL, TRAY_WALL, TRAY_WALL + (TRAY_DEPTH - ZONE_A_D)])
+            cube([TRAY_L - 2*TRAY_WALL, ZONE_A_W, ZONE_A_D + EPS]);
+
+        // Zone B (Signal) inner floor cut
+        translate([TRAY_WALL, TRAY_WALL + ZONE_A_W + DIV_T,
+                   TRAY_WALL + (TRAY_DEPTH - ZONE_B_D)])
+            cube([TRAY_L - 2*TRAY_WALL, ZONE_B_W, ZONE_B_D + EPS]);
+
+        // Zone C (Servo) inner floor cut
+        translate([TRAY_WALL, TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W + DIV_T,
+                   TRAY_WALL + (TRAY_DEPTH - ZONE_C_D)])
+            cube([TRAY_L - 2*TRAY_WALL, ZONE_C_W, ZONE_C_D + EPS]);
+
+        // ── Velcro tie-down slots (floor, every 40 mm) ──────────
+        for (i = [0:TIEDOWN_CNT-1]) {
+            x = TRAY_WALL + 20 + i * TIEDOWN_PITCH - TIEDOWN_W/2;
+            // Zone A slot
+            translate([x, TRAY_WALL + 2, -EPS])
+                cube([TIEDOWN_W, ZONE_A_W - 4, TRAY_WALL + 2*EPS]);
+            // Zone B slot
+            translate([x, TRAY_WALL + ZONE_A_W + DIV_T + 2, -EPS])
+                cube([TIEDOWN_W, ZONE_B_W - 4, TRAY_WALL + 2*EPS]);
+            // Zone C slot
+            translate([x, TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W + DIV_T + 2, -EPS])
+                cube([TIEDOWN_W, ZONE_C_W - 4, TRAY_WALL + 2*EPS]);
+        }
+
+        // ── Pass-through holes in floor (centre of each zone at mid-length) ──
+        mid_x = TRAY_L / 2;
+        // Zone A
+        translate([mid_x, TRAY_WALL + ZONE_A_W/2, -EPS])
+            cylinder(d=PASSTHRU_D, h=TRAY_WALL + 2*EPS);
+        // Zone B
+        translate([mid_x, TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W/2, -EPS])
+            cylinder(d=PASSTHRU_D, h=TRAY_WALL + 2*EPS);
+        // Zone C
+        translate([mid_x, TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W + DIV_T + ZONE_C_W/2, -EPS])
+            cylinder(d=PASSTHRU_D, h=TRAY_WALL + 2*EPS);
+
+        // ── Label slots on front wall (y = 0) — one per zone ────
+        zone_ctrs = [TRAY_WALL + ZONE_A_W/2,
+                     TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W/2,
+                     TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W + DIV_T + ZONE_C_W/2];
+        label_z = TRAY_WALL + 2;
+        for (yc = zone_ctrs)
+            translate([TRAY_L/2 - LABEL_W/2, -EPS, label_z])
+                cube([LABEL_W, LABEL_T + EPS, LABEL_H]);
+
+        // ── M3 bracket bolt holes in floor (4 corners) ──────────
+        for (x = [20, TRAY_L - 20])
+            for (y = [TRAY_W/4, 3*TRAY_W/4])
+                translate([x, y, -EPS])
+                    cylinder(d=M3_D, h=TRAY_WALL + 2*EPS);
+
+        // ── Channel clip snap sockets (top of each divider, every 80 mm) ──
+        for (i = [0:2]) {
+            cx = 40 + i * 80;
+            for (dy = [ZONE_A_W, ZONE_A_W + DIV_T + ZONE_B_W])
+                translate([cx - 3, TRAY_WALL + dy - 1, TRAY_DEPTH - 4])
+                    cube([6, DIV_T + 2, 4 + EPS]);
+        }
+    }
+
+    // ── Divider walls (positive geometry) ───────────────────
+    // Wall between Zone A and Zone B
+    translate([TRAY_WALL, TRAY_WALL + ZONE_A_W, TRAY_WALL])
+        cube([TRAY_L - 2*TRAY_WALL, DIV_T,
+              TRAY_DEPTH - ZONE_A_D]);  // partial height — lower in A zone
+
+    // Wall between Zone B and Zone C
+    translate([TRAY_WALL, TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W, TRAY_WALL])
+        cube([TRAY_L - 2*TRAY_WALL, DIV_T,
+              TRAY_DEPTH - ZONE_B_D]);
+}
+
+// ── Part 2: tnut_bracket ────────────────────────────────────
+module tnut_bracket() {
+    difference() {
+        chamfer_cube([BKT_L, BKT_W, BKT_T], ch=1.5);
+
+        // M5 T-nut holes (2 per bracket, on rail centreline)
+        for (x = [BKT_L/2 - BOLT_PITCH/2, BKT_L/2 + BOLT_PITCH/2]) {
+            translate([x, BKT_W/2, -EPS]) {
+                cylinder(d=M5_D, h=BKT_T + 2*EPS);
+                cylinder(d=M5_HEAD_D, h=M5_HEAD_H + EPS);
+            }
+            translate([x - TNUT_L/2, BKT_W/2 - TNUT_W/2, BKT_T - TNUT_H])
+                cube([TNUT_L, TNUT_W, TNUT_H + EPS]);
+        }
+
+        // M3 tray-attachment holes (4 corners)
+        for (x = [10, BKT_L - 10])
+            for (y = [10, BKT_W - 10]) {
+                translate([x, y, -EPS])
+                    cylinder(d=M3_D, h=BKT_T + 2*EPS);
+                // M3 hex nut captured pocket (from top)
+                translate([x, y, BKT_T - M3_NUT_H - 0.2])
+                    hex_pocket(M3_NUT_W + 0.3, M3_NUT_H + 0.3);
+            }
+
+        // Weight relief
+        translate([15, 8, -EPS])
+            cube([BKT_L - 30, BKT_W - 16, BKT_T/2]);
+    }
+}
+
+// ── Part 3: channel_clip ────────────────────────────────────
+// Snap-in clip that locks into divider-wall snap sockets;
+// holds individual bundles in their zone and acts as colour-coded zone marker.
+module channel_clip() {
+    clip_body_w = 6.0;
+    clip_body_h = DIV_T + 4.0;
+    clip_body_t = 8.0;
+    tab_h = 3.5;
+    tab_w = 2.5;
+
+    difference() {
+        union() {
+            // Body spanning divider
+            cube([clip_body_t, clip_body_w, clip_body_h]);
+
+            // Snap tabs (bottom, straddle divider)
+            for (s = [0, clip_body_w - tab_w])
+                translate([clip_body_t/2 - 1, s, -tab_h])
+                    cube([2, tab_w, tab_h + 1]);
+        }
+
+        // Cable radius slot on each face
+        translate([-EPS, clip_body_w/2, clip_body_h * 0.6])
+            rotate([0, 90, 0])
+                cylinder(d=5.0, h=clip_body_t + 2*EPS);
+
+        // Snap tab undercut for flex
+        for (s = [0, clip_body_w - tab_w])
+            translate([clip_body_t/2 - 2, s - EPS, -tab_h + 1.5])
+                cube([4, tab_w + 2*EPS, 1.5]);
+    }
+}
+
+// ── Part 4: cover_panel ─────────────────────────────────────
+// Flat snap-on lid with living-hinge along one long edge.
+// Print flat; PETG living hinge flexes ~90° to snap onto tray.
+module cover_panel() {
+    total_w = TRAY_W + 2 * SNAP_HOOK_T;
+
+    difference() {
+        union() {
+            // Main panel
+            cube([TRAY_L, TRAY_W, CVR_T]);
+
+            // Living hinge strip along back edge (thin, flexes)
+            translate([0, TRAY_W - EPS, 0])
+                cube([TRAY_L, HINGE_W, HINGE_T]);
+
+            // Snap hooks along front edge (clips under tray snap ledge)
+            for (x = [20, TRAY_L/2 - 20, TRAY_L/2 + 20, TRAY_L - 20])
+                translate([x - SNAP_HOOK_T/2, -SNAP_HOOK_H + EPS, 0])
+                    difference() {
+                        cube([SNAP_HOOK_T, SNAP_HOOK_H, CVR_T + 1.5]);
+                        // Hook nose chamfer
+                        translate([-EPS, -EPS, CVR_T])
+                            rotate([0, 0, 0])
+                                cube([SNAP_HOOK_T + 2*EPS, 1.5, 1.5]);
+                    }
+        }
+
+        // Ventilation slots (3 rows × 6 slots)
+        for (row = [0:2])
+            for (col = [0:5]) {
+                sx = 20 + col * 40 + row * 10;
+                sy = 10 + row * 12;
+                if (sx + 25 < TRAY_L && sy + 6 < TRAY_W)
+                    translate([sx, sy, -EPS])
+                        cube([25, 6, CVR_T + 2*EPS]);
+            }
+
+        // Zone label windows (align with tray label slots)
+        zone_ctrs = [TRAY_WALL + ZONE_A_W/2,
+                     TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W/2,
+                     TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W + DIV_T + ZONE_C_W/2];
+        for (yc = zone_ctrs)
+            translate([TRAY_L/2 - LABEL_W/2, yc - LABEL_H/2, -EPS])
+                cube([LABEL_W, LABEL_H, CVR_T + 2*EPS]);
+    }
+}
+
+// ── Part 5: cable_saddle ────────────────────────────────────
+// Snap-in cable saddle / strain-relief clip; press-fits onto tray top edge.
+module cable_saddle() {
+    arm_gap = TRAY_WALL + 0.4;  // fits over tray wall
+    arm_len = 8.0;
+
+    difference() {
+        union() {
+            // Body
+            chamfer_cube([SAD_W, SAD_T * 2 + arm_gap, SAD_H], ch=1.0);
+
+            // Cable retaining arch
+            translate([SAD_W/2, SAD_T + arm_gap/2, SAD_H])
+                scale([1, 0.6, 1])
+                    difference() {
+                        cylinder(d=SAD_BORE_D + SAD_CLIP_T * 2, h=SAD_T);
+                        translate([0, 0, -EPS])
+                            cylinder(d=SAD_BORE_D, h=SAD_T + 2*EPS);
+                        translate([-SAD_BORE_D, 0, -EPS])
+                            cube([SAD_BORE_D * 2, SAD_BORE_D, SAD_T + 2*EPS]);
+                    }
+        }
+
+        // Slot for tray wall (negative)
+        translate([0, SAD_T, -EPS])
+            cube([SAD_W, arm_gap, arm_len + EPS]);
+
+        // M3 tie-down hole
+        translate([SAD_W/2, SAD_T + arm_gap/2, -EPS])
+            cylinder(d=M3_D, h=SAD_H + 2*EPS);
+    }
+}
+
+// ── Assembly ────────────────────────────────────────────────
+module assembly() {
+    // Tray body (open face up for visibility)
+    color("SteelBlue")
+        tray_body();
+
+    // Two T-nut brackets underneath at 1/4 and 3/4 length
+    for (bx = [TRAY_L/4 - BKT_L/2, 3*TRAY_L/4 - BKT_L/2])
+        color("DodgerBlue")
+            translate([bx, 0, -BKT_T])
+                tnut_bracket();
+
+    // Channel clips (3 per divider position, every 80 mm)
+    for (i = [0:2]) {
+        cx = 40 + i * 80;
+        // Divider A/B
+        color("Tomato", 0.8)
+            translate([cx - 4, TRAY_WALL + ZONE_A_W - 2, TRAY_DEPTH - 3])
+                channel_clip();
+        // Divider B/C
+        color("Orange", 0.8)
+            translate([cx - 4,
+                       TRAY_WALL + ZONE_A_W + DIV_T + ZONE_B_W - 2,
+                       TRAY_DEPTH - 3])
+                channel_clip();
+    }
+
+    // Cover panel (raised above tray to show interior)
+    color("LightSteelBlue", 0.5)
+        translate([0, 0, TRAY_DEPTH + SNAP_LEDGE_H + 4])
+            cover_panel();
+
+    // Cable saddles along front tray edge
+    for (x = [40, 120, 200])
+        color("SlateGray")
+            translate([x - SAD_W/2, -SAD_T * 2 - TRAY_WALL, 0])
+                cable_saddle();
+}
+
+// ── Dispatch ────────────────────────────────────────────────
+if      (RENDER == "tray_body")    tray_body();
+else if (RENDER == "tnut_bracket") tnut_bracket();
+else if (RENDER == "channel_clip") channel_clip();
+else if (RENDER == "cover_panel")  cover_panel();
+else if (RENDER == "cable_saddle") cable_saddle();
+else                               assembly();

include/config.h

@@ -271,4 +271,11 @@
 #define LVC_HYSTERESIS_MV         200u  // recovery hysteresis to prevent threshold chatter
 #define LVC_TLM_HZ                 1u   // JLINK_TLM_LVC transmit rate (Hz)
 
+
+// --- UART Command Protocol (Issue #629) ---
+// Jetson-STM32 binary command protocol on UART5 (PC12/PD2)
+// NOTE: Spec requested USART1 @ 115200; USART1 is occupied by JLink @ 921600.
+#define UART_PROT_BAUD          115200u  // baud rate for UART5 Jetson protocol
+#define UART_PROT_HB_TIMEOUT_MS   500u  // heartbeat timeout: Jetson considered lost after 500 ms
+
 #endif // CONFIG_H

include/uart_protocol.h

@@ -0,0 +1,96 @@
+#ifndef UART_PROTOCOL_H
+#define UART_PROTOCOL_H
+
+/*
+ * uart_protocol.h — UART command protocol for Jetson-STM32 communication (Issue #629)
+ *
+ * Frame format:
+ *   [STX][LEN][CMD][PAYLOAD...][CRC8][ETX]
+ *    0x02  1B   1B   0-12 B     1B   0x03
+ *
+ * CRC8-SMBUS: poly=0x07, init=0x00, computed over CMD+PAYLOAD bytes.
+ *
+ * Physical layer: UART5 (PC12=TX / PD2=RX), GPIO_AF8_UART5, 115200 baud, no hw flow.
+ * NOTE: Spec requested USART1 @ 115200, but USART1 is occupied by JLink @ 921600.
+ *       Implemented on UART5 instead; Jetson must connect to PC12/PD2.
+ *
+ * DMA: DMA1_Stream0_Channel4, circular 256-byte ring buffer.
+ * Heartbeat: if no frame received in UART_PROT_HB_TIMEOUT_MS (500 ms), Jetson is
+ *            considered lost; caller must handle estop if needed.
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/* ── Frame delimiters ─────────────────────────────────────────────────────── */
+#define UPROT_STX  0x02u
+#define UPROT_ETX  0x03u
+
+/* ── Command IDs (host → STM32) ───────────────────────────────────────────── */
+#define UCMD_SET_VELOCITY  0x01u  /* payload: int16 left_rpm, int16 right_rpm (4 B) */
+#define UCMD_GET_STATUS    0x02u  /* payload: none */
+#define UCMD_SET_PID       0x03u  /* payload: float kp, float ki, float kd    (12 B) */
+#define UCMD_ESTOP         0x04u  /* payload: none */
+#define UCMD_CLEAR_ESTOP   0x05u  /* payload: none */
+
+/* ── Response IDs (STM32 → host) ──────────────────────────────────────────── */
+#define URESP_ACK    0x80u  /* payload: 1 B — echoed CMD */
+#define URESP_NACK   0x81u  /* payload: 2 B — CMD, error_code */
+#define URESP_STATUS 0x82u  /* payload: sizeof(uart_prot_status_t) = 8 B */
+
+/* ── NACK error codes ─────────────────────────────────────────────────────── */
+#define UERR_BAD_CRC    0x01u
+#define UERR_BAD_LEN    0x02u
+#define UERR_BAD_ETX    0x03u
+#define UERR_ESTOP      0x04u  /* command rejected — estop active */
+#define UERR_DISARMED   0x05u  /* velocity rejected — not armed */
+
+/* ── STATUS payload (URESP_STATUS, 8 bytes packed) ───────────────────────── */
+typedef struct __attribute__((packed)) {
+    int16_t  pitch_x10;      /* pitch angle ×10 deg (balance controller) */
+    int16_t  motor_cmd;      /* ESC motor command -1000..+1000 */
+    uint16_t vbat_mv;        /* battery voltage in mV */
+    uint8_t  balance_state;  /* BalanceState enum (0=DISARMED, 1=ARMED, …) */
+    uint8_t  estop_active;   /* non-zero if remote estop is latched */
+} uart_prot_status_t;
+
+/* ── Shared state (read by main.c) ────────────────────────────────────────── */
+typedef struct {
+    volatile uint8_t  vel_updated;      /* 1 when SET_VELOCITY received */
+    volatile int16_t  left_rpm;
+    volatile int16_t  right_rpm;
+
+    volatile uint8_t  pid_updated;      /* 1 when SET_PID received */
+    volatile float    pid_kp;
+    volatile float    pid_ki;
+    volatile float    pid_kd;
+
+    volatile uint8_t  estop_req;        /* 1 on UCMD_ESTOP */
+    volatile uint8_t  estop_clear_req;  /* 1 on UCMD_CLEAR_ESTOP */
+
+    volatile uint32_t last_rx_ms;       /* HAL_GetTick() of last valid frame */
+} UartProtState;
+
+extern UartProtState uart_prot_state;
+
+/* ── API ───────────────────────────────────────────────────────────────────── */
+
+/**
+ * uart_protocol_init() — configure UART5 + DMA, start circular receive.
+ * Must be called once during system init, before main loop.
+ */
+void uart_protocol_init(void);
+
+/**
+ * uart_protocol_process() — drain DMA ring buffer, parse frames, dispatch commands.
+ * Call once per main loop iteration (every ~1 ms).
+ */
+void uart_protocol_process(void);
+
+/**
+ * uart_protocol_send_status() — build and TX a URESP_STATUS frame.
+ * @param s  Pointer to status payload to send.
+ */
+void uart_protocol_send_status(const uart_prot_status_t *s);
+
+#endif /* UART_PROTOCOL_H */

jetson/ros2_ws/src/saltybot_aruco_detect/config/aruco_detect_params.yaml

@@ -0,0 +1,34 @@
+aruco_detect:
+  ros__parameters:
+    # ── Marker specification ─────────────────────────────────────────────────
+    marker_size_m:      0.10      # physical side length of printed ArUco markers (m)
+                                  # measure the black border edge-to-edge
+
+    # ── Dock target filter ───────────────────────────────────────────────────
+    # dock_marker_ids: IDs that may serve as dock target.
+    #   - Empty list [] → any detected marker is a dock candidate (closest wins)
+    #   - Non-empty     → only listed IDs are candidates; others still appear in
+    #                     /saltybot/aruco/markers but not in dock_target
+    # The saltybot dock uses marker ID 42 on the charging face by convention.
+    dock_marker_ids:    [42]
+
+    # Maximum distance to consider a marker as dock target (m)
+    max_dock_range_m:   3.0
+
+    # ── ArUco dictionary ─────────────────────────────────────────────────────
+    aruco_dict:         DICT_4X4_50          # cv2.aruco constant name
+
+    # Corner sub-pixel refinement (improves pose accuracy at close range)
+    #   CORNER_REFINE_NONE     — fastest, no refinement
+    #   CORNER_REFINE_SUBPIX   — best for high-contrast markers (recommended)
+    #   CORNER_REFINE_CONTOUR  — good for blurry or low-res images
+    corner_refinement:  CORNER_REFINE_SUBPIX
+
+    # ── Frame / topic config ─────────────────────────────────────────────────
+    camera_frame:       camera_color_optical_frame
+
+    # ── Debug image output (disabled by default) ─────────────────────────────
+    # When true, publishes annotated BGR image with markers + axes drawn.
+    # Useful for tuning but costs ~10 ms/frame encoding overhead.
+    draw_debug_image:   false
+    debug_image_topic:  /saltybot/aruco/debug_image

jetson/ros2_ws/src/saltybot_aruco_detect/launch/aruco_detect.launch.py

@@ -0,0 +1,29 @@
+"""aruco_detect.launch.py — ArUco marker detection for docking (Issue #627)."""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('saltybot_aruco_detect')
+    default_params = os.path.join(pkg_share, 'config', 'aruco_detect_params.yaml')
+
+    params_arg = DeclareLaunchArgument(
+        'params_file',
+        default_value=default_params,
+        description='Path to aruco_detect_params.yaml',
+    )
+
+    aruco_node = Node(
+        package='saltybot_aruco_detect',
+        executable='aruco_detect',
+        name='aruco_detect',
+        output='screen',
+        parameters=[LaunchConfiguration('params_file')],
+    )
+
+    return LaunchDescription([params_arg, aruco_node])

jetson/ros2_ws/src/saltybot_aruco_detect/package.xml

@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_aruco_detect</name>
+  <version>0.1.0</version>
+  <description>
+    ArUco marker detection for docking alignment (Issue #627).
+    Detects all DICT_4X4_50 markers from RealSense D435i RGB, estimates 6-DOF
+    pose with estimatePoseSingleMarkers, publishes PoseArray on
+    /saltybot/aruco/markers and closest dock-candidate marker on
+    /saltybot/aruco/dock_target (PoseStamped) with RViz MarkerArray and
+    JSON status.
+  </description>
+  <maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>cv_bridge</depend>
+
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-opencv</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_aruco_detect/saltybot_aruco_detect/aruco_detect_node.py

@@ -0,0 +1,518 @@
+"""
+aruco_detect_node.py — ArUco marker detection for docking alignment (Issue #627).
+
+Detects all DICT_4X4_50 ArUco markers visible in the RealSense D435i RGB stream,
+estimates their 6-DOF poses relative to the camera using estimatePoseSingleMarkers,
+and publishes:
+
+  /saltybot/aruco/markers      geometry_msgs/PoseArray        — all detected markers
+  /saltybot/aruco/dock_target  geometry_msgs/PoseStamped      — closest dock marker
+  /saltybot/aruco/viz          visualization_msgs/MarkerArray — RViz axes overlays
+  /saltybot/aruco/status       std_msgs/String                — JSON summary (10 Hz)
+
+Coordinate frame
+────────────────
+All poses are expressed in camera_color_optical_frame (ROS optical convention):
+  +X = right, +Y = down, +Z = forward (into scene)
+
+The dock_target pose gives the docking controller everything it needs:
+  position.z  — forward distance to dock (m)      → throttle target
+  position.x  — lateral offset (m)                → steer target
+  orientation — marker orientation for final align → yaw servo
+
+Dock target selection
+─────────────────────
+  dock_marker_ids   list[int]  []   — empty = accept any detected marker
+                                       non-empty = only these IDs are candidates
+  closest_wins      bool       true — among candidates, prefer the nearest
+  max_dock_range_m  float      3.0  — ignore markers beyond this distance
+
+estimatePoseSingleMarkers API
+──────────────────────────────
+  Uses cv2.aruco.estimatePoseSingleMarkers (legacy OpenCV API still present in
+  4.x with a deprecation notice).  Falls back to per-marker cv2.solvePnP with
+  SOLVEPNP_IPPE_SQUARE if the legacy function is unavailable.
+  Both paths use the same camera_matrix / dist_coeffs from CameraInfo.
+
+Subscribes
+──────────
+  /camera/color/image_raw       sensor_msgs/Image      30 Hz (BGR8)
+  /camera/color/camera_info     sensor_msgs/CameraInfo (latched, once)
+
+Publishes
+─────────
+  /saltybot/aruco/markers       geometry_msgs/PoseArray
+  /saltybot/aruco/dock_target   geometry_msgs/PoseStamped
+  /saltybot/aruco/viz           visualization_msgs/MarkerArray
+  /saltybot/aruco/status        std_msgs/String   (JSON, 10 Hz)
+
+Parameters (see config/aruco_detect_params.yaml)
+────────────────────────────────────────────────
+  marker_size_m      float   0.10     physical side length of printed markers (m)
+  dock_marker_ids    int[]   []       IDs to accept as dock targets (empty = all)
+  max_dock_range_m   float   3.0      ignore candidates beyond this (m)
+  aruco_dict         str     DICT_4X4_50
+  corner_refinement  str     CORNER_REFINE_SUBPIX
+  camera_frame       str     camera_color_optical_frame
+  draw_debug_image   bool    false    publish annotated image for debugging
+  debug_image_topic  str     /saltybot/aruco/debug_image
+"""
+
+from __future__ import annotations
+
+import json
+import math
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import (
+    QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+)
+
+import numpy as np
+import cv2
+from cv_bridge import CvBridge
+
+from geometry_msgs.msg import (
+    Pose, PoseArray, PoseStamped, Point, Quaternion, Vector3, TransformStamped
+)
+from sensor_msgs.msg import Image, CameraInfo
+from std_msgs.msg import Header, String, ColorRGBA
+from visualization_msgs.msg import Marker, MarkerArray
+
+from .aruco_math import MarkerPose, rvec_to_quat, tvec_distance
+
+
+# ── QoS ───────────────────────────────────────────────────────────────────────
+_SENSOR_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=5,
+)
+_LATCHED_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.RELIABLE,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+    durability=DurabilityPolicy.TRANSIENT_LOCAL,
+)
+
+# ArUco axis length for viz (fraction of marker size)
+_AXIS_LEN_FRAC = 0.5
+
+
+class ArucoDetectNode(Node):
+    """ArUco marker detection + pose estimation node — see module docstring."""
+
+    def __init__(self) -> None:
+        super().__init__('aruco_detect')
+
+        # ── Parameters ────────────────────────────────────────────────────────
+        self.declare_parameter('marker_size_m',     0.10)
+        self.declare_parameter('dock_marker_ids',   [])       # empty list = all
+        self.declare_parameter('max_dock_range_m',  3.0)
+        self.declare_parameter('aruco_dict',        'DICT_4X4_50')
+        self.declare_parameter('corner_refinement', 'CORNER_REFINE_SUBPIX')
+        self.declare_parameter('camera_frame',      'camera_color_optical_frame')
+        self.declare_parameter('draw_debug_image',  False)
+        self.declare_parameter('debug_image_topic', '/saltybot/aruco/debug_image')
+
+        self._marker_size  = self.get_parameter('marker_size_m').value
+        self._dock_ids     = set(self.get_parameter('dock_marker_ids').value)
+        self._max_range    = self.get_parameter('max_dock_range_m').value
+        self._cam_frame    = self.get_parameter('camera_frame').value
+        self._draw_debug   = self.get_parameter('draw_debug_image').value
+        self._debug_topic  = self.get_parameter('debug_image_topic').value
+
+        aruco_dict_name    = self.get_parameter('aruco_dict').value
+        refine_name        = self.get_parameter('corner_refinement').value
+
+        # ── ArUco detector setup ──────────────────────────────────────────────
+        dict_id    = getattr(cv2.aruco, aruco_dict_name, cv2.aruco.DICT_4X4_50)
+        params     = cv2.aruco.DetectorParameters()
+        # Apply corner refinement
+        refine_id  = getattr(cv2.aruco, refine_name, cv2.aruco.CORNER_REFINE_SUBPIX)
+        params.cornerRefinementMethod = refine_id
+
+        aruco_dict     = cv2.aruco.getPredefinedDictionary(dict_id)
+        self._detector = cv2.aruco.ArucoDetector(aruco_dict, params)
+
+        # Object points for solvePnP fallback (counter-clockwise from top-left)
+        half = self._marker_size / 2.0
+        self._obj_pts = np.array([
+            [-half,  half, 0.0],
+            [ half,  half, 0.0],
+            [ half, -half, 0.0],
+            [-half, -half, 0.0],
+        ], dtype=np.float64)
+
+        # ── Camera intrinsics (set on first CameraInfo) ───────────────────────
+        self._K:    np.ndarray | None = None
+        self._dist: np.ndarray | None = None
+        self._bridge = CvBridge()
+
+        # ── Publishers ─────────────────────────────────────────────────────────
+        self._pose_array_pub = self.create_publisher(
+            PoseArray, '/saltybot/aruco/markers', 10
+        )
+        self._dock_pub = self.create_publisher(
+            PoseStamped, '/saltybot/aruco/dock_target', 10
+        )
+        self._viz_pub = self.create_publisher(
+            MarkerArray, '/saltybot/aruco/viz', 10
+        )
+        self._status_pub = self.create_publisher(
+            String, '/saltybot/aruco/status', 10
+        )
+        if self._draw_debug:
+            self._debug_pub = self.create_publisher(
+                Image, self._debug_topic, 5
+            )
+        else:
+            self._debug_pub = None
+
+        # ── Subscriptions ──────────────────────────────────────────────────────
+        self.create_subscription(
+            CameraInfo,
+            '/camera/color/camera_info',
+            self._on_camera_info,
+            _LATCHED_QOS,
+        )
+        self.create_subscription(
+            Image,
+            '/camera/color/image_raw',
+            self._on_image,
+            _SENSOR_QOS,
+        )
+
+        # ── Status timer (10 Hz) ────────────────────────────────────────────────
+        self._last_detections: list[MarkerPose] = []
+        self.create_timer(0.1, self._on_status_timer)
+
+        self.get_logger().info(
+            f'aruco_detect ready — '
+            f'dict={aruco_dict_name} '
+            f'marker_size={self._marker_size * 100:.0f}cm '
+            f'dock_ids={list(self._dock_ids) if self._dock_ids else "any"} '
+            f'max_range={self._max_range}m'
+        )
+
+    # ── Camera info ───────────────────────────────────────────────────────────
+
+    def _on_camera_info(self, msg: CameraInfo) -> None:
+        if self._K is not None:
+            return
+        self._K    = np.array(msg.k, dtype=np.float64).reshape(3, 3)
+        self._dist = np.array(msg.d, dtype=np.float64).reshape(1, -1)
+        self.get_logger().info(
+            f'camera_info received — '
+            f'fx={self._K[0,0]:.1f} fy={self._K[1,1]:.1f}'
+        )
+
+    # ── Image callback ────────────────────────────────────────────────────────
+
+    def _on_image(self, msg: Image) -> None:
+        if self._K is None:
+            return
+
+        # ── Decode ────────────────────────────────────────────────────────────
+        try:
+            bgr = self._bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+        except Exception as exc:
+            self.get_logger().warning(f'image decode error: {exc}',
+                                      throttle_duration_sec=5.0)
+            return
+
+        # Convert to greyscale for detection (faster, same accuracy)
+        gray = cv2.cvtColor(bgr, cv2.COLOR_BGR2GRAY)
+
+        stamp  = msg.header.stamp
+
+        # ── Detect markers ────────────────────────────────────────────────────
+        corners, ids, _ = self._detector.detectMarkers(gray)
+
+        if ids is None or len(ids) == 0:
+            self._last_detections = []
+            self._publish_empty(stamp)
+            return
+
+        ids_flat = ids.flatten().tolist()
+
+        # ── Pose estimation ───────────────────────────────────────────────────
+        detections = self._estimate_poses(corners, ids_flat)
+
+        # Filter by range
+        detections = [d for d in detections if d.distance_m <= self._max_range]
+
+        self._last_detections = detections
+
+        # ── Publish ───────────────────────────────────────────────────────────
+        self._publish_pose_array(detections, stamp)
+        self._publish_dock_target(detections, stamp)
+        self._publish_viz(detections, stamp)
+
+        # Debug image
+        if self._debug_pub is not None:
+            self._publish_debug(bgr, corners, ids, detections, stamp)
+
+    # ── Pose estimation ───────────────────────────────────────────────────────
+
+    def _estimate_poses(
+        self,
+        corners: list,
+        ids:     list[int],
+    ) -> list[MarkerPose]:
+        """
+        Estimate 6-DOF pose for each detected marker.
+
+        Uses estimatePoseSingleMarkers (legacy API, still present in cv2 4.x).
+        Falls back to per-marker solvePnP(IPPE_SQUARE) if unavailable.
+        """
+        results: list[MarkerPose] = []
+
+        # ── Try legacy estimatePoseSingleMarkers ──────────────────────────────
+        if hasattr(cv2.aruco, 'estimatePoseSingleMarkers'):
+            try:
+                rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
+                    corners, self._marker_size, self._K, self._dist
+                )
+                for i, mid in enumerate(ids):
+                    rvec = rvecs[i].ravel().astype(np.float64)
+                    tvec = tvecs[i].ravel().astype(np.float64)
+                    results.append(MarkerPose(
+                        marker_id = int(mid),
+                        tvec      = tvec,
+                        rvec      = rvec,
+                        corners   = corners[i].reshape(4, 2).astype(np.float64),
+                    ))
+                return results
+            except Exception as exc:
+                self.get_logger().debug(
+                    f'estimatePoseSingleMarkers failed ({exc}), '
+                    'falling back to solvePnP'
+                )
+
+        # ── Fallback: per-marker solvePnP ─────────────────────────────────────
+        for i, mid in enumerate(ids):
+            img_pts = corners[i].reshape(4, 2).astype(np.float64)
+            ok, rvec, tvec = cv2.solvePnP(
+                self._obj_pts, img_pts,
+                self._K, self._dist,
+                flags=cv2.SOLVEPNP_IPPE_SQUARE,
+            )
+            if not ok:
+                continue
+            results.append(MarkerPose(
+                marker_id = int(mid),
+                tvec      = tvec.ravel().astype(np.float64),
+                rvec      = rvec.ravel().astype(np.float64),
+                corners   = img_pts,
+            ))
+        return results
+
+    # ── Dock target selection ─────────────────────────────────────────────────
+
+    def _select_dock_target(
+        self,
+        detections: list[MarkerPose],
+    ) -> MarkerPose | None:
+        """Select the closest marker that matches dock_marker_ids filter."""
+        candidates = [
+            d for d in detections
+            if not self._dock_ids or d.marker_id in self._dock_ids
+        ]
+        if not candidates:
+            return None
+        return min(candidates, key=lambda d: d.distance_m)
+
+    # ── Publishers ────────────────────────────────────────────────────────────
+
+    def _publish_pose_array(
+        self,
+        detections: list[MarkerPose],
+        stamp,
+    ) -> None:
+        pa              = PoseArray()
+        pa.header.stamp    = stamp
+        pa.header.frame_id = self._cam_frame
+
+        for d in detections:
+            qx, qy, qz, qw = d.quat
+            pose               = Pose()
+            pose.position      = Point(x=float(d.tvec[0]),
+                                       y=float(d.tvec[1]),
+                                       z=float(d.tvec[2]))
+            pose.orientation   = Quaternion(x=qx, y=qy, z=qz, w=qw)
+            pa.poses.append(pose)
+
+        self._pose_array_pub.publish(pa)
+
+    def _publish_dock_target(
+        self,
+        detections: list[MarkerPose],
+        stamp,
+    ) -> None:
+        target = self._select_dock_target(detections)
+        if target is None:
+            return
+
+        qx, qy, qz, qw = target.quat
+        ps                  = PoseStamped()
+        ps.header.stamp     = stamp
+        ps.header.frame_id  = self._cam_frame
+        ps.pose.position    = Point(x=float(target.tvec[0]),
+                                    y=float(target.tvec[1]),
+                                    z=float(target.tvec[2]))
+        ps.pose.orientation = Quaternion(x=qx, y=qy, z=qz, w=qw)
+        self._dock_pub.publish(ps)
+
+    def _publish_viz(
+        self,
+        detections: list[MarkerPose],
+        stamp,
+    ) -> None:
+        """Publish coordinate-axes MarkerArray for each detected marker."""
+        ma      = MarkerArray()
+        lifetime = _ros_duration(0.2)
+
+        # Delete-all
+        dm              = Marker()
+        dm.action       = Marker.DELETEALL
+        dm.header.stamp    = stamp
+        dm.header.frame_id = self._cam_frame
+        ma.markers.append(dm)
+
+        target = self._select_dock_target(detections)
+        axis_len = self._marker_size * _AXIS_LEN_FRAC
+
+        for idx, d in enumerate(detections):
+            is_target = (target is not None and d.marker_id == target.marker_id)
+            cx, cy, cz = float(d.tvec[0]), float(d.tvec[1]), float(d.tvec[2])
+            qx, qy, qz, qw = d.quat
+            base_id = idx * 10
+
+            # Sphere at marker centre
+            sph                       = Marker()
+            sph.header.stamp          = stamp
+            sph.header.frame_id       = self._cam_frame
+            sph.ns                    = 'aruco_centers'
+            sph.id                    = base_id
+            sph.type                  = Marker.SPHERE
+            sph.action                = Marker.ADD
+            sph.pose.position         = Point(x=cx, y=cy, z=cz)
+            sph.pose.orientation      = Quaternion(x=qx, y=qy, z=qz, w=qw)
+            r_s = 0.015 if not is_target else 0.025
+            sph.scale                 = Vector3(x=r_s * 2, y=r_s * 2, z=r_s * 2)
+            sph.color = (
+                ColorRGBA(r=1.0, g=0.2, b=0.2, a=1.0) if is_target else
+                ColorRGBA(r=0.2, g=0.8, b=1.0, a=0.9)
+            )
+            sph.lifetime              = lifetime
+            ma.markers.append(sph)
+
+            # Text label: ID + distance
+            txt                       = Marker()
+            txt.header.stamp          = stamp
+            txt.header.frame_id       = self._cam_frame
+            txt.ns                    = 'aruco_labels'
+            txt.id                    = base_id + 1
+            txt.type                  = Marker.TEXT_VIEW_FACING
+            txt.action                = Marker.ADD
+            txt.pose.position         = Point(x=cx, y=cy - 0.08, z=cz)
+            txt.pose.orientation.w    = 1.0
+            txt.scale.z               = 0.06
+            txt.color                 = ColorRGBA(r=1.0, g=1.0, b=0.0, a=1.0)
+            txt.text = f'ID={d.marker_id}\n{d.distance_m:.2f}m'
+            if is_target:
+                txt.text += '\n[DOCK]'
+            txt.lifetime              = lifetime
+            ma.markers.append(txt)
+
+        self._viz_pub.publish(ma)
+
+    def _publish_empty(self, stamp) -> None:
+        pa              = PoseArray()
+        pa.header.stamp    = stamp
+        pa.header.frame_id = self._cam_frame
+        self._pose_array_pub.publish(pa)
+        self._publish_viz([], stamp)
+
+    def _publish_debug(self, bgr, corners, ids, detections, stamp) -> None:
+        """Annotate and publish debug image with detected markers drawn."""
+        debug = bgr.copy()
+        if ids is not None and len(ids) > 0:
+            cv2.aruco.drawDetectedMarkers(debug, corners, ids)
+
+        # Draw axes for each detection using the estimated poses
+        for d in detections:
+            try:
+                cv2.drawFrameAxes(
+                    debug, self._K, self._dist,
+                    d.rvec.reshape(3, 1),
+                    d.tvec.reshape(3, 1),
+                    self._marker_size * _AXIS_LEN_FRAC,
+                )
+            except Exception:
+                pass
+
+        try:
+            img_msg = self._bridge.cv2_to_imgmsg(debug, encoding='bgr8')
+            img_msg.header.stamp    = stamp
+            img_msg.header.frame_id = self._cam_frame
+            self._debug_pub.publish(img_msg)
+        except Exception:
+            pass
+
+    # ── Status timer ─────────────────────────────────────────────────────────
+
+    def _on_status_timer(self) -> None:
+        detections = self._last_detections
+        target     = self._select_dock_target(detections)
+
+        markers_info = [
+            {
+                'id':         d.marker_id,
+                'distance_m': round(d.distance_m,  3),
+                'yaw_deg':    round(math.degrees(d.yaw_rad), 2),
+                'lateral_m':  round(d.lateral_m,   3),
+                'forward_m':  round(d.forward_m,   3),
+                'is_target':  target is not None and d.marker_id == target.marker_id,
+            }
+            for d in detections
+        ]
+
+        status = {
+            'detected_count':  len(detections),
+            'dock_target_id':  target.marker_id  if target else None,
+            'dock_distance_m': round(target.distance_m, 3) if target else None,
+            'dock_yaw_deg':    round(math.degrees(target.yaw_rad), 2) if target else None,
+            'dock_lateral_m':  round(target.lateral_m, 3) if target else None,
+            'markers':         markers_info,
+        }
+        msg      = String()
+        msg.data = json.dumps(status)
+        self._status_pub.publish(msg)
+
+
+# ── Helpers ───────────────────────────────────────────────────────────────────
+
+def _ros_duration(seconds: float):
+    from rclpy.duration import Duration
+    sec  = int(seconds)
+    nsec = int((seconds - sec) * 1e9)
+    return Duration(seconds=sec, nanoseconds=nsec).to_msg()
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = ArucoDetectNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_aruco_detect/saltybot_aruco_detect/aruco_math.py

@@ -0,0 +1,156 @@
+"""
+aruco_math.py — Pure-math helpers for ArUco detection (Issue #627).
+
+No ROS2 dependencies — importable in unit tests without a ROS2 install.
+
+Provides:
+  rot_mat_to_quat(R)         → (qx, qy, qz, qw)
+  rvec_to_quat(rvec)         → (qx, qy, qz, qw)   [via cv2.Rodrigues]
+  tvec_distance(tvec)        → float   (Euclidean norm)
+  tvec_yaw_rad(tvec)         → float   (atan2(tx, tz) — lateral bearing error)
+  MarkerPose dataclass       — per-marker detection result
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field
+from typing import List, Optional, Tuple
+
+import numpy as np
+
+
+# ── Rotation helpers ──────────────────────────────────────────────────────────
+
+def rot_mat_to_quat(R: np.ndarray) -> Tuple[float, float, float, float]:
+    """
+    Convert a 3×3 rotation matrix to quaternion (qx, qy, qz, qw).
+
+    Uses Shepperd's method for numerical stability.
+    R must be a valid rotation matrix (‖R‖₂ = 1, det R = +1).
+    """
+    R = np.asarray(R, dtype=np.float64)
+    trace = R[0, 0] + R[1, 1] + R[2, 2]
+
+    if trace > 0.0:
+        s = 0.5 / math.sqrt(trace + 1.0)
+        w = 0.25 / s
+        x = (R[2, 1] - R[1, 2]) * s
+        y = (R[0, 2] - R[2, 0]) * s
+        z = (R[1, 0] - R[0, 1]) * s
+    elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
+        s = 2.0 * math.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2])
+        w = (R[2, 1] - R[1, 2]) / s
+        x = 0.25 * s
+        y = (R[0, 1] + R[1, 0]) / s
+        z = (R[0, 2] + R[2, 0]) / s
+    elif R[1, 1] > R[2, 2]:
+        s = 2.0 * math.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2])
+        w = (R[0, 2] - R[2, 0]) / s
+        x = (R[0, 1] + R[1, 0]) / s
+        y = 0.25 * s
+        z = (R[1, 2] + R[2, 1]) / s
+    else:
+        s = 2.0 * math.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1])
+        w = (R[1, 0] - R[0, 1]) / s
+        x = (R[0, 2] + R[2, 0]) / s
+        y = (R[1, 2] + R[2, 1]) / s
+        z = 0.25 * s
+
+    # Normalise
+    norm = math.sqrt(x * x + y * y + z * z + w * w)
+    if norm < 1e-10:
+        return 0.0, 0.0, 0.0, 1.0
+    inv = 1.0 / norm
+    return x * inv, y * inv, z * inv, w * inv
+
+
+def rvec_to_quat(rvec: np.ndarray) -> Tuple[float, float, float, float]:
+    """
+    Convert a Rodrigues rotation vector (3,) or (1,3) to quaternion.
+
+    Requires cv2 for cv2.Rodrigues(); falls back to manual exponential map
+    if cv2 is unavailable.
+    """
+    vec = np.asarray(rvec, dtype=np.float64).ravel()
+    try:
+        import cv2
+        R, _ = cv2.Rodrigues(vec)
+    except ImportError:
+        # Manual Rodrigues exponential map
+        angle = float(np.linalg.norm(vec))
+        if angle < 1e-12:
+            return 0.0, 0.0, 0.0, 1.0
+        axis = vec / angle
+        K = np.array([
+            [     0.0, -axis[2],  axis[1]],
+            [ axis[2],      0.0, -axis[0]],
+            [-axis[1],  axis[0],      0.0],
+        ])
+        R = np.eye(3) + math.sin(angle) * K + (1.0 - math.cos(angle)) * K @ K
+
+    return rot_mat_to_quat(R)
+
+
+# ── Metric helpers ────────────────────────────────────────────────────────────
+
+def tvec_distance(tvec: np.ndarray) -> float:
+    """Euclidean distance from camera to marker (m)."""
+    t = np.asarray(tvec, dtype=np.float64).ravel()
+    return float(math.sqrt(t[0] ** 2 + t[1] ** 2 + t[2] ** 2))
+
+
+def tvec_yaw_rad(tvec: np.ndarray) -> float:
+    """
+    Horizontal bearing error to marker (rad).
+
+    atan2(tx, tz): +ve when marker is to the right of the camera optical axis.
+    Zero when the marker is directly in front.
+    """
+    t = np.asarray(tvec, dtype=np.float64).ravel()
+    return float(math.atan2(t[0], t[2]))
+
+
+# ── Per-marker result ─────────────────────────────────────────────────────────
+
+@dataclass
+class MarkerPose:
+    """Pose result for a single detected ArUco marker."""
+    marker_id:  int
+    tvec:       np.ndarray    # (3,) translation in camera optical frame (m)
+    rvec:       np.ndarray    # (3,) Rodrigues rotation vector
+    corners:    np.ndarray    # (4, 2) image-plane corner coordinates (px)
+
+    # Derived (computed lazily)
+    _distance_m: Optional[float] = field(default=None, repr=False)
+    _yaw_rad:    Optional[float] = field(default=None, repr=False)
+    _quat:       Optional[Tuple[float, float, float, float]] = field(default=None, repr=False)
+
+    @property
+    def distance_m(self) -> float:
+        if self._distance_m is None:
+            self._distance_m = tvec_distance(self.tvec)
+        return self._distance_m
+
+    @property
+    def yaw_rad(self) -> float:
+        if self._yaw_rad is None:
+            self._yaw_rad = tvec_yaw_rad(self.tvec)
+        return self._yaw_rad
+
+    @property
+    def lateral_m(self) -> float:
+        """Lateral offset (m); +ve = marker is to the right."""
+        return float(self.tvec[0])
+
+    @property
+    def forward_m(self) -> float:
+        """Forward distance (Z component in camera frame, m)."""
+        return float(self.tvec[2])
+
+    @property
+    def quat(self) -> Tuple[float, float, float, float]:
+        """Quaternion (qx, qy, qz, qw) from rvec."""
+        if self._quat is None:
+            self._quat = rvec_to_quat(self.rvec)
+        return self._quat

jetson/ros2_ws/src/saltybot_aruco_detect/setup.cfg

@@ -0,0 +1,5 @@
+[develop]
+script_dir=$base/lib/saltybot_aruco_detect
+
+[install]
+install_scripts=$base/lib/saltybot_aruco_detect

jetson/ros2_ws/src/saltybot_aruco_detect/setup.py

@@ -0,0 +1,30 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = 'saltybot_aruco_detect'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='sl-perception',
+    maintainer_email='sl-perception@saltylab.local',
+    description='ArUco marker detection for docking alignment (Issue #627)',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'aruco_detect = saltybot_aruco_detect.aruco_detect_node:main',
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_aruco_detect/test/test_aruco_math.py

@@ -0,0 +1,206 @@
+"""Unit tests for aruco_math.py — no ROS2, no live camera required."""
+
+import math
+import numpy as np
+import pytest
+
+from saltybot_aruco_detect.aruco_math import (
+    rot_mat_to_quat,
+    rvec_to_quat,
+    tvec_distance,
+    tvec_yaw_rad,
+    MarkerPose,
+)
+
+
+# ── Helpers ───────────────────────────────────────────────────────────────────
+
+def _quat_norm(q):
+    return math.sqrt(sum(x * x for x in q))
+
+
+def _quat_rotate(q, v):
+    """Rotate vector v by quaternion q (qx, qy, qz, qw)."""
+    qx, qy, qz, qw = q
+    # Quaternion product: q * (0,v) * q^-1
+    # Using formula: v' = v + 2qw(q × v) + 2(q × (q × v))
+    qvec = np.array([qx, qy, qz])
+    t    = 2.0 * np.cross(qvec, v)
+    return v + qw * t + np.cross(qvec, t)
+
+
+# ── rot_mat_to_quat ───────────────────────────────────────────────────────────
+
+def test_identity_rotation():
+    R = np.eye(3)
+    q = rot_mat_to_quat(R)
+    assert abs(q[3] - 1.0) < 1e-9   # w = 1 for identity
+    assert abs(q[0]) < 1e-9
+    assert abs(q[1]) < 1e-9
+    assert abs(q[2]) < 1e-9
+
+
+def test_90deg_yaw():
+    """90° rotation about Z axis."""
+    angle = math.pi / 2
+    R = np.array([
+        [math.cos(angle), -math.sin(angle), 0],
+        [math.sin(angle),  math.cos(angle), 0],
+        [0,                0,               1],
+    ])
+    qx, qy, qz, qw = rot_mat_to_quat(R)
+    # For 90° Z rotation: q = (0, 0, sin45°, cos45°)
+    assert abs(qx) < 1e-6
+    assert abs(qy) < 1e-6
+    assert abs(qz - math.sin(angle / 2)) < 1e-6
+    assert abs(qw - math.cos(angle / 2)) < 1e-6
+
+
+def test_unit_norm():
+    for yaw in [0, 0.5, 1.0, 2.0, math.pi]:
+        R = np.array([
+            [math.cos(yaw), -math.sin(yaw), 0],
+            [math.sin(yaw),  math.cos(yaw), 0],
+            [0, 0, 1],
+        ])
+        q = rot_mat_to_quat(R)
+        assert abs(_quat_norm(q) - 1.0) < 1e-9, f"yaw={yaw}: norm={_quat_norm(q)}"
+
+
+def test_roundtrip_rotation_matrix():
+    """Rotating a vector with the matrix and quaternion should give same result."""
+    angle = 1.23
+    R = np.array([
+        [math.cos(angle), -math.sin(angle), 0],
+        [math.sin(angle),  math.cos(angle), 0],
+        [0, 0, 1],
+    ])
+    q = rot_mat_to_quat(R)
+    v = np.array([1.0, 0.0, 0.0])
+    v_R = R @ v
+    v_q = _quat_rotate(q, v)
+    np.testing.assert_allclose(v_R, v_q, atol=1e-9)
+
+
+def test_180deg_pitch():
+    """180° rotation about Y axis."""
+    R = np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]], dtype=float)
+    q = rot_mat_to_quat(R)
+    assert abs(_quat_norm(q) - 1.0) < 1e-9
+    # For 180° Y: q = (0, 1, 0, 0) or (0, -1, 0, 0)
+    assert abs(abs(q[1]) - 1.0) < 1e-6
+
+
+# ── rvec_to_quat ──────────────────────────────────────────────────────────────
+
+def test_rvec_zero_is_identity():
+    q = rvec_to_quat(np.array([0.0, 0.0, 0.0]))
+    assert abs(_quat_norm(q) - 1.0) < 1e-6
+    assert abs(q[3] - 1.0) < 1e-6   # w = 1
+
+
+def test_rvec_unit_norm():
+    for rvec in [
+        [0.1, 0.0, 0.0],
+        [0.0, 0.5, 0.0],
+        [0.0, 0.0, 1.57],
+        [0.3, 0.4, 0.5],
+    ]:
+        q = rvec_to_quat(np.array(rvec))
+        assert abs(_quat_norm(q) - 1.0) < 1e-9, f"rvec={rvec}: norm={_quat_norm(q)}"
+
+
+def test_rvec_z_rotation_matches_rot_mat():
+    """rvec=[0,0,π/3] should match 60° Z rotation matrix quaternion."""
+    angle = math.pi / 3
+    rvec  = np.array([0.0, 0.0, angle])
+    R     = np.array([
+        [math.cos(angle), -math.sin(angle), 0],
+        [math.sin(angle),  math.cos(angle), 0],
+        [0, 0, 1],
+    ])
+    q_rvec = rvec_to_quat(rvec)
+    q_R    = rot_mat_to_quat(R)
+    # Allow sign flip (q and -q represent the same rotation)
+    diff = min(
+        abs(_quat_norm(np.array(q_rvec) - np.array(q_R))),
+        abs(_quat_norm(np.array(q_rvec) + np.array(q_R))),
+    )
+    assert diff < 1e-6, f"rvec q={q_rvec}, mat q={q_R}"
+
+
+# ── tvec_distance ─────────────────────────────────────────────────────────────
+
+def test_tvec_distance_basic():
+    assert abs(tvec_distance(np.array([3.0, 4.0, 0.0])) - 5.0) < 1e-9
+
+
+def test_tvec_distance_zero():
+    assert tvec_distance(np.array([0.0, 0.0, 0.0])) == 0.0
+
+
+def test_tvec_distance_forward():
+    assert abs(tvec_distance(np.array([0.0, 0.0, 2.5])) - 2.5) < 1e-9
+
+
+def test_tvec_distance_3d():
+    t = np.array([1.0, 2.0, 3.0])
+    expected = math.sqrt(14.0)
+    assert abs(tvec_distance(t) - expected) < 1e-9
+
+
+# ── tvec_yaw_rad ──────────────────────────────────────────────────────────────
+
+def test_yaw_zero_when_centred():
+    """Marker directly in front: tx=0 → yaw=0."""
+    assert abs(tvec_yaw_rad(np.array([0.0, 0.0, 2.0]))) < 1e-9
+
+
+def test_yaw_positive_right():
+    """Marker to the right (tx>0) → positive yaw."""
+    assert tvec_yaw_rad(np.array([1.0, 0.0, 2.0])) > 0.0
+
+
+def test_yaw_negative_left():
+    """Marker to the left (tx<0) → negative yaw."""
+    assert tvec_yaw_rad(np.array([-1.0, 0.0, 2.0])) < 0.0
+
+
+def test_yaw_45deg():
+    """tx=tz → 45°."""
+    assert abs(tvec_yaw_rad(np.array([1.0, 0.0, 1.0])) - math.pi / 4) < 1e-9
+
+
+# ── MarkerPose ────────────────────────────────────────────────────────────────
+
+def _make_marker(mid=42, tx=0.0, ty=0.0, tz=2.0):
+    rvec = np.array([0.0, 0.0, 0.1])
+    tvec = np.array([tx, ty, tz])
+    corners = np.zeros((4, 2))
+    return MarkerPose(marker_id=mid, tvec=tvec, rvec=rvec, corners=corners)
+
+
+def test_marker_distance_cached():
+    m = _make_marker(tz=3.0)
+    d1 = m.distance_m
+    d2 = m.distance_m
+    assert d1 == d2
+
+
+def test_marker_lateral_and_forward():
+    m = _make_marker(tx=0.5, tz=2.0)
+    assert abs(m.lateral_m  - 0.5) < 1e-9
+    assert abs(m.forward_m  - 2.0) < 1e-9
+
+
+def test_marker_quat_unit_norm():
+    m = _make_marker()
+    q = m.quat
+    assert abs(_quat_norm(q) - 1.0) < 1e-9
+
+
+def test_marker_yaw_sign():
+    m_right = _make_marker(tx=+0.3, tz=1.0)
+    m_left  = _make_marker(tx=-0.3, tz=1.0)
+    assert m_right.yaw_rad > 0
+    assert m_left.yaw_rad  < 0

jetson/ros2_ws/src/saltybot_system_monitor/config/system_monitor.yaml

@@ -0,0 +1,17 @@
+# system_monitor.yaml — Config for saltybot_system_monitor (Issue #631)
+system_monitor_node:
+  ros__parameters:
+    # Publishing rate in Hz
+    publish_rate_hz: 1.0
+
+    # Stats source: auto | jtop | tegrastats | mock
+    # auto = prefer jtop, fall back to tegrastats, then mock
+    stats_source: "auto"
+
+    # Alert thresholds — WARN at these values, ERROR at +10% (or +5% for disk/ram)
+    cpu_warn_pct:  85.0   # CPU utilisation  (%)
+    gpu_warn_pct:  85.0   # GPU utilisation  (%)
+    temp_warn_c:   80.0   # Any temperature sensor (°C)
+    disk_warn_pct: 90.0   # Disk usage       (%)
+    ram_warn_pct:  90.0   # RAM usage        (%)
+    power_warn_mw: 30000.0  # Total board power (mW)

jetson/ros2_ws/src/saltybot_system_monitor/launch/system_monitor.launch.py

@@ -0,0 +1,27 @@
+"""system_monitor.launch.py — Launch the Jetson system monitor node (Issue #631)."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description() -> LaunchDescription:
+    pkg_share = FindPackageShare("saltybot_system_monitor")
+
+    config_arg = DeclareLaunchArgument(
+        "config_file",
+        default_value=PathJoinSubstitution([pkg_share, "config", "system_monitor.yaml"]),
+        description="Path to the system monitor YAML config file",
+    )
+
+    node = Node(
+        package="saltybot_system_monitor",
+        executable="system_monitor_node",
+        name="system_monitor_node",
+        output="screen",
+        parameters=[LaunchConfiguration("config_file")],
+    )
+
+    return LaunchDescription([config_arg, node])

jetson/ros2_ws/src/saltybot_system_monitor/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>saltybot_system_monitor</name>
+  <version>0.1.0</version>
+  <description>
+    Jetson Orin system monitor for SaltyBot (Issue #631).
+    Publishes CPU/GPU/RAM/disk/temperature/fan/power stats at 1 Hz on
+    /saltybot/system/stats (JSON) and DiagnosticArray on /saltybot/diagnostics.
+    Alerts when CPU &gt; 85%, GPU &gt; 85%, temp &gt; 80°C, or disk &gt; 90%.
+    Reads hardware stats via jtop (jetson_stats) when available, falls back
+    to tegrastats subprocess parsing.
+  </description>
+  <maintainer email="sl-jetson@saltylab.local">sl-jetson</maintainer>
+  <license>Apache-2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <test_depend>pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_system_monitor/saltybot_system_monitor/jetson_stats.py

@@ -0,0 +1,558 @@
+"""jetson_stats.py — Jetson Orin hardware stats collection (Issue #631).
+
+No ROS2 dependencies — pure Python, fully unit-testable.
+
+Design
+──────
+JetsonStatsReader.read() returns a JetsonStats snapshot.  The reader
+tries three backends in priority order:
+
+  1. jtop  (jetson_stats pip package) — richest data, low overhead
+  2. tegrastats subprocess           — always present on JetPack, parseable
+  3. MockReader (tests / dev)        — deterministic values, no hardware
+
+AlertChecker.check(stats, thresholds) returns a list of Alert objects
+describing any threshold violations.  All thresholds are per-dataclass
+config so they're easy to unit-test.
+
+Tegrastats sample line (Jetson Orin NX, JetPack 6):
+    RAM 1234/7772MB (lfb 2x2MB) SWAP 0/3886MB (cached 0MB)
+    CPU [12%@1190,5%@729,...] EMC_FREQ 4%@2133 GPC_FREQ 4%@918
+    AO@35C CPU@39.6C Tboard@32C Tdiode@35.3C GPU@37.6C tj@41.3C
+    VDD_GPU_SOC 1866mW/1866mW VDD_CPU_CV 700mW/682mW VDD_IN 4002mW/3996mW
+"""
+
+from __future__ import annotations
+
+import re
+import shutil
+import subprocess
+import time
+from dataclasses import dataclass, field, asdict
+from typing import Dict, List, Optional, Tuple
+
+# psutil is available everywhere; used for disk stats fallback
+try:
+    import psutil as _psutil
+    HAS_PSUTIL = True
+except ImportError:
+    HAS_PSUTIL = False
+
+# jtop (jetson_stats) is Jetson-only
+try:
+    from jtop import jtop as _jtop
+    HAS_JTOP = True
+except ImportError:
+    HAS_JTOP = False
+
+
+# ── Data model ────────────────────────────────────────────────────────────────
+
+@dataclass
+class CpuCore:
+    """Stats for one CPU core."""
+    index:     int
+    usage_pct: float   # 0–100
+    freq_mhz:  int     = 0
+
+
+@dataclass
+class JetsonStats:
+    """Snapshot of all monitored Jetson Orin metrics."""
+
+    timestamp: float = field(default_factory=time.time)
+
+    # CPU
+    cpu_cores:   List[CpuCore] = field(default_factory=list)
+    cpu_avg_pct: float = 0.0   # mean across online cores
+
+    # GPU
+    gpu_pct:      float = 0.0
+    gpu_freq_mhz: int   = 0
+
+    # RAM
+    ram_used_mb:  int   = 0
+    ram_total_mb: int   = 0
+    ram_pct:      float = 0.0
+
+    # SWAP
+    swap_used_mb:  int = 0
+    swap_total_mb: int = 0
+
+    # Temperatures (name → °C)
+    temperatures: Dict[str, float] = field(default_factory=dict)
+    temp_max_c:   float = 0.0    # hottest sensor
+
+    # Fan
+    fan_pct: float = 0.0   # 0–100 (0 if unavailable)
+
+    # Power (mW)
+    power_total_mw: int = 0
+    power_cpu_mw:   int = 0
+    power_gpu_mw:   int = 0
+
+    # Disk (/, primary partition)
+    disk_used_gb:  float = 0.0
+    disk_total_gb: float = 0.0
+    disk_pct:      float = 0.0
+
+    # Source identification
+    source: str = "unknown"
+
+    # ── Derived helpers ────────────────────────────────────────────────────
+
+    def to_dict(self) -> dict:
+        """Return JSON-serialisable dict (cpu_cores as plain dicts)."""
+        d = asdict(self)
+        # dataclasses.asdict already recurses into nested dataclasses
+        return d
+
+    @property
+    def cpu_count(self) -> int:
+        return len(self.cpu_cores)
+
+    @property
+    def ram_free_mb(self) -> int:
+        return max(0, self.ram_total_mb - self.ram_used_mb)
+
+    @property
+    def disk_free_gb(self) -> float:
+        return max(0.0, self.disk_total_gb - self.disk_used_gb)
+
+
+# ── Alert model ───────────────────────────────────────────────────────────────
+
+@dataclass
+class AlertThresholds:
+    """Configurable alert thresholds for Jetson stats (Issue #631 spec)."""
+    cpu_pct:   float = 85.0    # alert when CPU average exceeds this
+    gpu_pct:   float = 85.0    # alert when GPU usage exceeds this
+    temp_c:    float = 80.0    # alert when any sensor exceeds this
+    disk_pct:  float = 90.0    # alert when disk usage exceeds this
+    ram_pct:   float = 90.0    # alert when RAM usage exceeds this
+    power_mw:  float = 30_000  # alert when total power exceeds 30 W
+
+
+@dataclass
+class Alert:
+    """A single threshold-violation alert."""
+    component: str            # e.g. "cpu", "gpu", "temp_CPU", "disk"
+    message:   str
+    level:     int            # 1=WARN, 2=ERROR (matches DiagnosticStatus levels)
+    value:     float = 0.0    # observed value
+    threshold: float = 0.0    # configured threshold
+
+    WARN  = 1
+    ERROR = 2
+
+    def is_error(self) -> bool:
+        return self.level == self.ERROR
+
+    def is_warn(self) -> bool:
+        return self.level == self.WARN
+
+
+class AlertChecker:
+    """Check a JetsonStats snapshot against AlertThresholds.
+
+    Returns a list of Alert objects (empty = all OK).
+    Levels:
+        WARN  → value is between threshold and threshold+10% (or single step)
+        ERROR → value is above threshold+10% (or temp > threshold)
+    """
+
+    def check(
+        self,
+        stats:      JetsonStats,
+        thresholds: AlertThresholds,
+    ) -> List[Alert]:
+        alerts: List[Alert] = []
+
+        # CPU
+        if stats.cpu_avg_pct >= thresholds.cpu_pct:
+            level = (Alert.ERROR if stats.cpu_avg_pct >= thresholds.cpu_pct + 10
+                     else Alert.WARN)
+            alerts.append(Alert(
+                component = "cpu",
+                message   = (f"CPU avg {stats.cpu_avg_pct:.1f}% "
+                             f">= {thresholds.cpu_pct:.0f}%"),
+                level     = level,
+                value     = stats.cpu_avg_pct,
+                threshold = thresholds.cpu_pct,
+            ))
+
+        # GPU
+        if stats.gpu_pct >= thresholds.gpu_pct:
+            level = (Alert.ERROR if stats.gpu_pct >= thresholds.gpu_pct + 10
+                     else Alert.WARN)
+            alerts.append(Alert(
+                component = "gpu",
+                message   = (f"GPU {stats.gpu_pct:.1f}% "
+                             f">= {thresholds.gpu_pct:.0f}%"),
+                level     = level,
+                value     = stats.gpu_pct,
+                threshold = thresholds.gpu_pct,
+            ))
+
+        # Temperatures — check every sensor
+        for sensor, temp_c in stats.temperatures.items():
+            if temp_c >= thresholds.temp_c:
+                level = (Alert.ERROR if temp_c >= thresholds.temp_c + 10
+                         else Alert.WARN)
+                alerts.append(Alert(
+                    component = f"temp_{sensor}",
+                    message   = (f"{sensor} {temp_c:.1f}°C "
+                                 f">= {thresholds.temp_c:.0f}°C"),
+                    level     = level,
+                    value     = temp_c,
+                    threshold = thresholds.temp_c,
+                ))
+
+        # Disk
+        if stats.disk_pct >= thresholds.disk_pct:
+            level = (Alert.ERROR if stats.disk_pct >= thresholds.disk_pct + 5
+                     else Alert.WARN)
+            alerts.append(Alert(
+                component = "disk",
+                message   = (f"Disk {stats.disk_pct:.1f}% "
+                             f">= {thresholds.disk_pct:.0f}%"),
+                level     = level,
+                value     = stats.disk_pct,
+                threshold = thresholds.disk_pct,
+            ))
+
+        # RAM
+        if stats.ram_pct >= thresholds.ram_pct:
+            level = (Alert.ERROR if stats.ram_pct >= thresholds.ram_pct + 5
+                     else Alert.WARN)
+            alerts.append(Alert(
+                component = "ram",
+                message   = (f"RAM {stats.ram_pct:.1f}% "
+                             f">= {thresholds.ram_pct:.0f}%"),
+                level     = level,
+                value     = stats.ram_pct,
+                threshold = thresholds.ram_pct,
+            ))
+
+        # Power
+        if stats.power_total_mw >= thresholds.power_mw:
+            alerts.append(Alert(
+                component = "power",
+                message   = (f"Power {stats.power_total_mw/1000:.1f}W "
+                             f">= {thresholds.power_mw/1000:.0f}W"),
+                level     = Alert.WARN,
+                value     = stats.power_total_mw,
+                threshold = thresholds.power_mw,
+            ))
+
+        return alerts
+
+
+# ── Tegrastats parser ─────────────────────────────────────────────────────────
+
+class TegrastatsParser:
+    """Parse a single tegrastats output line into a JetsonStats snapshot."""
+
+    # RAM 1234/7772MB (lfb …)
+    _RE_RAM   = re.compile(r"RAM\s+(\d+)/(\d+)MB")
+    # SWAP 0/3886MB
+    _RE_SWAP  = re.compile(r"SWAP\s+(\d+)/(\d+)MB")
+    # CPU [12%@1190,5%@729,…]
+    _RE_CPU   = re.compile(r"CPU\s+\[([^\]]+)\]")
+    # GPC_FREQ 4%@918  (GPU usage + freq)
+    _RE_GPU   = re.compile(r"GPC_FREQ\s+(\d+)%@(\d+)")
+    # SensorName@Temp  e.g. CPU@39.6C  GPU@37.6C  tj@41.3C
+    _RE_TEMP  = re.compile(r"(\w+)@(\d+\.?\d*)C")
+    # VDD_IN 4002mW/4002mW  (total board power: instant/average)
+    _RE_VDD   = re.compile(r"VDD_IN\s+(\d+)mW/(\d+)mW")
+    # VDD_CPU_CV 700mW
+    _RE_VCPU  = re.compile(r"VDD_CPU_CV\s+(\d+)mW/(\d+)mW")
+    # VDD_GPU_SOC 1866mW
+    _RE_VGPU  = re.compile(r"VDD_GPU_SOC\s+(\d+)mW/(\d+)mW")
+
+    def parse(self, line: str) -> JetsonStats:
+        """Parse one tegrastats text line.  Returns JetsonStats."""
+        stats = JetsonStats(source="tegrastats")
+
+        # RAM
+        m = self._RE_RAM.search(line)
+        if m:
+            stats.ram_used_mb  = int(m.group(1))
+            stats.ram_total_mb = int(m.group(2))
+            if stats.ram_total_mb > 0:
+                stats.ram_pct = stats.ram_used_mb / stats.ram_total_mb * 100.0
+
+        # SWAP
+        m = self._RE_SWAP.search(line)
+        if m:
+            stats.swap_used_mb  = int(m.group(1))
+            stats.swap_total_mb = int(m.group(2))
+
+        # CPU cores
+        m = self._RE_CPU.search(line)
+        if m:
+            cores: List[CpuCore] = []
+            for idx, token in enumerate(m.group(1).split(",")):
+                parts = token.split("@")
+                pct_str = parts[0].rstrip("%")
+                freq    = int(parts[1]) if len(parts) > 1 else 0
+                try:
+                    cores.append(CpuCore(index=idx,
+                                         usage_pct=float(pct_str),
+                                         freq_mhz=freq))
+                except ValueError:
+                    pass
+            stats.cpu_cores   = cores
+            if cores:
+                stats.cpu_avg_pct = sum(c.usage_pct for c in cores) / len(cores)
+
+        # GPU
+        m = self._RE_GPU.search(line)
+        if m:
+            stats.gpu_pct      = float(m.group(1))
+            stats.gpu_freq_mhz = int(m.group(2))
+
+        # Temperatures
+        temps: Dict[str, float] = {}
+        for m in self._RE_TEMP.finditer(line):
+            name, val = m.group(1), float(m.group(2))
+            # Skip tokens that are part of CPU/freq strings like "1190"
+            if name.isdigit():
+                continue
+            temps[name] = val
+        stats.temperatures = temps
+        stats.temp_max_c   = max(temps.values(), default=0.0)
+
+        # Power
+        m = self._RE_VDD.search(line)
+        if m:
+            stats.power_total_mw = int(m.group(1))
+        m = self._RE_VCPU.search(line)
+        if m:
+            stats.power_cpu_mw = int(m.group(1))
+        m = self._RE_VGPU.search(line)
+        if m:
+            stats.power_gpu_mw = int(m.group(1))
+
+        # Disk — tegrastats does not include disk; use psutil fallback
+        stats.disk_used_gb, stats.disk_total_gb, stats.disk_pct = \
+            _read_disk_psutil()
+
+        stats.timestamp = time.time()
+        return stats
+
+
+# ── Disk helper ───────────────────────────────────────────────────────────────
+
+def _read_disk_psutil(path: str = "/") -> Tuple[float, float, float]:
+    """Return (used_gb, total_gb, pct) for the given path."""
+    if HAS_PSUTIL:
+        try:
+            u = _psutil.disk_usage(path)
+            used_gb  = u.used  / 1024**3
+            total_gb = u.total / 1024**3
+            pct      = u.percent
+            return used_gb, total_gb, pct
+        except OSError:
+            pass
+    # shutil fallback
+    try:
+        u = shutil.disk_usage(path)
+        total_gb = u.total / 1024**3
+        used_gb  = (u.total - u.free) / 1024**3
+        pct      = used_gb / total_gb * 100.0 if total_gb else 0.0
+        return used_gb, total_gb, pct
+    except OSError:
+        return 0.0, 0.0, 0.0
+
+
+# ── Stats readers ─────────────────────────────────────────────────────────────
+
+class TegrastatsReader:
+    """Reads one snapshot from tegrastats subprocess."""
+
+    _TEGRASTATS_BIN = "tegrastats"
+
+    def __init__(self, interval_ms: int = 200) -> None:
+        self._interval_ms = interval_ms
+        self._parser      = TegrastatsParser()
+
+    def is_available(self) -> bool:
+        return shutil.which(self._TEGRASTATS_BIN) is not None
+
+    def read(self) -> Optional[JetsonStats]:
+        """Launch tegrastats, capture one line, parse it."""
+        if not self.is_available():
+            return None
+        try:
+            # --interval N: emit every N ms; capture 1 line then kill
+            result = subprocess.run(
+                [self._TEGRASTATS_BIN, f"--interval", str(self._interval_ms)],
+                capture_output=True,
+                text=True,
+                timeout=3.0,
+            )
+            # tegrastats may not exit on its own; take first non-empty line
+            for line in result.stdout.splitlines():
+                line = line.strip()
+                if line:
+                    return self._parser.parse(line)
+        except (subprocess.TimeoutExpired, subprocess.SubprocessError,
+                FileNotFoundError, OSError):
+            pass
+        return None
+
+
+class JtopReader:
+    """Reads one snapshot via the jtop Python library (jetson_stats)."""
+
+    def is_available(self) -> bool:
+        return HAS_JTOP
+
+    def read(self) -> Optional[JetsonStats]:
+        if not HAS_JTOP:
+            return None
+        try:
+            with _jtop() as jetson:
+                stats = JetsonStats(source="jtop")
+
+                # CPU
+                if hasattr(jetson, "cpu"):
+                    cores = []
+                    cpu_data = jetson.cpu
+                    # jetson_stats 4.x: cpu is a dict with keys 'CPU1', 'CPU2', ...
+                    if isinstance(cpu_data, dict):
+                        for i, (k, v) in enumerate(sorted(cpu_data.items())):
+                            if isinstance(v, dict):
+                                pct  = float(v.get("val", 0) or 0)
+                                freq = int(v.get("frq", 0) or 0)
+                            else:
+                                pct, freq = 0.0, 0
+                            cores.append(CpuCore(index=i, usage_pct=pct,
+                                                  freq_mhz=freq))
+                    stats.cpu_cores   = cores
+                    stats.cpu_avg_pct = (sum(c.usage_pct for c in cores)
+                                         / len(cores)) if cores else 0.0
+
+                # GPU
+                if hasattr(jetson, "gpu"):
+                    gpu_d = jetson.gpu
+                    if isinstance(gpu_d, dict):
+                        stats.gpu_pct      = float(gpu_d.get("val", 0) or 0)
+                        stats.gpu_freq_mhz = int(gpu_d.get("frq", 0) or 0)
+
+                # RAM
+                if hasattr(jetson, "memory"):
+                    mem = jetson.memory
+                    if isinstance(mem, dict):
+                        ram = mem.get("RAM", {})
+                        if isinstance(ram, dict):
+                            stats.ram_used_mb  = int(ram.get("used", 0) or 0)
+                            stats.ram_total_mb = int(ram.get("tot",  0) or 0)
+                        swap = mem.get("SWAP", {})
+                        if isinstance(swap, dict):
+                            stats.swap_used_mb  = int(swap.get("used", 0) or 0)
+                            stats.swap_total_mb = int(swap.get("tot",  0) or 0)
+                    if stats.ram_total_mb > 0:
+                        stats.ram_pct = (stats.ram_used_mb /
+                                         stats.ram_total_mb * 100.0)
+
+                # Temperatures
+                if hasattr(jetson, "temperature"):
+                    temps = {}
+                    for k, v in jetson.temperature.items():
+                        if isinstance(v, dict):
+                            val = v.get("temp", None)
+                        else:
+                            val = v
+                        if val is not None:
+                            try:
+                                temps[str(k)] = float(val)
+                            except (TypeError, ValueError):
+                                pass
+                    stats.temperatures = temps
+                    stats.temp_max_c   = max(temps.values(), default=0.0)
+
+                # Fan
+                if hasattr(jetson, "fan"):
+                    fan_d = jetson.fan
+                    if isinstance(fan_d, dict):
+                        stats.fan_pct = float(fan_d.get("speed", 0) or 0)
+
+                # Power
+                if hasattr(jetson, "power"):
+                    pwr = jetson.power
+                    if isinstance(pwr, dict):
+                        tot = pwr.get("tot", {})
+                        if isinstance(tot, dict):
+                            stats.power_total_mw = int(
+                                (tot.get("power", 0) or 0) * 1000)
+
+                # Disk
+                stats.disk_used_gb, stats.disk_total_gb, stats.disk_pct = \
+                    _read_disk_psutil()
+
+                stats.timestamp = time.time()
+                return stats
+        except Exception:
+            return None
+
+
+class MockReader:
+    """Returns deterministic stats for testing and development."""
+
+    def __init__(self, **overrides) -> None:
+        self._overrides = overrides
+
+    def is_available(self) -> bool:
+        return True
+
+    def read(self) -> JetsonStats:
+        cores = [CpuCore(index=i, usage_pct=20.0 + i * 2, freq_mhz=1200)
+                 for i in range(4)]
+        s = JetsonStats(
+            cpu_cores    = cores,
+            cpu_avg_pct  = sum(c.usage_pct for c in cores) / len(cores),
+            gpu_pct      = 30.0,
+            gpu_freq_mhz = 918,
+            ram_used_mb  = 3000,
+            ram_total_mb = 7772,
+            ram_pct      = 3000 / 7772 * 100.0,
+            swap_used_mb = 0,
+            swap_total_mb= 3886,
+            temperatures = {"CPU": 40.0, "GPU": 38.0, "AO": 35.0, "tj": 42.0},
+            temp_max_c   = 42.0,
+            fan_pct      = 30.0,
+            power_total_mw = 8000,
+            power_cpu_mw   = 2000,
+            power_gpu_mw   = 1500,
+            disk_used_gb   = 50.0,
+            disk_total_gb  = 500.0,
+            disk_pct       = 10.0,
+            source         = "mock",
+        )
+        for k, v in self._overrides.items():
+            if hasattr(s, k):
+                setattr(s, k, v)
+        return s
+
+
+# ── Auto-selecting reader factory ─────────────────────────────────────────────
+
+def make_reader(prefer: str = "auto"):
+    """Return the best available reader.
+
+    prefer: "jtop" | "tegrastats" | "mock" | "auto"
+    """
+    if prefer == "mock":
+        return MockReader()
+    if prefer == "jtop" or (prefer == "auto" and HAS_JTOP):
+        r = JtopReader()
+        if r.is_available():
+            return r
+    if prefer == "tegrastats" or prefer == "auto":
+        r = TegrastatsReader()
+        if r.is_available():
+            return r
+    # Fallback: mock (dev machine without Jetson hw)
+    return MockReader()

jetson/ros2_ws/src/saltybot_system_monitor/saltybot_system_monitor/system_monitor_node.py

@@ -0,0 +1,215 @@
+"""system_monitor_node.py — ROS2 node for Jetson Orin system monitoring (Issue #631).
+
+Publishes at 1 Hz:
+  /saltybot/system/stats    std_msgs/String   JSON stats payload
+  /saltybot/diagnostics     diagnostic_msgs/DiagnosticArray  health status
+
+Stats source priority: jtop > tegrastats > mock (for testing).
+"""
+
+from __future__ import annotations
+
+import json
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+
+from saltybot_system_monitor.jetson_stats import (
+    AlertChecker,
+    AlertThresholds,
+    JetsonStats,
+    Alert,
+    make_reader,
+)
+
+
+class SystemMonitorNode(Node):
+    """Publishes Jetson Orin hardware stats at 1 Hz."""
+
+    def __init__(self) -> None:
+        super().__init__("system_monitor_node")
+
+        # ── Parameters ─────────────────────────────────────────────────────
+        self.declare_parameter("publish_rate_hz", 1.0)
+        self.declare_parameter("stats_source", "auto")   # auto | jtop | tegrastats | mock
+        self.declare_parameter("cpu_warn_pct",  85.0)
+        self.declare_parameter("gpu_warn_pct",  85.0)
+        self.declare_parameter("temp_warn_c",   80.0)
+        self.declare_parameter("disk_warn_pct", 90.0)
+        self.declare_parameter("ram_warn_pct",  90.0)
+        self.declare_parameter("power_warn_mw", 30_000.0)
+
+        rate     = self.get_parameter("publish_rate_hz").value
+        source   = self.get_parameter("stats_source").value
+
+        self._thresholds = AlertThresholds(
+            cpu_pct   = float(self.get_parameter("cpu_warn_pct").value),
+            gpu_pct   = float(self.get_parameter("gpu_warn_pct").value),
+            temp_c    = float(self.get_parameter("temp_warn_c").value),
+            disk_pct  = float(self.get_parameter("disk_warn_pct").value),
+            ram_pct   = float(self.get_parameter("ram_warn_pct").value),
+            power_mw  = float(self.get_parameter("power_warn_mw").value),
+        )
+
+        self._reader  = make_reader(prefer=source)
+        self._checker = AlertChecker()
+
+        # ── Publishers ─────────────────────────────────────────────────────
+        self._stats_pub = self.create_publisher(String, "/saltybot/system/stats", 10)
+        self._diag_pub  = self.create_publisher(
+            DiagnosticArray, "/saltybot/diagnostics", 10
+        )
+
+        # ── Timer ──────────────────────────────────────────────────────────
+        period = 1.0 / max(rate, 0.1)
+        self._timer = self.create_timer(period, self._on_timer)
+
+        self.get_logger().info(
+            f"SystemMonitorNode started (source={source}, rate={rate} Hz)"
+        )
+
+    # ── Timer callback ─────────────────────────────────────────────────────
+
+    def _on_timer(self) -> None:
+        stats = self._reader.read()
+        if stats is None:
+            self.get_logger().warning("Stats reader returned None — skipping cycle")
+            return
+
+        alerts = self._checker.check(stats, self._thresholds)
+
+        self._publish_stats(stats, alerts)
+        self._publish_diagnostics(stats, alerts)
+
+    # ── Stats publisher ────────────────────────────────────────────────────
+
+    def _publish_stats(self, stats: JetsonStats, alerts: list) -> None:
+        payload = {
+            "timestamp": stats.timestamp,
+            "source":    stats.source,
+            "cpu": {
+                "avg_pct": stats.cpu_avg_pct,
+                "cores":   [
+                    {"index": c.index, "usage_pct": c.usage_pct, "freq_mhz": c.freq_mhz}
+                    for c in stats.cpu_cores
+                ],
+            },
+            "gpu": {
+                "pct":      stats.gpu_pct,
+                "freq_mhz": stats.gpu_freq_mhz,
+            },
+            "ram": {
+                "used_mb":  stats.ram_used_mb,
+                "total_mb": stats.ram_total_mb,
+                "pct":      stats.ram_pct,
+            },
+            "swap": {
+                "used_mb":  stats.swap_used_mb,
+                "total_mb": stats.swap_total_mb,
+            },
+            "temperatures": stats.temperatures,
+            "temp_max_c":   stats.temp_max_c,
+            "fan_pct":      stats.fan_pct,
+            "power": {
+                "total_mw": stats.power_total_mw,
+                "cpu_mw":   stats.power_cpu_mw,
+                "gpu_mw":   stats.power_gpu_mw,
+            },
+            "disk": {
+                "used_gb":  stats.disk_used_gb,
+                "total_gb": stats.disk_total_gb,
+                "pct":      stats.disk_pct,
+            },
+            "alerts": [
+                {
+                    "component": a.component,
+                    "message":   a.message,
+                    "level":     a.level,
+                    "value":     a.value,
+                    "threshold": a.threshold,
+                }
+                for a in alerts
+            ],
+        }
+        msg = String()
+        msg.data = json.dumps(payload)
+        self._stats_pub.publish(msg)
+
+    # ── DiagnosticArray publisher ──────────────────────────────────────────
+
+    def _publish_diagnostics(self, stats: JetsonStats, alerts: list) -> None:
+        now = self.get_clock().now().to_msg()
+        diag = DiagnosticArray()
+        diag.header.stamp = now
+
+        # Overall system status — worst level among alerts
+        worst = 0
+        for a in alerts:
+            if a.level > worst:
+                worst = a.level
+
+        overall = DiagnosticStatus()
+        overall.name    = "saltybot/system"
+        overall.hardware_id = "jetson_orin"
+        overall.level   = worst
+        overall.message = "OK" if worst == 0 else (
+            "WARN" if worst == Alert.WARN else "ERROR"
+        )
+        overall.values  = self._stats_to_kv(stats)
+        diag.status.append(overall)
+
+        # One status per alert
+        for a in alerts:
+            s = DiagnosticStatus()
+            s.name        = f"saltybot/system/{a.component}"
+            s.hardware_id = "jetson_orin"
+            s.level       = a.level
+            s.message     = a.message
+            s.values      = [
+                KeyValue(key="value",     value=str(a.value)),
+                KeyValue(key="threshold", value=str(a.threshold)),
+            ]
+            diag.status.append(s)
+
+        self._diag_pub.publish(diag)
+
+    # ── Helper: stats → KeyValue list ──────────────────────────────────────
+
+    @staticmethod
+    def _stats_to_kv(stats: JetsonStats) -> list:
+        pairs = [
+            ("source",       stats.source),
+            ("cpu_avg_pct",  f"{stats.cpu_avg_pct:.1f}"),
+            ("gpu_pct",      f"{stats.gpu_pct:.1f}"),
+            ("gpu_freq_mhz", str(stats.gpu_freq_mhz)),
+            ("ram_pct",      f"{stats.ram_pct:.1f}"),
+            ("ram_used_mb",  str(stats.ram_used_mb)),
+            ("ram_total_mb", str(stats.ram_total_mb)),
+            ("temp_max_c",   f"{stats.temp_max_c:.1f}"),
+            ("fan_pct",      f"{stats.fan_pct:.1f}"),
+            ("power_total_mw", str(stats.power_total_mw)),
+            ("disk_pct",     f"{stats.disk_pct:.1f}"),
+            ("disk_used_gb", f"{stats.disk_used_gb:.1f}"),
+            ("disk_total_gb", f"{stats.disk_total_gb:.1f}"),
+        ]
+        for name, temp in stats.temperatures.items():
+            pairs.append((f"temp_{name}_c", f"{temp:.1f}"))
+        return [KeyValue(key=k, value=v) for k, v in pairs]
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = SystemMonitorNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_system_monitor/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/saltybot_system_monitor
+[install]
+install_scripts=$base/lib/saltybot_system_monitor

jetson/ros2_ws/src/saltybot_system_monitor/setup.py

@@ -0,0 +1,30 @@
+from setuptools import setup
+
+package_name = "saltybot_system_monitor"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (f"share/{package_name}/launch", ["launch/system_monitor.launch.py"]),
+        (f"share/{package_name}/config",  ["config/system_monitor.yaml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="sl-jetson",
+    maintainer_email="sl-jetson@saltylab.local",
+    description=(
+        "Jetson Orin system monitor: CPU/GPU/RAM/disk/temp/fan/power → "
+        "/saltybot/system/stats + DiagnosticArray (Issue #631)"
+    ),
+    license="Apache-2.0",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "system_monitor_node = saltybot_system_monitor.system_monitor_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_system_monitor/test/test_system_monitor.py

@@ -0,0 +1,507 @@
+"""test_system_monitor.py — Unit tests for saltybot_system_monitor (Issue #631).
+
+All tests are ROS2-free: they only import pure-Python modules from
+saltybot_system_monitor.jetson_stats.
+"""
+
+from __future__ import annotations
+
+import math
+import sys
+import time
+from unittest.mock import patch, MagicMock
+
+import pytest
+
+# ---------------------------------------------------------------------------
+# Ensure the package under test is importable (no ROS2 required)
+# ---------------------------------------------------------------------------
+
+import os
+import importlib
+
+_PKG_DIR = os.path.join(
+    os.path.dirname(__file__), "..", "saltybot_system_monitor"
+)
+sys.path.insert(0, os.path.join(_PKG_DIR, ".."))  # adds src/ to path
+
+from saltybot_system_monitor.jetson_stats import (
+    Alert,
+    AlertChecker,
+    AlertThresholds,
+    CpuCore,
+    DiskStats,
+    JetsonStats,
+    JtopReader,
+    MockReader,
+    TegrastatsParser,
+    TegrastatsReader,
+    make_reader,
+)
+
+
+# ---------------------------------------------------------------------------
+# Helpers
+# ---------------------------------------------------------------------------
+
+def _make_stats(**kw) -> JetsonStats:
+    """Build a minimal JetsonStats with safe defaults, overriding with kw."""
+    defaults = dict(
+        timestamp=time.monotonic(),
+        cpu_cores=[CpuCore(index=0, usage_pct=10.0, freq_mhz=1200)],
+        cpu_avg_pct=10.0,
+        gpu_pct=5.0,
+        gpu_freq_mhz=500,
+        ram_used_mb=4096,
+        ram_total_mb=16384,
+        ram_pct=25.0,
+        swap_used_mb=0,
+        swap_total_mb=8192,
+        temperatures={"CPU": 45.0, "GPU": 42.0},
+        temp_max_c=45.0,
+        fan_pct=20.0,
+        power_total_mw=8000,
+        power_cpu_mw=3000,
+        power_gpu_mw=2000,
+        disk_used_gb=50.0,
+        disk_total_gb=500.0,
+        disk_pct=10.0,
+        source="test",
+    )
+    defaults.update(kw)
+    return JetsonStats(**defaults)
+
+
+_DEFAULT_THRESH = AlertThresholds()
+_CHECKER = AlertChecker()
+
+
+def _alerts(**kw) -> list[Alert]:
+    return _CHECKER.check(_make_stats(**kw), _DEFAULT_THRESH)
+
+
+def _alert_components(**kw) -> set[str]:
+    return {a.component for a in _alerts(**kw)}
+
+
+# ===========================================================================
+# CpuCore
+# ===========================================================================
+
+class TestCpuCore:
+    def test_basic(self):
+        c = CpuCore(index=2, usage_pct=55.5, freq_mhz=1800)
+        assert c.index == 2
+        assert c.usage_pct == pytest.approx(55.5)
+        assert c.freq_mhz == 1800
+
+    def test_default_freq(self):
+        c = CpuCore(index=0, usage_pct=0.0)
+        assert c.freq_mhz == 0
+
+
+# ===========================================================================
+# JetsonStats
+# ===========================================================================
+
+class TestJetsonStats:
+    def test_fields_present(self):
+        s = _make_stats()
+        assert hasattr(s, "cpu_avg_pct")
+        assert hasattr(s, "gpu_pct")
+        assert hasattr(s, "ram_pct")
+        assert hasattr(s, "temp_max_c")
+        assert hasattr(s, "fan_pct")
+        assert hasattr(s, "power_total_mw")
+        assert hasattr(s, "disk_pct")
+        assert hasattr(s, "source")
+
+    def test_source_default(self):
+        s = _make_stats(source="jtop")
+        assert s.source == "jtop"
+
+
+# ===========================================================================
+# AlertThresholds
+# ===========================================================================
+
+class TestAlertThresholds:
+    def test_defaults(self):
+        t = AlertThresholds()
+        assert t.cpu_pct   == pytest.approx(85.0)
+        assert t.gpu_pct   == pytest.approx(85.0)
+        assert t.temp_c    == pytest.approx(80.0)
+        assert t.disk_pct  == pytest.approx(90.0)
+        assert t.ram_pct   == pytest.approx(90.0)
+        assert t.power_mw  == pytest.approx(30_000.0)
+
+    def test_custom(self):
+        t = AlertThresholds(cpu_pct=70.0, temp_c=75.0)
+        assert t.cpu_pct == pytest.approx(70.0)
+        assert t.temp_c  == pytest.approx(75.0)
+
+
+# ===========================================================================
+# AlertChecker — no alerts in normal range
+# ===========================================================================
+
+class TestAlertCheckerNoAlerts:
+    def test_no_alert_all_ok(self):
+        assert _alerts() == []
+
+    def test_cpu_just_below_warn(self):
+        # 84.9 < 85.0 — no alert
+        assert "cpu" not in _alert_components(cpu_avg_pct=84.9)
+
+    def test_gpu_just_below_warn(self):
+        assert "gpu" not in _alert_components(gpu_pct=84.9)
+
+    def test_temp_just_below_warn(self):
+        s = _make_stats(temperatures={"CPU": 79.9}, temp_max_c=79.9)
+        assert _CHECKER.check(s, _DEFAULT_THRESH) == []
+
+    def test_disk_just_below_warn(self):
+        assert "disk" not in _alert_components(disk_pct=89.9)
+
+    def test_ram_just_below_warn(self):
+        assert "ram" not in _alert_components(ram_pct=89.9)
+
+
+# ===========================================================================
+# AlertChecker — WARN level
+# ===========================================================================
+
+class TestAlertCheckerWarn:
+    def test_cpu_warn_at_threshold(self):
+        comps = _alert_components(cpu_avg_pct=85.0)
+        assert "cpu" in comps
+        alerts = _alerts(cpu_avg_pct=85.0)
+        cpu_alert = next(a for a in alerts if a.component == "cpu")
+        assert cpu_alert.level == Alert.WARN
+
+    def test_gpu_warn(self):
+        alerts = _alerts(gpu_pct=90.0)
+        gpu_alert = next(a for a in alerts if a.component == "gpu")
+        assert gpu_alert.level == Alert.WARN
+
+    def test_temp_warn(self):
+        s = _make_stats(temperatures={"AO": 80.0}, temp_max_c=80.0)
+        alerts = _CHECKER.check(s, _DEFAULT_THRESH)
+        temp_alerts = [a for a in alerts if "temp" in a.component.lower()]
+        assert any(a.level == Alert.WARN for a in temp_alerts)
+
+    def test_disk_warn(self):
+        alerts = _alerts(disk_pct=90.0)
+        disk_alert = next(a for a in alerts if a.component == "disk")
+        assert disk_alert.level == Alert.WARN
+
+    def test_ram_warn(self):
+        alerts = _alerts(ram_pct=90.0)
+        ram_alert = next(a for a in alerts if a.component == "ram")
+        assert ram_alert.level == Alert.WARN
+
+    def test_power_warn(self):
+        alerts = _alerts(power_total_mw=30_000)
+        power_alert = next(a for a in alerts if a.component == "power")
+        assert power_alert.level == Alert.WARN
+
+    def test_alert_carries_value_and_threshold(self):
+        alerts = _alerts(cpu_avg_pct=87.0)
+        a = next(x for x in alerts if x.component == "cpu")
+        assert a.value    == pytest.approx(87.0)
+        assert a.threshold == pytest.approx(85.0)
+
+
+# ===========================================================================
+# AlertChecker — ERROR level (threshold + escalation)
+# ===========================================================================
+
+class TestAlertCheckerError:
+    def test_cpu_error_at_95(self):
+        # 85 + 10% = 95 → ERROR
+        alerts = _alerts(cpu_avg_pct=95.0)
+        a = next(x for x in alerts if x.component == "cpu")
+        assert a.level == Alert.ERROR
+
+    def test_gpu_error_at_95(self):
+        alerts = _alerts(gpu_pct=95.0)
+        a = next(x for x in alerts if x.component == "gpu")
+        assert a.level == Alert.ERROR
+
+    def test_temp_error_at_90(self):
+        # 80 + 10 = 90 → ERROR
+        s = _make_stats(temperatures={"CPU": 90.0}, temp_max_c=90.0)
+        alerts = _CHECKER.check(s, _DEFAULT_THRESH)
+        a = next(x for x in alerts if "temp" in x.component.lower())
+        assert a.level == Alert.ERROR
+
+    def test_disk_error_at_95(self):
+        # 90 + 5 = 95 → ERROR
+        alerts = _alerts(disk_pct=95.0)
+        a = next(x for x in alerts if x.component == "disk")
+        assert a.level == Alert.ERROR
+
+    def test_ram_error_at_95(self):
+        alerts = _alerts(ram_pct=95.0)
+        a = next(x for x in alerts if x.component == "ram")
+        assert a.level == Alert.ERROR
+
+    def test_multiple_errors(self):
+        alerts = _alerts(cpu_avg_pct=99.0, gpu_pct=99.0, disk_pct=99.0)
+        comps = {a.component for a in alerts if a.level == Alert.ERROR}
+        assert "cpu" in comps
+        assert "gpu" in comps
+        assert "disk" in comps
+
+
+# ===========================================================================
+# TegrastatsParser
+# ===========================================================================
+
+class TestTegrastatsParser:
+    P = TegrastatsParser()
+
+    # ── RAM ────────────────────────────────────────────────────────────────
+
+    def test_ram(self):
+        s = self.P.parse("RAM 2048/16384MB (lfb 512x4MB) SWAP 256/8192MB")
+        assert s.ram_used_mb  == 2048
+        assert s.ram_total_mb == 16384
+        assert s.ram_pct      == pytest.approx(2048 / 16384 * 100, rel=1e-3)
+
+    def test_ram_zero_total_doesnt_crash(self):
+        # Shouldn't happen in practice, but parser must not divide-by-zero
+        s = self.P.parse("RAM 0/0MB SWAP 0/0MB CPU [] GPC_FREQ 0%@0")
+        assert s.ram_pct == pytest.approx(0.0)
+
+    # ── SWAP ───────────────────────────────────────────────────────────────
+
+    def test_swap(self):
+        s = self.P.parse("RAM 1024/8192MB (lfb 4x4MB) SWAP 512/4096MB")
+        assert s.swap_used_mb  == 512
+        assert s.swap_total_mb == 4096
+
+    # ── CPU ────────────────────────────────────────────────────────────────
+
+    def test_cpu_single_core(self):
+        s = self.P.parse("RAM 1000/8000MB SWAP 0/0MB CPU [50%@1200]")
+        assert len(s.cpu_cores) == 1
+        assert s.cpu_cores[0].usage_pct  == pytest.approx(50.0)
+        assert s.cpu_cores[0].freq_mhz   == 1200
+        assert s.cpu_avg_pct             == pytest.approx(50.0)
+
+    def test_cpu_four_cores(self):
+        s = self.P.parse(
+            "RAM 4096/16384MB SWAP 0/8192MB "
+            "CPU [10%@1000,20%@1200,30%@1400,40%@1600]"
+        )
+        assert len(s.cpu_cores) == 4
+        assert s.cpu_cores[0].usage_pct == pytest.approx(10.0)
+        assert s.cpu_cores[3].usage_pct == pytest.approx(40.0)
+        assert s.cpu_avg_pct            == pytest.approx(25.0)
+
+    def test_cpu_idle_core(self):
+        # off/0 means core is offline — should be parsed as 0% usage
+        s = self.P.parse(
+            "RAM 4096/16384MB SWAP 0/8192MB CPU [10%@1200,off,20%@1400,off]"
+        )
+        # off cores are skipped or treated as 0
+        assert len(s.cpu_cores) >= 2
+
+    # ── GPU ────────────────────────────────────────────────────────────────
+
+    def test_gpu(self):
+        s = self.P.parse(
+            "RAM 4096/16384MB SWAP 0/8192MB CPU [50%@1200] GPC_FREQ 75%@850"
+        )
+        assert s.gpu_pct      == pytest.approx(75.0)
+        assert s.gpu_freq_mhz == 850
+
+    def test_gpu_absent(self):
+        s = self.P.parse("RAM 4096/16384MB SWAP 0/8192MB CPU [50%@1200]")
+        assert s.gpu_pct      == pytest.approx(0.0)
+        assert s.gpu_freq_mhz == 0
+
+    # ── Temperatures ───────────────────────────────────────────────────────
+
+    def test_temperatures(self):
+        s = self.P.parse(
+            "RAM 4096/16384MB SWAP 0/8192MB CPU [50%@1200] "
+            "CPU@45.5C GPU@42.0C AO@38.0C"
+        )
+        assert "CPU" in s.temperatures
+        assert s.temperatures["CPU"] == pytest.approx(45.5)
+        assert s.temperatures["GPU"] == pytest.approx(42.0)
+        assert s.temp_max_c          == pytest.approx(45.5)
+
+    def test_temp_max_computed(self):
+        s = self.P.parse(
+            "RAM 4096/16384MB SWAP 0/8192MB CPU [50%@1200] "
+            "CPU@55.0C GPU@72.0C"
+        )
+        assert s.temp_max_c == pytest.approx(72.0)
+
+    # ── Power ──────────────────────────────────────────────────────────────
+
+    def test_power_vdd_in(self):
+        s = self.P.parse(
+            "RAM 4096/16384MB SWAP 0/8192MB CPU [50%@1200] "
+            "VDD_IN 8000mW/9000mW VDD_CPU_CV 3000mW/3500mW VDD_GPU_SOC 2000mW/2500mW"
+        )
+        assert s.power_total_mw == 8000
+        assert s.power_cpu_mw   == 3000
+        assert s.power_gpu_mw   == 2000
+
+    def test_power_absent(self):
+        s = self.P.parse("RAM 4096/16384MB SWAP 0/8192MB CPU [50%@1200]")
+        assert s.power_total_mw == 0
+
+    # ── Full realistic line ────────────────────────────────────────────────
+
+    def test_full_tegrastats_line(self):
+        line = (
+            "RAM 5012/15625MB (lfb 1x2MB) SWAP 0/7812MB "
+            "CPU [12%@1190,8%@1190,15%@1190,9%@1190,5%@1190,7%@1190] "
+            "GPC_FREQ 0%@318 CPU@47.218C GPU@44.125C Tdiode@45.25C AO@47.5C "
+            "thermal@46.7C VDD_IN 6391mW/6391mW VDD_CPU_CV 2042mW/2042mW "
+            "VDD_GPU_SOC 1499mW/1499mW"
+        )
+        s = self.P.parse(line)
+        assert len(s.cpu_cores) == 6
+        assert s.gpu_pct      == pytest.approx(0.0)
+        assert s.ram_used_mb  == 5012
+        assert s.ram_total_mb == 15625
+        assert s.temperatures["CPU"] == pytest.approx(47.218)
+        assert s.power_total_mw == 6391
+        assert s.source == "tegrastats"
+
+
+# ===========================================================================
+# MockReader
+# ===========================================================================
+
+class TestMockReader:
+    def test_returns_jetson_stats(self):
+        r = MockReader()
+        s = r.read()
+        assert isinstance(s, JetsonStats)
+
+    def test_source_is_mock(self):
+        s = MockReader().read()
+        assert s.source == "mock"
+
+    def test_deterministic(self):
+        r = MockReader()
+        s1 = r.read()
+        s2 = r.read()
+        assert s1.cpu_avg_pct == s2.cpu_avg_pct
+        assert s1.gpu_pct     == s2.gpu_pct
+
+    def test_overrides(self):
+        r = MockReader(cpu_avg_pct=99.9, gpu_pct=88.8)
+        s = r.read()
+        assert s.cpu_avg_pct == pytest.approx(99.9)
+        assert s.gpu_pct     == pytest.approx(88.8)
+
+    def test_override_temperatures(self):
+        r = MockReader(temperatures={"CPU": 75.0}, temp_max_c=75.0)
+        s = r.read()
+        assert s.temperatures == {"CPU": 75.0}
+        assert s.temp_max_c   == pytest.approx(75.0)
+
+
+# ===========================================================================
+# TegrastatsReader availability check
+# ===========================================================================
+
+class TestTegrastatsReader:
+    def test_is_available_true_when_found(self):
+        r = TegrastatsReader()
+        with patch("shutil.which", return_value="/usr/bin/tegrastats"):
+            assert r.is_available() is True
+
+    def test_is_available_false_when_missing(self):
+        r = TegrastatsReader()
+        with patch("shutil.which", return_value=None):
+            assert r.is_available() is False
+
+
+# ===========================================================================
+# JtopReader availability check
+# ===========================================================================
+
+class TestJtopReader:
+    def test_is_available_false_without_jtop(self):
+        r = JtopReader()
+        with patch.dict(sys.modules, {"jtop": None}):
+            # If jtop is not importable, should return False
+            # (actual behaviour depends on HAS_JTOP at import time)
+            result = r.is_available()
+            assert isinstance(result, bool)
+
+
+# ===========================================================================
+# make_reader factory
+# ===========================================================================
+
+class TestMakeReader:
+    def test_mock_returns_mock_reader(self):
+        r = make_reader(prefer="mock")
+        assert isinstance(r, MockReader)
+
+    def test_auto_with_no_backends_falls_back_to_mock(self):
+        with (
+            patch("shutil.which", return_value=None),
+            patch("saltybot_system_monitor.jetson_stats.HAS_JTOP", False),
+        ):
+            r = make_reader(prefer="auto")
+            assert isinstance(r, MockReader)
+
+    def test_tegrastats_prefer(self):
+        with patch("shutil.which", return_value="/usr/bin/tegrastats"):
+            r = make_reader(prefer="tegrastats")
+            assert isinstance(r, TegrastatsReader)
+
+    def test_auto_prefers_tegrastats_over_mock(self):
+        with (
+            patch("shutil.which", return_value="/usr/bin/tegrastats"),
+            patch("saltybot_system_monitor.jetson_stats.HAS_JTOP", False),
+        ):
+            r = make_reader(prefer="auto")
+            assert isinstance(r, TegrastatsReader)
+
+
+# ===========================================================================
+# Edge cases
+# ===========================================================================
+
+class TestEdgeCases:
+    P = TegrastatsParser()
+
+    def test_parse_empty_string(self):
+        s = self.P.parse("")
+        assert isinstance(s, JetsonStats)
+        assert s.ram_used_mb  == 0
+        assert s.cpu_cores    == []
+
+    def test_parse_partial_line(self):
+        s = self.P.parse("RAM 1024/8192MB")
+        assert s.ram_used_mb  == 1024
+        assert s.cpu_cores    == []
+
+    def test_alert_checker_empty_temps(self):
+        s = _make_stats(temperatures={}, temp_max_c=0.0)
+        alerts = _CHECKER.check(s, _DEFAULT_THRESH)
+        temp_alerts = [a for a in alerts if "temp" in a.component.lower()]
+        assert temp_alerts == []
+
+    def test_alert_checker_multiple_temp_sensors(self):
+        s = _make_stats(
+            temperatures={"CPU": 82.0, "GPU": 50.0, "AO": 40.0},
+            temp_max_c=82.0,
+        )
+        alerts = _CHECKER.check(s, _DEFAULT_THRESH)
+        temp_alerts = [a for a in alerts if "temp" in a.component.lower()]
+        assert len(temp_alerts) >= 1
+        # Only CPU temp should fire
+        assert all("CPU" in a.component or a.value >= 80.0 for a in temp_alerts)

jetson/ros2_ws/src/saltybot_uwb_logger/config/uwb_logger_params.yaml

@@ -0,0 +1,7 @@
+uwb_logger:
+  ros__parameters:
+    log_dir: ~/uwb_logs
+    enable_csv: true
+    default_n_samples: 200
+    default_timeout_s: 30.0
+    flush_interval_s: 5.0

jetson/ros2_ws/src/saltybot_uwb_logger/launch/uwb_logger.launch.py

@@ -0,0 +1,28 @@
+"""uwb_logger.launch.py — Launch UWB position logger (Issue #634)."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description() -> LaunchDescription:
+    pkg_dir = get_package_share_directory("saltybot_uwb_logger")
+    params_file = os.path.join(pkg_dir, "config", "uwb_logger_params.yaml")
+
+    return LaunchDescription([
+        DeclareLaunchArgument("log_dir", default_value="~/uwb_logs"),
+        Node(
+            package="saltybot_uwb_logger",
+            executable="uwb_logger",
+            name="uwb_logger",
+            parameters=[
+                params_file,
+                {"log_dir": LaunchConfiguration("log_dir")},
+            ],
+            output="screen",
+            emulate_tty=True,
+        ),
+    ])

jetson/ros2_ws/src/saltybot_uwb_logger/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_uwb_logger</name>
+  <version>0.1.0</version>
+  <description>
+    UWB position logger and accuracy analyzer (Issue #634).
+    Logs fused pose, raw UWB pose, per-anchor ranges to CSV.
+    Provides /saltybot/uwb/start_accuracy_test: collects N samples at a
+    known position and computes CEP50, CEP95, RMSE, max error.
+  </description>
+  <maintainer email="sl-uwb@saltylab.local">sl-uwb</maintainer>
+  <license>Apache-2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>saltybot_uwb_msgs</depend>
+  <depend>saltybot_uwb_logger_msgs</depend>
+
+  <exec_depend>python3-numpy</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_uwb_logger/saltybot_uwb_logger/accuracy_stats.py

@@ -0,0 +1,99 @@
+"""
+accuracy_stats.py — UWB positioning accuracy statistics (Issue #634)
+
+Pure numpy, no ROS2 dependency.
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field
+
+import numpy as np
+
+
+@dataclass
+class AccuracyStats:
+    n_samples: int
+    mean_x: float
+    mean_y: float
+    std_x: float
+    std_y: float
+    std_2d: float
+    bias_x: float
+    bias_y: float
+    cep50: float
+    cep95: float
+    max_error: float
+    rmse: float
+
+
+def compute_stats(
+    xs: np.ndarray,
+    ys: np.ndarray,
+    truth_x: float,
+    truth_y: float,
+) -> AccuracyStats:
+    """
+    Compute accuracy statistics from position samples.
+
+    Parameters
+    ----------
+    xs, ys       : 1-D arrays of measured x / y positions (metres)
+    truth_x/y    : known ground-truth position (metres)
+
+    Raises
+    ------
+    ValueError if fewer than 2 samples or mismatched lengths.
+    """
+    xs = np.asarray(xs, dtype=float)
+    ys = np.asarray(ys, dtype=float)
+    n = len(xs)
+    if n < 2:
+        raise ValueError(f"Need at least 2 samples, got {n}")
+    if len(ys) != n:
+        raise ValueError("xs and ys must have the same length")
+
+    errors = np.sqrt((xs - truth_x) ** 2 + (ys - truth_y) ** 2)
+
+    mean_x = float(np.mean(xs))
+    mean_y = float(np.mean(ys))
+    std_x  = float(np.std(xs, ddof=1))
+    std_y  = float(np.std(ys, ddof=1))
+
+    return AccuracyStats(
+        n_samples  = n,
+        mean_x     = mean_x,
+        mean_y     = mean_y,
+        std_x      = std_x,
+        std_y      = std_y,
+        std_2d     = math.sqrt(std_x ** 2 + std_y ** 2),
+        bias_x     = mean_x - truth_x,
+        bias_y     = mean_y - truth_y,
+        cep50      = float(np.percentile(errors, 50)),
+        cep95      = float(np.percentile(errors, 95)),
+        max_error  = float(np.max(errors)),
+        rmse       = float(np.sqrt(np.mean(errors ** 2))),
+    )
+
+
+@dataclass
+class RangeAccum:
+    """Running accumulator for per-anchor range statistics."""
+    anchor_id: int
+    _samples: list = field(default_factory=list, repr=False)
+
+    def add(self, range_m: float) -> None:
+        self._samples.append(range_m)
+
+    @property
+    def count(self) -> int:
+        return len(self._samples)
+
+    @property
+    def mean(self) -> float:
+        return float(np.mean(self._samples)) if self._samples else 0.0
+
+    @property
+    def std(self) -> float:
+        return float(np.std(self._samples, ddof=1)) if len(self._samples) >= 2 else 0.0

jetson/ros2_ws/src/saltybot_uwb_logger/saltybot_uwb_logger/logger_node.py

@@ -0,0 +1,409 @@
+"""
+logger_node.py — UWB position logger and accuracy analyzer (Issue #634)
+
+Subscriptions
+─────────────
+  /saltybot/pose/fused   geometry_msgs/PoseStamped   — EKF-fused pose (200 Hz)
+  /saltybot/uwb/pose     geometry_msgs/PoseStamped   — raw UWB pose (10 Hz)
+  /uwb/ranges            saltybot_uwb_msgs/UwbRangeArray — per-anchor ranges
+
+Service
+───────
+  /saltybot/uwb/start_accuracy_test  StartAccuracyTest
+    Collects N fused-pose samples at known position, computes accuracy
+    metrics, publishes AccuracyReport, writes JSON summary.
+
+Publishes
+─────────
+  /saltybot/uwb/accuracy_report  AccuracyReport
+
+CSV logging (continuous, to log_dir)
+─────────────────────────────────────
+  fused_pose_<DATE>.csv   — timestamp_ns, x_m, y_m, heading_rad
+  uwb_pose_<DATE>.csv     — timestamp_ns, x_m, y_m
+  uwb_ranges_<DATE>.csv   — timestamp_ns, anchor_id, range_m, raw_mm, rssi, tag_id
+  accuracy_<test_id>.json — accuracy test results
+
+Parameters
+──────────
+  log_dir              ~/uwb_logs
+  enable_csv           true
+  default_n_samples    200
+  default_timeout_s    30.0
+  flush_interval_s     5.0
+"""
+
+from __future__ import annotations
+
+import csv
+import json
+import math
+import os
+import threading
+import time
+from datetime import datetime, timezone
+from pathlib import Path
+from typing import Optional
+
+import numpy as np
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+from geometry_msgs.msg import PoseStamped
+from saltybot_uwb_msgs.msg import UwbRangeArray
+from saltybot_uwb_logger_msgs.msg import AccuracyReport
+from saltybot_uwb_logger_msgs.srv import StartAccuracyTest
+
+from saltybot_uwb_logger.accuracy_stats import compute_stats, RangeAccum
+
+
+_SENSOR_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=10,
+)
+
+
+class UwbLoggerNode(Node):
+
+    def __init__(self) -> None:
+        super().__init__("uwb_logger")
+
+        # ── Parameters ────────────────────────────────────────────────────
+        self.declare_parameter("log_dir",            os.path.expanduser("~/uwb_logs"))
+        self.declare_parameter("enable_csv",         True)
+        self.declare_parameter("default_n_samples",  200)
+        self.declare_parameter("default_timeout_s",  30.0)
+        self.declare_parameter("flush_interval_s",   5.0)
+
+        self._log_dir     = Path(self.get_parameter("log_dir").value)
+        self._enable_csv  = self.get_parameter("enable_csv").value
+        self._default_n   = self.get_parameter("default_n_samples").value
+        self._default_to  = self.get_parameter("default_timeout_s").value
+        self._flush_iv    = self.get_parameter("flush_interval_s").value
+
+        if self._enable_csv:
+            self._log_dir.mkdir(parents=True, exist_ok=True)
+
+        # ── CSV writers ───────────────────────────────────────────────────
+        date_tag = datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%SZ")
+        self._csv_lock   = threading.Lock()
+        self._csv_files  = []
+        self._fused_csv  = self._open_csv(
+            f"fused_pose_{date_tag}.csv",
+            ["timestamp_ns", "x_m", "y_m", "heading_rad"])
+        self._uwb_csv    = self._open_csv(
+            f"uwb_pose_{date_tag}.csv",
+            ["timestamp_ns", "x_m", "y_m"])
+        self._ranges_csv = self._open_csv(
+            f"uwb_ranges_{date_tag}.csv",
+            ["timestamp_ns", "anchor_id", "range_m", "raw_mm", "rssi", "tag_id"])
+
+        # ── Accuracy test state ────────────────────────────────────────────
+        self._test_lock       = threading.Lock()
+        self._test_active     = False
+        self._test_id         = ""
+        self._test_truth_x    = 0.0
+        self._test_truth_y    = 0.0
+        self._test_n_target   = 0
+        self._test_xs: list   = []
+        self._test_ys: list   = []
+        self._test_ranges: dict[int, RangeAccum] = {}
+        self._test_t0         = 0.0
+        self._test_timeout    = 0.0
+        self._test_done_event = threading.Event()
+
+        # ── Publisher ─────────────────────────────────────────────────────
+        self._report_pub = self.create_publisher(
+            AccuracyReport, "/saltybot/uwb/accuracy_report", 10
+        )
+
+        # ── Subscriptions ─────────────────────────────────────────────────
+        self.create_subscription(
+            PoseStamped, "/saltybot/pose/fused",
+            self._fused_cb, _SENSOR_QOS)
+        self.create_subscription(
+            PoseStamped, "/saltybot/uwb/pose",
+            self._uwb_pose_cb, _SENSOR_QOS)
+        self.create_subscription(
+            UwbRangeArray, "/uwb/ranges",
+            self._ranges_cb, _SENSOR_QOS)
+
+        # ── Service ───────────────────────────────────────────────────────
+        self._srv = self.create_service(
+            StartAccuracyTest,
+            "/saltybot/uwb/start_accuracy_test",
+            self._handle_start_test)
+
+        # ── Periodic flush ────────────────────────────────────────────────
+        self.create_timer(self._flush_iv, self._flush_csv)
+
+        self.get_logger().info(
+            f"UWB logger ready — log_dir={self._log_dir} csv={self._enable_csv}"
+        )
+
+    def destroy_node(self) -> None:
+        self._flush_csv()
+        for fh in self._csv_files:
+            try:
+                fh.close()
+            except Exception:
+                pass
+        super().destroy_node()
+
+    # ── Callbacks ─────────────────────────────────────────────────────────
+
+    def _fused_cb(self, msg: PoseStamped) -> None:
+        ts = _stamp_ns(msg.header.stamp)
+        x  = msg.pose.position.x
+        y  = msg.pose.position.y
+        q  = msg.pose.orientation
+        heading = 2.0 * math.atan2(float(q.z), float(q.w))
+
+        if self._enable_csv:
+            with self._csv_lock:
+                if self._fused_csv:
+                    self._fused_csv.writerow([ts, x, y, heading])
+
+        with self._test_lock:
+            if self._test_active:
+                self._test_xs.append(x)
+                self._test_ys.append(y)
+                if len(self._test_xs) >= self._test_n_target:
+                    self._test_active = False
+                    self._test_done_event.set()
+
+    def _uwb_pose_cb(self, msg: PoseStamped) -> None:
+        ts = _stamp_ns(msg.header.stamp)
+        x  = msg.pose.position.x
+        y  = msg.pose.position.y
+        if self._enable_csv:
+            with self._csv_lock:
+                if self._uwb_csv:
+                    self._uwb_csv.writerow([ts, x, y])
+
+    def _ranges_cb(self, msg: UwbRangeArray) -> None:
+        ts = _stamp_ns(msg.header.stamp)
+        for r in msg.ranges:
+            if self._enable_csv:
+                with self._csv_lock:
+                    if self._ranges_csv:
+                        self._ranges_csv.writerow([
+                            ts, r.anchor_id, r.range_m,
+                            r.raw_mm, r.rssi, r.tag_id,
+                        ])
+            with self._test_lock:
+                if self._test_active:
+                    aid = int(r.anchor_id)
+                    if aid not in self._test_ranges:
+                        self._test_ranges[aid] = RangeAccum(anchor_id=aid)
+                    self._test_ranges[aid].add(float(r.range_m))
+
+    # ── Service ────────────────────────────────────────────────────────────
+
+    def _handle_start_test(
+        self,
+        request: StartAccuracyTest.Request,
+        response: StartAccuracyTest.Response,
+    ) -> StartAccuracyTest.Response:
+
+        with self._test_lock:
+            if self._test_active:
+                response.success = False
+                response.message = "Another test is already running"
+                response.test_id = self._test_id
+                return response
+
+        n = int(request.n_samples) if request.n_samples > 0 else self._default_n
+        timeout = float(request.timeout_s) if request.timeout_s > 0.0 else self._default_to
+        test_id = (
+            request.test_id.strip()
+            if request.test_id.strip()
+            else datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%SZ")
+        )
+
+        self.get_logger().info(
+            f"Accuracy test '{test_id}' — "
+            f"truth=({request.truth_x_m:.3f}, {request.truth_y_m:.3f}) "
+            f"n={n} timeout={timeout}s"
+        )
+
+        with self._test_lock:
+            self._test_id       = test_id
+            self._test_truth_x  = float(request.truth_x_m)
+            self._test_truth_y  = float(request.truth_y_m)
+            self._test_n_target = n
+            self._test_xs       = []
+            self._test_ys       = []
+            self._test_ranges   = {}
+            self._test_t0       = time.monotonic()
+            self._test_timeout  = timeout
+            self._test_done_event.clear()
+            self._test_active   = True
+
+        threading.Thread(
+            target=self._run_test,
+            args=(test_id, request.truth_x_m, request.truth_y_m, n, timeout),
+            daemon=True,
+        ).start()
+
+        response.success = True
+        response.message = f"Test '{test_id}' started, collecting {n} samples"
+        response.test_id = test_id
+        return response
+
+    def _run_test(
+        self,
+        test_id: str,
+        truth_x: float,
+        truth_y: float,
+        n_target: int,
+        timeout: float,
+    ) -> None:
+        finished = self._test_done_event.wait(timeout=timeout)
+
+        with self._test_lock:
+            self._test_active = False
+            xs = list(self._test_xs)
+            ys = list(self._test_ys)
+            ranges_snap = dict(self._test_ranges)
+            t0 = self._test_t0
+
+        duration = time.monotonic() - t0
+        n_got = len(xs)
+
+        if n_got < 2:
+            self.get_logger().error(
+                f"Test '{test_id}' failed: only {n_got} samples in {duration:.1f}s"
+            )
+            return
+
+        if not finished:
+            self.get_logger().warn(
+                f"Test '{test_id}' timed out — {n_got}/{n_target} samples"
+            )
+
+        try:
+            stats = compute_stats(np.array(xs), np.array(ys), truth_x, truth_y)
+        except ValueError as exc:
+            self.get_logger().error(f"Stats failed: {exc}")
+            return
+
+        self.get_logger().info(
+            f"Test '{test_id}' — n={stats.n_samples} "
+            f"CEP50={stats.cep50*100:.1f}cm CEP95={stats.cep95*100:.1f}cm "
+            f"RMSE={stats.rmse*100:.1f}cm max={stats.max_error*100:.1f}cm"
+        )
+
+        # Publish report
+        report = AccuracyReport()
+        report.header.stamp    = self.get_clock().now().to_msg()
+        report.header.frame_id = "map"
+        report.truth_x_m   = truth_x
+        report.truth_y_m   = truth_y
+        report.n_samples   = stats.n_samples
+        report.mean_x_m    = stats.mean_x
+        report.mean_y_m    = stats.mean_y
+        report.std_x_m     = stats.std_x
+        report.std_y_m     = stats.std_y
+        report.std_2d_m    = stats.std_2d
+        report.bias_x_m    = stats.bias_x
+        report.bias_y_m    = stats.bias_y
+        report.cep50_m     = stats.cep50
+        report.cep95_m     = stats.cep95
+        report.max_error_m = stats.max_error
+        report.rmse_m      = stats.rmse
+        report.test_id     = test_id
+        report.duration_s  = duration
+
+        anchor_ids = sorted(ranges_snap.keys())
+        report.anchor_ids          = [int(a) for a in anchor_ids]
+        report.anchor_range_mean_m = [ranges_snap[a].mean for a in anchor_ids]
+        report.anchor_range_std_m  = [ranges_snap[a].std  for a in anchor_ids]
+
+        self._report_pub.publish(report)
+
+        # Write JSON
+        if self._enable_csv:
+            self._write_json(test_id, truth_x, truth_y, stats,
+                             anchor_ids, ranges_snap, duration)
+
+    # ── CSV / JSON helpers ─────────────────────────────────────────────────
+
+    def _open_csv(
+        self, filename: str, headers: list
+    ) -> Optional[csv.writer]:
+        if not self._enable_csv:
+            return None
+        path = self._log_dir / filename
+        fh = open(path, "w", newline="", buffering=1)
+        writer = csv.writer(fh)
+        writer.writerow(headers)
+        self._csv_files.append(fh)
+        return writer
+
+    def _flush_csv(self) -> None:
+        for fh in self._csv_files:
+            try:
+                fh.flush()
+            except Exception:
+                pass
+
+    def _write_json(self, test_id, truth_x, truth_y, stats,
+                    anchor_ids, ranges_snap, duration) -> None:
+        path = self._log_dir / f"accuracy_{test_id}.json"
+        data = {
+            "test_id":     test_id,
+            "truth":       {"x_m": truth_x, "y_m": truth_y},
+            "n_samples":   stats.n_samples,
+            "duration_s":  round(duration, 3),
+            "mean":        {"x_m": round(stats.mean_x, 4),
+                            "y_m": round(stats.mean_y, 4)},
+            "bias":        {"x_m": round(stats.bias_x, 4),
+                            "y_m": round(stats.bias_y, 4)},
+            "std":         {"x_m": round(stats.std_x, 4),
+                            "y_m": round(stats.std_y, 4),
+                            "2d_m": round(stats.std_2d, 4)},
+            "cep50_m":     round(stats.cep50, 4),
+            "cep95_m":     round(stats.cep95, 4),
+            "rmse_m":      round(stats.rmse, 4),
+            "max_error_m": round(stats.max_error, 4),
+            "anchors": {
+                str(aid): {
+                    "range_mean_m": round(ranges_snap[aid].mean, 4),
+                    "range_std_m":  round(ranges_snap[aid].std,  4),
+                    "n":            ranges_snap[aid].count,
+                }
+                for aid in anchor_ids
+            },
+        }
+        try:
+            with open(path, "w") as f:
+                json.dump(data, f, indent=2)
+            self.get_logger().info(f"Report written: {path}")
+        except OSError as exc:
+            self.get_logger().error(f"JSON write failed: {exc}")
+
+
+# ── Helpers ────────────────────────────────────────────────────────────────
+
+def _stamp_ns(stamp) -> int:
+    return stamp.sec * 1_000_000_000 + stamp.nanosec
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = UwbLoggerNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_uwb_logger/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/saltybot_uwb_logger
+[install]
+install_scripts=$base/lib/saltybot_uwb_logger

jetson/ros2_ws/src/saltybot_uwb_logger/setup.py

@@ -0,0 +1,29 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = "saltybot_uwb_logger"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages",
+            [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (os.path.join("share", package_name, "launch"), glob("launch/*.py")),
+        (os.path.join("share", package_name, "config"), glob("config/*.yaml")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="sl-uwb",
+    maintainer_email="sl-uwb@saltylab.local",
+    description="UWB position logger and accuracy analyzer (Issue #634)",
+    license="Apache-2.0",
+    entry_points={
+        "console_scripts": [
+            "uwb_logger = saltybot_uwb_logger.logger_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_uwb_logger/test/test_accuracy_stats.py

@@ -0,0 +1,116 @@
+"""Unit tests for saltybot_uwb_logger.accuracy_stats (Issue #634). No ROS2 needed."""
+
+import math
+import sys
+import os
+
+import numpy as np
+import pytest
+
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
+from saltybot_uwb_logger.accuracy_stats import compute_stats, RangeAccum
+
+
+def _circle(n, r, cx=0.0, cy=0.0):
+    a = np.linspace(0, 2 * math.pi, n, endpoint=False)
+    return cx + r * np.cos(a), cy + r * np.sin(a)
+
+
+class TestComputeStats:
+    def test_zero_error(self):
+        xs = np.full(5, 2.0); ys = np.full(5, 3.0)
+        s = compute_stats(xs, ys, 2.0, 3.0)
+        assert abs(s.cep50) < 1e-9
+        assert abs(s.rmse) < 1e-9
+        assert abs(s.max_error) < 1e-9
+
+    def test_known_bias(self):
+        rng = np.random.default_rng(0)
+        xs = rng.normal(4.0, 0.01, 100); ys = rng.normal(2.0, 0.01, 100)
+        s = compute_stats(xs, ys, 3.0, 2.0)
+        assert abs(s.bias_x - 1.0) < 0.05
+        assert abs(s.bias_y) < 0.05
+
+    def test_cep50_below_cep95(self):
+        rng = np.random.default_rng(1)
+        xs = rng.normal(0, 0.1, 500); ys = rng.normal(0, 0.1, 500)
+        s = compute_stats(xs, ys, 0.0, 0.0)
+        assert s.cep50 < s.cep95
+
+    def test_cep50_is_median(self):
+        rng = np.random.default_rng(2)
+        xs = rng.normal(0, 0.15, 1000); ys = rng.normal(0, 0.15, 1000)
+        errors = np.sqrt(xs**2 + ys**2)
+        s = compute_stats(xs, ys, 0.0, 0.0)
+        assert abs(s.cep50 - float(np.median(errors))) < 1e-9
+
+    def test_rmse(self):
+        xs = np.array([0.1, -0.1, 0.2, -0.2]); ys = np.zeros(4)
+        s = compute_stats(xs, ys, 0.0, 0.0)
+        assert abs(s.rmse - math.sqrt(np.mean(xs**2))) < 1e-9
+
+    def test_max_error(self):
+        xs = np.array([0.0, 0.0, 1.0]); ys = np.zeros(3)
+        s = compute_stats(xs, ys, 0.0, 0.0)
+        assert abs(s.max_error - 1.0) < 1e-9
+
+    def test_circle_symmetry(self):
+        xs, ys = _circle(360, 0.12)
+        s = compute_stats(xs, ys, 0.0, 0.0)
+        assert abs(s.bias_x) < 1e-6
+        assert abs(s.cep50 - 0.12) < 1e-6
+
+    def test_std_2d_pythagorean(self):
+        rng = np.random.default_rng(3)
+        xs = rng.normal(0, 0.1, 200); ys = rng.normal(0, 0.2, 200)
+        s = compute_stats(xs, ys, 0.0, 0.0)
+        assert abs(s.std_2d - math.sqrt(s.std_x**2 + s.std_y**2)) < 1e-9
+
+    def test_n_samples(self):
+        xs = np.ones(42); ys = np.zeros(42)
+        s = compute_stats(xs, ys, 0.0, 0.0)
+        assert s.n_samples == 42
+
+    def test_too_few_raises(self):
+        with pytest.raises(ValueError):
+            compute_stats(np.array([1.0]), np.array([1.0]), 0.0, 0.0)
+
+    def test_length_mismatch_raises(self):
+        with pytest.raises(ValueError):
+            compute_stats(np.array([1.0, 2.0]), np.array([1.0]), 0.0, 0.0)
+
+
+class TestRealistic:
+    def test_10cm_cep50(self):
+        rng = np.random.default_rng(42)
+        xs = rng.normal(3.0, 0.07, 500); ys = rng.normal(2.0, 0.07, 500)
+        s = compute_stats(xs, ys, 3.0, 2.0)
+        assert 0.05 < s.cep50 < 0.20
+
+    def test_biased_uwb(self):
+        rng = np.random.default_rng(7)
+        xs = rng.normal(3.15, 0.05, 300); ys = rng.normal(2.0, 0.05, 300)
+        s = compute_stats(xs, ys, 3.0, 2.0)
+        assert abs(s.bias_x - 0.15) < 0.02
+
+
+class TestRangeAccum:
+    def test_empty(self):
+        a = RangeAccum(0)
+        assert a.mean == 0.0 and a.std == 0.0 and a.count == 0
+
+    def test_single_std_zero(self):
+        a = RangeAccum(0); a.add(2.0)
+        assert a.std == 0.0
+
+    def test_mean_std(self):
+        a = RangeAccum(0)
+        for v in [1.0, 2.0, 3.0, 4.0, 5.0]:
+            a.add(v)
+        assert abs(a.mean - 3.0) < 1e-9
+        assert abs(a.std - float(np.std([1,2,3,4,5], ddof=1))) < 1e-9
+        assert a.count == 5
+
+
+if __name__ == "__main__":
+    pytest.main([__file__, "-v"])

jetson/ros2_ws/src/saltybot_uwb_logger_msgs/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.8)
+project(saltybot_uwb_logger_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/AccuracyReport.msg"
+  "srv/StartAccuracyTest.srv"
+  DEPENDENCIES std_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

jetson/ros2_ws/src/saltybot_uwb_logger_msgs/msg/AccuracyReport.msg

@@ -0,0 +1,32 @@
+# AccuracyReport.msg — UWB positioning accuracy test results (Issue #634)
+#
+# Published on /saltybot/uwb/accuracy_report after StartAccuracyTest completes.
+
+std_msgs/Header header
+
+float64 truth_x_m
+float64 truth_y_m
+
+uint32  n_samples
+
+float64 mean_x_m
+float64 mean_y_m
+
+float64 std_x_m
+float64 std_y_m
+float64 std_2d_m
+
+float64 bias_x_m
+float64 bias_y_m
+
+float64 cep50_m
+float64 cep95_m
+float64 max_error_m
+float64 rmse_m
+
+uint8[]   anchor_ids
+float64[] anchor_range_mean_m
+float64[] anchor_range_std_m
+
+string  test_id
+float64 duration_s

jetson/ros2_ws/src/saltybot_uwb_logger_msgs/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_uwb_logger_msgs</name>
+  <version>0.1.0</version>
+  <description>Message and service definitions for UWB logger (Issue #634).</description>
+  <maintainer email="sl-uwb@saltylab.local">sl-uwb</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <depend>std_msgs</depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_uwb_logger_msgs/srv/StartAccuracyTest.srv

@@ -0,0 +1,15 @@
+# StartAccuracyTest.srv — trigger UWB accuracy test at a known position (Issue #634)
+
+# ── Request ────────────────────────────────────────────────────────────────
+float64 truth_x_m
+float64 truth_y_m
+uint32  n_samples
+float64 timeout_s
+string  test_id
+
+---
+
+# ── Response ───────────────────────────────────────────────────────────────
+bool    success
+string  message
+string  test_id

phone/voice_cmd.py

@@ -0,0 +1,398 @@
+#!/usr/bin/env python3
+"""
+voice_cmd.py — Termux voice command interface for SaltyBot (Issue #633)
+
+Listens for voice commands via termux-speech-to-text (Android built-in STT),
+parses them into structured JSON payloads, and publishes to MQTT.
+Provides TTS confirmation via termux-tts-speak.
+Falls back to manual text input when STT is unavailable.
+
+MQTT Topic
+──────────
+  saltybot/phone/voice_cmd  — published on each recognised command
+
+JSON Payload
+────────────
+  {
+    "ts":      1710000000.000,   # Unix timestamp
+    "cmd":     "go_forward",     # command enum (see COMMANDS below)
+    "param":   null,             # optional parameter (e.g. waypoint name)
+    "raw":     "go forward",     # original recognised text
+    "source":  "stt"             # "stt" | "text"
+  }
+
+Commands
+────────
+  go_forward, go_back, turn_left, turn_right, stop, estop,
+  go_waypoint (param = waypoint name), speed_up, speed_down, status
+
+Usage
+─────
+  python3 phone/voice_cmd.py [OPTIONS]
+
+  --broker HOST     MQTT broker IP/hostname  (default: 192.168.1.100)
+  --port   PORT     MQTT broker port         (default: 1883)
+  --qos    INT      MQTT QoS level 0/1/2     (default: 0)
+  --text            Manual text fallback mode (no STT)
+  --no-tts          Disable TTS confirmation
+  --timeout SECS    STT listen timeout in seconds (default: 10)
+  --debug           Verbose logging
+
+Dependencies (Termux)
+─────────────────────
+  pkg install termux-api python
+  pip install paho-mqtt
+"""
+
+import argparse
+import json
+import logging
+import subprocess
+import sys
+import threading
+import time
+from typing import Optional, Tuple
+
+try:
+    import paho.mqtt.client as mqtt
+    MQTT_AVAILABLE = True
+except ImportError:
+    MQTT_AVAILABLE = False
+
+# ── MQTT topic ────────────────────────────────────────────────────────────────
+
+TOPIC_VOICE_CMD = "saltybot/phone/voice_cmd"
+
+# ── Command table ─────────────────────────────────────────────────────────────
+#
+# Each entry: (command_id, [phrase_fragments, ...])
+# Phrases are matched as case-insensitive substrings of the recognised text.
+# First match wins; order matters for disambiguation.
+
+_CMD_TABLE = [
+    # E-stop must be checked before plain "stop"
+    ("estop",        ["emergency stop", "e stop", "estop", "abort"]),
+    ("stop",         ["stop", "halt", "freeze", "cancel"]),
+    ("go_forward",   ["go forward", "move forward", "forward", "ahead", "advance"]),
+    ("go_back",      ["go back", "move back", "backward", "reverse", "back up"]),
+    ("turn_left",    ["turn left", "rotate left", "left"]),
+    ("turn_right",   ["turn right", "rotate right", "right"]),
+    ("go_waypoint",  ["go to waypoint", "go to", "navigate to", "waypoint"]),
+    ("speed_up",     ["speed up", "faster", "increase speed"]),
+    ("speed_down",   ["slow down", "slower", "decrease speed", "decelerate"]),
+    ("status",       ["status", "report", "where are you", "how are you"]),
+]
+
+# TTS responses per command
+_TTS_RESPONSES = {
+    "go_forward":   "Going forward",
+    "go_back":      "Going back",
+    "turn_left":    "Turning left",
+    "turn_right":   "Turning right",
+    "stop":         "Stopping",
+    "estop":        "Emergency stop!",
+    "go_waypoint":  "Navigating to waypoint",
+    "speed_up":     "Speeding up",
+    "speed_down":   "Slowing down",
+    "status":       "Requesting status",
+}
+
+# ── termux-api helpers ────────────────────────────────────────────────────────
+
+def stt_listen(timeout: float = 10.0) -> Optional[str]:
+    """
+    Call termux-speech-to-text and return the recognised text string, or None.
+
+    termux-speech-to-text returns JSON:
+      {"partial": "...", "text": "final text"}
+    or on failure returns empty / error output.
+    """
+    try:
+        result = subprocess.run(
+            ["termux-speech-to-text"],
+            capture_output=True,
+            text=True,
+            timeout=timeout + 5.0,   # extra buffer for app round-trip
+        )
+        stdout = result.stdout.strip()
+        if not stdout:
+            logging.debug("STT: empty response (rc=%d)", result.returncode)
+            return None
+
+        # termux-speech-to-text may return bare text or JSON
+        try:
+            data = json.loads(stdout)
+            # Prefer "text" (final); fall back to "partial"
+            text = data.get("text") or data.get("partial") or ""
+            text = text.strip()
+        except (json.JSONDecodeError, AttributeError):
+            # Some versions return plain text directly
+            text = stdout
+
+        if not text:
+            logging.debug("STT: no text in response: %r", stdout)
+            return None
+
+        logging.info("STT recognised: %r", text)
+        return text
+
+    except subprocess.TimeoutExpired:
+        logging.warning("STT: timed out after %.1f s", timeout + 5.0)
+        return None
+    except FileNotFoundError:
+        logging.error("STT: termux-speech-to-text not found — install termux-api")
+        return None
+    except Exception as e:
+        logging.warning("STT error: %s", e)
+        return None
+
+
+def tts_speak(text: str) -> None:
+    """Speak @text via termux-tts-speak (fire-and-forget)."""
+    try:
+        subprocess.Popen(
+            ["termux-tts-speak", text],
+            stdout=subprocess.DEVNULL,
+            stderr=subprocess.DEVNULL,
+        )
+    except FileNotFoundError:
+        logging.debug("TTS: termux-tts-speak not found")
+    except Exception as e:
+        logging.debug("TTS error: %s", e)
+
+
+# ── Command parser ────────────────────────────────────────────────────────────
+
+def parse_command(text: str) -> Tuple[Optional[str], Optional[str]]:
+    """
+    Match @text against the command table.
+
+    Returns (command_id, param) where param is non-None only for go_waypoint.
+    Returns (None, None) if no command matched.
+    """
+    lower = text.lower().strip()
+
+    for cmd_id, phrases in _CMD_TABLE:
+        for phrase in phrases:
+            if phrase in lower:
+                param = None
+                if cmd_id == "go_waypoint":
+                    # Extract waypoint name: text after "waypoint" / "go to" / "navigate to"
+                    for marker in ("waypoint", "navigate to", "go to"):
+                        idx = lower.find(marker)
+                        if idx != -1:
+                            remainder = text[idx + len(marker):].strip()
+                            if remainder:
+                                param = remainder
+                            break
+                return cmd_id, param
+
+    return None, None
+
+
+# ── MQTT publisher (same pattern as sensor_dashboard.py) ─────────────────────
+
+class MQTTPublisher:
+    """Thin paho-mqtt wrapper with auto-reconnect."""
+
+    _RECONNECT_BASE = 2.0
+    _RECONNECT_MAX  = 60.0
+
+    def __init__(self, broker: str, port: int, qos: int = 0):
+        self._broker = broker
+        self._port   = port
+        self._qos    = qos
+        self._lock   = threading.Lock()
+        self._connected = False
+
+        self._client = mqtt.Client(client_id="saltybot-voice-cmd", clean_session=True)
+        self._client.on_connect    = self._on_connect
+        self._client.on_disconnect = self._on_disconnect
+        self._client.reconnect_delay_set(
+            min_delay=int(self._RECONNECT_BASE),
+            max_delay=int(self._RECONNECT_MAX),
+        )
+        self._connect()
+
+    def _connect(self) -> None:
+        try:
+            self._client.connect_async(self._broker, self._port, keepalive=60)
+            self._client.loop_start()
+            logging.info("MQTT connecting to %s:%d …", self._broker, self._port)
+        except Exception as e:
+            logging.warning("MQTT connect error: %s", e)
+
+    def _on_connect(self, client, userdata, flags, rc) -> None:
+        if rc == 0:
+            with self._lock:
+                self._connected = True
+            logging.info("MQTT connected to %s:%d", self._broker, self._port)
+        else:
+            logging.warning("MQTT connect failed rc=%d", rc)
+
+    def _on_disconnect(self, client, userdata, rc) -> None:
+        with self._lock:
+            self._connected = False
+        if rc != 0:
+            logging.warning("MQTT disconnected (rc=%d) — paho will retry", rc)
+
+    @property
+    def connected(self) -> bool:
+        with self._lock:
+            return self._connected
+
+    def publish(self, topic: str, payload: dict) -> bool:
+        if not self.connected:
+            logging.debug("MQTT offline — dropping %s", topic)
+            return False
+        try:
+            msg = json.dumps(payload, separators=(",", ":"))
+            info = self._client.publish(topic, msg, qos=self._qos, retain=False)
+            return info.rc == mqtt.MQTT_ERR_SUCCESS
+        except Exception as e:
+            logging.warning("MQTT publish error: %s", e)
+            return False
+
+    def shutdown(self) -> None:
+        self._client.loop_stop()
+        self._client.disconnect()
+        logging.info("MQTT disconnected.")
+
+
+# ── Main listen loop ──────────────────────────────────────────────────────────
+
+def _handle_text(raw: str, source: str, publisher: MQTTPublisher,
+                 no_tts: bool) -> bool:
+    """
+    Parse @raw, publish command, speak confirmation.
+    Returns True if a command was recognised and published.
+    """
+    cmd, param = parse_command(raw)
+    if cmd is None:
+        logging.info("No command matched for: %r", raw)
+        if not no_tts:
+            tts_speak("Sorry, I didn't understand that")
+        return False
+
+    payload = {
+        "ts":     time.time(),
+        "cmd":    cmd,
+        "param":  param,
+        "raw":    raw,
+        "source": source,
+    }
+    ok = publisher.publish(TOPIC_VOICE_CMD, payload)
+    log_msg = "Published %s (param=%r) — MQTT %s" % (cmd, param, "OK" if ok else "FAIL")
+    logging.info(log_msg)
+
+    if not no_tts:
+        response = _TTS_RESPONSES.get(cmd, cmd.replace("_", " "))
+        if param:
+            response = f"{response}: {param}"
+        tts_speak(response)
+
+    return ok
+
+
+def run_stt_loop(publisher: MQTTPublisher, args: argparse.Namespace) -> None:
+    """Continuous STT listen → parse → publish loop."""
+    logging.info("Voice command loop started (STT mode). Say a command.")
+    if not args.no_tts:
+        tts_speak("Voice commands ready")
+
+    consecutive_failures = 0
+
+    while True:
+        logging.info("Listening…")
+        text = stt_listen(timeout=args.timeout)
+
+        if text is None:
+            consecutive_failures += 1
+            logging.warning("STT failed (%d consecutive)", consecutive_failures)
+            if consecutive_failures >= 3:
+                logging.warning("STT unavailable — switch to --text mode if needed")
+                if not args.no_tts:
+                    tts_speak("Speech recognition unavailable")
+                time.sleep(5.0)
+                consecutive_failures = 0
+            continue
+
+        consecutive_failures = 0
+        _handle_text(text, "stt", publisher, args.no_tts)
+
+
+def run_text_loop(publisher: MQTTPublisher, args: argparse.Namespace) -> None:
+    """Manual text input loop (--text / --no-stt mode)."""
+    logging.info("Text input mode. Type a command (Ctrl-C or 'quit' to exit).")
+    print("\nAvailable commands: go forward, go back, turn left, turn right, "
+          "stop, emergency stop, go to waypoint <name>, speed up, slow down, status")
+    print("Type 'quit' to exit.\n")
+
+    while True:
+        try:
+            raw = input("Command> ").strip()
+        except EOFError:
+            break
+
+        if not raw:
+            continue
+        if raw.lower() in ("quit", "exit", "q"):
+            break
+
+        _handle_text(raw, "text", publisher, args.no_tts)
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main() -> None:
+    parser = argparse.ArgumentParser(
+        description="SaltyBot Termux voice command interface (Issue #633)"
+    )
+    parser.add_argument("--broker",  default="192.168.1.100",
+                        help="MQTT broker IP/hostname (default: 192.168.1.100)")
+    parser.add_argument("--port",    type=int, default=1883,
+                        help="MQTT broker port (default: 1883)")
+    parser.add_argument("--qos",     type=int, default=0, choices=[0, 1, 2],
+                        help="MQTT QoS level (default: 0)")
+    parser.add_argument("--text",    action="store_true",
+                        help="Manual text fallback mode (no STT)")
+    parser.add_argument("--no-tts",  action="store_true",
+                        help="Disable TTS confirmation")
+    parser.add_argument("--timeout", type=float, default=10.0,
+                        help="STT listen timeout in seconds (default: 10)")
+    parser.add_argument("--debug",   action="store_true",
+                        help="Verbose logging")
+    args = parser.parse_args()
+
+    logging.basicConfig(
+        level=logging.DEBUG if args.debug else logging.INFO,
+        format="%(asctime)s [%(levelname)s] %(message)s",
+    )
+
+    if not MQTT_AVAILABLE:
+        logging.error("paho-mqtt not installed.  Run: pip install paho-mqtt")
+        sys.exit(1)
+
+    publisher = MQTTPublisher(args.broker, args.port, qos=args.qos)
+
+    # Wait for initial MQTT connection
+    deadline = time.monotonic() + 10.0
+    while not publisher.connected and time.monotonic() < deadline:
+        time.sleep(0.2)
+    if not publisher.connected:
+        logging.warning("MQTT not connected — commands will be dropped until connected")
+
+    try:
+        if args.text:
+            run_text_loop(publisher, args)
+        else:
+            run_stt_loop(publisher, args)
+    except KeyboardInterrupt:
+        logging.info("Shutting down…")
+    finally:
+        publisher.shutdown()
+        logging.info("Done.")
+
+
+if __name__ == "__main__":
+    main()

src/main.c

@@ -36,6 +36,7 @@
 #include "servo_bus.h"
 #include "gimbal.h"
 #include "lvc.h"
+#include "uart_protocol.h"
 #include <math.h>
 #include <string.h>
 #include <stdio.h>
@@ -261,6 +262,9 @@ int main(void) {
     /* Init LVC: low voltage cutoff state machine (Issue #613) */
     lvc_init();
 
+    /* Init UART command protocol for Jetson (UART5 PC12/PD2, 115200 baud, Issue #629) */
+    uart_protocol_init();
+
     /* Probe I2C1 for optional sensors — skip gracefully if not found */
     int baro_chip = 0;  /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */
     mag_type_t mag_type = MAG_NONE;
@@ -293,6 +297,7 @@ int main(void) {
     uint32_t can_cmd_tick   = 0;   /* CAN velocity command TX timer (Issue #597) */
     uint32_t can_tlm_tick   = 0;   /* JLINK_TLM_CAN_STATS transmit timer (Issue #597) */
     uint32_t lvc_tlm_tick   = 0;   /* JLINK_TLM_LVC transmit timer (Issue #613) */
+    uint32_t uart_status_tick = 0;  /* UART protocol STATUS frame timer (Issue #629) */
     uint8_t  pm_pwm_phase   = 0;   /* Software PWM counter for sleep LED */
     const float dt = 1.0f / PID_LOOP_HZ;  /* 1ms at 1kHz */
 
@@ -368,6 +373,9 @@ int main(void) {
         /* CAN bus RX: drain FIFO0 and parse feedback frames (Issue #597) */
         can_driver_process();
 
+        /* UART command protocol RX: parse Jetson frames (Issue #629) */
+        uart_protocol_process();
+
         /* Handle JLink one-shot flags from Jetson binary protocol */
         if (jlink_state.estop_req) {
             jlink_state.estop_req = 0u;
@@ -430,6 +438,37 @@ int main(void) {
                    (double)bal.kp, (double)bal.ki, (double)bal.kd);
         }
 
+        /* UART protocol: handle commands from Jetson (Issue #629) */
+        {
+            if (uart_prot_state.vel_updated) {
+                uart_prot_state.vel_updated = 0u;
+                if (bal.state == BALANCE_ARMED && !lvc_is_cutoff()) {
+                    can_cmd_t ucmd_l = { uart_prot_state.left_rpm,  0 };
+                    can_cmd_t ucmd_r = { uart_prot_state.right_rpm, 0 };
+                    can_driver_send_cmd(CAN_NODE_LEFT,  &ucmd_l);
+                    can_driver_send_cmd(CAN_NODE_RIGHT, &ucmd_r);
+                }
+            }
+            if (uart_prot_state.pid_updated) {
+                uart_prot_state.pid_updated = 0u;
+                bal.kp = uart_prot_state.pid_kp;
+                bal.ki = uart_prot_state.pid_ki;
+                bal.kd = uart_prot_state.pid_kd;
+            }
+            if (uart_prot_state.estop_req) {
+                uart_prot_state.estop_req = 0u;
+                safety_remote_estop(ESTOP_REMOTE);
+                safety_arm_cancel();
+                balance_disarm(&bal);
+                motor_driver_estop(&motors);
+            }
+            if (uart_prot_state.estop_clear_req) {
+                uart_prot_state.estop_clear_req = 0u;
+                if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED)
+                    safety_remote_estop_clear();
+            }
+        }
+
         /* FAULT_LOG_GET: send fault log telemetry to Jetson (Issue #565) */
         if (jlink_state.fault_log_req) {
             jlink_state.fault_log_req = 0u;
@@ -768,6 +807,19 @@ int main(void) {
             jlink_send_lvc_tlm(&ltlm);
         }
 
+        /* UART protocol: send STATUS to Jetson at 10 Hz (Issue #629) */
+        if (now - uart_status_tick >= 100u) {
+            uart_status_tick = now;
+            uart_prot_status_t ups;
+            ups.pitch_x10     = (int16_t)(bal.pitch_deg * 10.0f);
+            ups.motor_cmd     = bal.motor_cmd;
+            uint32_t _uv      = battery_read_mv();
+            ups.vbat_mv       = (_uv > 65535u) ? 65535u : (uint16_t)_uv;
+            ups.balance_state = (uint8_t)bal.state;
+            ups.estop_active  = safety_remote_estop_active() ? 1u : 0u;
+            uart_protocol_send_status(&ups);
+        }
+
         /* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
         if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
             send_tick = now;

src/uart_protocol.c

@@ -0,0 +1,270 @@
+/*
+ * uart_protocol.c — UART command protocol for Jetson-STM32 communication (Issue #629)
+ *
+ * Physical: UART5, PC12 (TX, AF8) / PD2 (RX, AF8), 115200 baud, 8N1, no flow control.
+ * NOTE: Spec requested USART1 @ 115200, but USART1 is already used by JLink @ 921600.
+ *       Implemented on UART5 (PC12/PD2) instead.
+ *
+ * RX: DMA1_Stream0 (Channel 4), 256-byte circular buffer, no interrupt needed.
+ * TX: Polled (HAL_UART_Transmit), frames are short (<20 B) so blocking is acceptable.
+ *
+ * CRC: CRC8-SMBUS — poly 0x07, init 0x00, computed over CMD+PAYLOAD bytes only.
+ */
+
+#include "uart_protocol.h"
+#include "config.h"
+#include "stm32f7xx_hal.h"
+#include <string.h>
+
+/* ── Configuration ─────────────────────────────────────────────────────────── */
+#define RX_BUF_SIZE  256u   /* must be power-of-two for wrap math */
+#define TX_TIMEOUT   5u     /* HAL_UART_Transmit timeout ms */
+
+/* ── Peripheral handles ─────────────────────────────────────────────────────  */
+static UART_HandleTypeDef  huart5;
+static DMA_HandleTypeDef   hdma_rx;
+
+/* ── DMA ring buffer ────────────────────────────────────────────────────────  */
+static uint8_t rx_buf[RX_BUF_SIZE];
+static uint32_t rx_head = 0u;   /* next byte to consume */
+
+/* ── Shared state (read by main.c) ─────────────────────────────────────────  */
+UartProtState uart_prot_state;
+
+/* ── Parser state machine ───────────────────────────────────────────────────  */
+typedef enum {
+    PS_IDLE,
+    PS_LEN,
+    PS_CMD,
+    PS_PAYLOAD,
+    PS_CRC,
+    PS_ETX
+} ParseState;
+
+static ParseState ps        = PS_IDLE;
+static uint8_t    ps_len    = 0u;   /* expected payload bytes */
+static uint8_t    ps_cmd    = 0u;   /* command byte */
+static uint8_t    ps_payload[12];   /* max payload = SET_PID = 12 B */
+static uint8_t    ps_pi     = 0u;   /* payload index */
+static uint8_t    ps_crc    = 0u;   /* received CRC byte */
+
+/* ── CRC8-SMBUS ─────────────────────────────────────────────────────────────  */
+static uint8_t crc8(const uint8_t *data, uint8_t len)
+{
+    uint8_t crc = 0x00u;
+    while (len--) {
+        crc ^= *data++;
+        for (uint8_t i = 0u; i < 8u; i++) {
+            if (crc & 0x80u)
+                crc = (uint8_t)((crc << 1) ^ 0x07u);
+            else
+                crc <<= 1;
+        }
+    }
+    return crc;
+}
+
+/* ── TX helper ──────────────────────────────────────────────────────────────  */
+static void tx_frame(uint8_t cmd, const uint8_t *payload, uint8_t plen)
+{
+    uint8_t frame[20];
+    uint8_t fi = 0u;
+    frame[fi++] = UPROT_STX;
+    frame[fi++] = plen;
+    frame[fi++] = cmd;
+    for (uint8_t i = 0u; i < plen; i++)
+        frame[fi++] = payload[i];
+    /* CRC over CMD + PAYLOAD */
+    frame[fi++] = crc8(&frame[2], (uint8_t)(1u + plen));
+    frame[fi++] = UPROT_ETX;
+    HAL_UART_Transmit(&huart5, frame, fi, TX_TIMEOUT);
+}
+
+static void send_ack(uint8_t cmd)
+{
+    tx_frame(URESP_ACK, &cmd, 1u);
+}
+
+static void send_nack(uint8_t cmd, uint8_t err)
+{
+    uint8_t p[2] = { cmd, err };
+    tx_frame(URESP_NACK, p, 2u);
+}
+
+/* ── Command dispatcher ─────────────────────────────────────────────────────  */
+static void dispatch(uint8_t cmd, const uint8_t *payload, uint8_t plen)
+{
+    /* Validate CRC (computed over cmd + payload) */
+    uint8_t buf[13];
+    buf[0] = cmd;
+    memcpy(&buf[1], payload, plen);
+    if (crc8(buf, (uint8_t)(1u + plen)) != ps_crc) {
+        send_nack(cmd, UERR_BAD_CRC);
+        return;
+    }
+
+    uart_prot_state.last_rx_ms = HAL_GetTick();
+
+    switch (cmd) {
+        case UCMD_SET_VELOCITY:
+            if (plen != 4u) { send_nack(cmd, UERR_BAD_LEN); break; }
+            {
+                int16_t lrpm, rrpm;
+                memcpy(&lrpm, &payload[0], 2u);
+                memcpy(&rrpm, &payload[2], 2u);
+                uart_prot_state.left_rpm    = lrpm;
+                uart_prot_state.right_rpm   = rrpm;
+                uart_prot_state.vel_updated = 1u;
+                send_ack(cmd);
+            }
+            break;
+
+        case UCMD_GET_STATUS:
+            if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; }
+            /* ACK immediately; main.c sends URESP_STATUS at next 10 Hz tick */
+            send_ack(cmd);
+            break;
+
+        case UCMD_SET_PID:
+            if (plen != 12u) { send_nack(cmd, UERR_BAD_LEN); break; }
+            {
+                float kp, ki, kd;
+                memcpy(&kp, &payload[0], 4u);
+                memcpy(&ki, &payload[4], 4u);
+                memcpy(&kd, &payload[8], 4u);
+                uart_prot_state.pid_kp      = kp;
+                uart_prot_state.pid_ki      = ki;
+                uart_prot_state.pid_kd      = kd;
+                uart_prot_state.pid_updated = 1u;
+                send_ack(cmd);
+            }
+            break;
+
+        case UCMD_ESTOP:
+            if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; }
+            uart_prot_state.estop_req = 1u;
+            send_ack(cmd);
+            break;
+
+        case UCMD_CLEAR_ESTOP:
+            if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; }
+            uart_prot_state.estop_clear_req = 1u;
+            send_ack(cmd);
+            break;
+
+        default:
+            send_nack(cmd, UERR_BAD_LEN);
+            break;
+    }
+}
+
+/* ── Parser byte handler ────────────────────────────────────────────────────  */
+static void parse_byte(uint8_t b)
+{
+    switch (ps) {
+        case PS_IDLE:
+            if (b == UPROT_STX) ps = PS_LEN;
+            break;
+
+        case PS_LEN:
+            if (b > 12u) { ps = PS_IDLE; break; }  /* sanity: max payload 12 B */
+            ps_len = b;
+            ps = PS_CMD;
+            break;
+
+        case PS_CMD:
+            ps_cmd = b;
+            ps_pi  = 0u;
+            ps     = (ps_len == 0u) ? PS_CRC : PS_PAYLOAD;
+            break;
+
+        case PS_PAYLOAD:
+            ps_payload[ps_pi++] = b;
+            if (ps_pi >= ps_len) ps = PS_CRC;
+            break;
+
+        case PS_CRC:
+            ps_crc = b;
+            ps     = PS_ETX;
+            break;
+
+        case PS_ETX:
+            if (b == UPROT_ETX)
+                dispatch(ps_cmd, ps_payload, ps_len);
+            else
+                send_nack(ps_cmd, UERR_BAD_ETX);
+            ps = PS_IDLE;
+            break;
+
+        default:
+            ps = PS_IDLE;
+            break;
+    }
+}
+
+/* ── Public API ─────────────────────────────────────────────────────────────  */
+
+void uart_protocol_init(void)
+{
+    memset(&uart_prot_state, 0, sizeof(uart_prot_state));
+    ps = PS_IDLE;
+
+    /* GPIO: PC12 (TX, AF8) and PD2 (RX, AF8) */
+    __HAL_RCC_GPIOC_CLK_ENABLE();
+    __HAL_RCC_GPIOD_CLK_ENABLE();
+    GPIO_InitTypeDef gpio = {0};
+    gpio.Mode      = GPIO_MODE_AF_PP;
+    gpio.Pull      = GPIO_NOPULL;
+    gpio.Speed     = GPIO_SPEED_FREQ_HIGH;
+    gpio.Alternate = GPIO_AF8_UART5;
+    gpio.Pin       = GPIO_PIN_12;
+    HAL_GPIO_Init(GPIOC, &gpio);
+    gpio.Pin       = GPIO_PIN_2;
+    HAL_GPIO_Init(GPIOD, &gpio);
+
+    /* UART5 */
+    __HAL_RCC_UART5_CLK_ENABLE();
+    huart5.Instance          = UART5;
+    huart5.Init.BaudRate     = UART_PROT_BAUD;
+    huart5.Init.WordLength   = UART_WORDLENGTH_8B;
+    huart5.Init.StopBits     = UART_STOPBITS_1;
+    huart5.Init.Parity       = UART_PARITY_NONE;
+    huart5.Init.Mode         = UART_MODE_TX_RX;
+    huart5.Init.HwFlowCtl    = UART_HWCONTROL_NONE;
+    huart5.Init.OverSampling = UART_OVERSAMPLING_16;
+    if (HAL_UART_Init(&huart5) != HAL_OK) return;
+
+    /* DMA1_Stream0, Channel 4 — UART5_RX */
+    __HAL_RCC_DMA1_CLK_ENABLE();
+    hdma_rx.Instance                 = DMA1_Stream0;
+    hdma_rx.Init.Channel             = DMA_CHANNEL_4;
+    hdma_rx.Init.Direction           = DMA_PERIPH_TO_MEMORY;
+    hdma_rx.Init.PeriphInc           = DMA_PINC_DISABLE;
+    hdma_rx.Init.MemInc              = DMA_MINC_ENABLE;
+    hdma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+    hdma_rx.Init.MemDataAlignment    = DMA_MDATAALIGN_BYTE;
+    hdma_rx.Init.Mode                = DMA_CIRCULAR;
+    hdma_rx.Init.Priority            = DMA_PRIORITY_LOW;
+    hdma_rx.Init.FIFOMode            = DMA_FIFOMODE_DISABLE;
+    HAL_DMA_Init(&hdma_rx);
+    __HAL_LINKDMA(&huart5, hdmarx, hdma_rx);
+
+    /* Start circular DMA receive */
+    HAL_UART_Receive_DMA(&huart5, rx_buf, RX_BUF_SIZE);
+}
+
+void uart_protocol_process(void)
+{
+    /* DMA writes forward; NDTR counts down from RX_BUF_SIZE */
+    uint32_t ndtr = __HAL_DMA_GET_COUNTER(&hdma_rx);
+    uint32_t tail = (RX_BUF_SIZE - ndtr) & (RX_BUF_SIZE - 1u);
+    while (rx_head != tail) {
+        parse_byte(rx_buf[rx_head]);
+        rx_head = (rx_head + 1u) & (RX_BUF_SIZE - 1u);
+    }
+}
+
+void uart_protocol_send_status(const uart_prot_status_t *s)
+{
+    tx_frame(URESP_STATUS, (const uint8_t *)s, (uint8_t)sizeof(*s));
+}

ui/dashboard.css

@@ -0,0 +1,429 @@
+/* dashboard.css — Saltybot Main Dashboard (Issue #630) */
+
+*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
+
+:root {
+  --bg0:   #050510;
+  --bg1:   #070712;
+  --bg2:   #0a0a1a;
+  --bd:    #0c2a3a;
+  --bd2:   #1e3a5f;
+  --dim:   #374151;
+  --mid:   #6b7280;
+  --base:  #9ca3af;
+  --hi:    #d1d5db;
+  --cyan:  #06b6d4;
+  --green: #22c55e;
+  --amber: #f59e0b;
+  --red:   #ef4444;
+}
+
+html, body {
+  height: 100%;
+  font-family: 'Courier New', Courier, monospace;
+  font-size: 12px;
+  background: var(--bg0);
+  color: var(--base);
+  display: flex;
+  flex-direction: column;
+  overflow: hidden;
+}
+
+/* ══════════════════ TOP BAR ══════════════════ */
+#topbar {
+  display: flex;
+  align-items: center;
+  gap: 10px;
+  padding: 6px 14px;
+  background: var(--bg1);
+  border-bottom: 1px solid var(--bd);
+  flex-shrink: 0;
+  flex-wrap: wrap;
+}
+
+#topbar-left {
+  display: flex;
+  align-items: baseline;
+  gap: 8px;
+  flex-shrink: 0;
+}
+.logo {
+  color: #f97316;
+  font-weight: bold;
+  letter-spacing: .18em;
+  font-size: 14px;
+}
+#robot-name {
+  color: var(--mid);
+  font-size: 10px;
+  letter-spacing: .12em;
+}
+
+#topbar-status {
+  display: flex;
+  align-items: center;
+  gap: 0;
+  flex: 1;
+  min-width: 0;
+  flex-wrap: wrap;
+  gap: 2px;
+}
+
+.stat-block {
+  display: flex;
+  align-items: center;
+  gap: 5px;
+  padding: 2px 10px;
+}
+.stat-icon { font-size: 14px; flex-shrink: 0; }
+.stat-body { display: flex; flex-direction: column; gap: 1px; }
+.stat-label {
+  font-size: 8px;
+  color: var(--dim);
+  letter-spacing: .1em;
+  text-transform: uppercase;
+}
+.stat-val {
+  font-size: 11px;
+  color: var(--hi);
+  font-family: monospace;
+  white-space: nowrap;
+}
+.stat-sep {
+  width: 1px;
+  height: 28px;
+  background: var(--bd);
+  flex-shrink: 0;
+}
+
+/* Battery bar */
+.batt-bar {
+  width: 28px;
+  height: 12px;
+  border: 1px solid var(--bd2);
+  border-radius: 2px;
+  overflow: hidden;
+  position: relative;
+  background: var(--bg0);
+}
+.batt-bar::after {
+  content: '';
+  position: absolute;
+  right: -3px; top: 3px;
+  width: 3px; height: 6px;
+  background: var(--bd2);
+  border-radius: 0 1px 1px 0;
+}
+#batt-fill {
+  height: 100%;
+  width: 0%;
+  background: var(--green);
+  transition: width .5s, background .5s;
+}
+
+#topbar-right {
+  display: flex;
+  align-items: center;
+  gap: 6px;
+  flex-shrink: 0;
+  margin-left: auto;
+}
+
+#conn-dot {
+  width: 8px; height: 8px; border-radius: 50%;
+  background: var(--dim); flex-shrink: 0;
+  transition: background .3s;
+}
+#conn-dot.connected { background: var(--green); }
+#conn-dot.error     { background: var(--red); animation: blink 1s infinite; }
+
+#conn-label {
+  font-size: 10px;
+  color: var(--mid);
+  white-space: nowrap;
+}
+
+#ws-input {
+  background: var(--bg2); border: 1px solid var(--bd2); border-radius: 4px;
+  color: #67e8f9; padding: 2px 7px; font-family: monospace; font-size: 11px;
+  width: 175px;
+}
+#ws-input:focus { outline: none; border-color: var(--cyan); }
+
+.hbtn {
+  padding: 2px 8px; border-radius: 4px; border: 1px solid var(--bd2);
+  background: var(--bg2); color: #67e8f9; font-family: monospace;
+  font-size: 10px; font-weight: bold; cursor: pointer; transition: background .15s;
+  white-space: nowrap;
+}
+.hbtn:hover { background: #0e4f69; }
+
+@keyframes blink { 0%,100%{opacity:1} 50%{opacity:.3} }
+@keyframes pulse { 0%,100%{opacity:1} 50%{opacity:.5} }
+
+/* ══════════════════ MAIN / GRID ══════════════════ */
+#main {
+  flex: 1;
+  display: flex;
+  flex-direction: column;
+  overflow-y: auto;
+  padding: 16px;
+  gap: 12px;
+  min-height: 0;
+}
+
+/* Auto-detect bar */
+#detect-bar {
+  display: flex;
+  align-items: center;
+  gap: 10px;
+  padding: 6px 12px;
+  background: var(--bg2);
+  border: 1px solid var(--bd2);
+  border-radius: 6px;
+  font-size: 10px;
+  color: var(--mid);
+  flex-shrink: 0;
+}
+#detect-bar.connected {
+  border-color: #14532d;
+  color: var(--green);
+  background: #020f06;
+}
+#detect-bar.error {
+  border-color: #7f1d1d;
+  color: var(--red);
+}
+#detect-status { flex: 1; }
+#detect-dots { display: flex; gap: 4px; }
+.detect-dot {
+  width: 6px; height: 6px; border-radius: 50%;
+  background: var(--dim);
+  transition: background .3s;
+}
+.detect-dot.trying { background: var(--amber); animation: pulse .7s infinite; }
+.detect-dot.ok     { background: var(--green); }
+.detect-dot.fail   { background: var(--red); }
+
+/* Panel grid */
+#grid {
+  display: grid;
+  grid-template-columns: repeat(auto-fill, minmax(280px, 1fr));
+  gap: 12px;
+}
+
+/* Panel card */
+.panel-card {
+  display: flex;
+  flex-direction: column;
+  gap: 8px;
+  padding: 14px;
+  background: var(--bg2);
+  border: 1px solid var(--bd2);
+  border-radius: 10px;
+  text-decoration: none;
+  color: inherit;
+  cursor: pointer;
+  transition: border-color .2s, background .2s, transform .1s;
+  position: relative;
+  overflow: hidden;
+}
+.panel-card::before {
+  content: '';
+  position: absolute;
+  inset: 0;
+  background: radial-gradient(ellipse at top left, rgba(6,182,212,.04), transparent 60%);
+  pointer-events: none;
+}
+.panel-card:hover {
+  border-color: var(--cyan);
+  background: #0a0a1e;
+  transform: translateY(-1px);
+}
+.panel-card:active { transform: translateY(0); }
+
+.card-header {
+  display: flex;
+  align-items: center;
+  gap: 10px;
+}
+.card-icon { font-size: 24px; flex-shrink: 0; }
+.card-title {
+  font-size: 12px;
+  font-weight: bold;
+  letter-spacing: .12em;
+  color: var(--hi);
+  text-transform: uppercase;
+}
+.card-sub { font-size: 9px; color: var(--dim); }
+.card-dot {
+  width: 9px; height: 9px; border-radius: 50%;
+  background: var(--dim);
+  margin-left: auto;
+  flex-shrink: 0;
+  transition: background .3s;
+}
+.card-dot.live   { background: var(--green); }
+.card-dot.idle   { background: var(--amber); }
+.card-dot.config { background: var(--mid); }
+
+.card-desc {
+  font-size: 10px;
+  color: var(--mid);
+  line-height: 1.5;
+}
+
+.card-topics {
+  display: flex;
+  flex-wrap: wrap;
+  gap: 4px;
+}
+.card-topics code {
+  font-size: 8px;
+  color: #4b5563;
+  background: var(--bg0);
+  border: 1px solid var(--bd);
+  border-radius: 2px;
+  padding: 1px 4px;
+}
+
+.card-footer {
+  display: flex;
+  align-items: center;
+  justify-content: space-between;
+  border-top: 1px solid var(--bd);
+  padding-top: 6px;
+  margin-top: auto;
+}
+.card-status {
+  font-size: 9px;
+  font-weight: bold;
+  letter-spacing: .1em;
+  color: var(--dim);
+}
+.card-status.live   { color: var(--green); }
+.card-status.idle   { color: var(--amber); }
+.card-msg { font-size: 9px; color: var(--dim); font-family: monospace; }
+
+/* Estop active state on card */
+.panel-card.estop-flash {
+  border-color: var(--red) !important;
+  animation: blink .6s infinite;
+}
+
+/* Info strip */
+#info-strip {
+  display: flex;
+  align-items: center;
+  gap: 0;
+  padding: 6px 12px;
+  background: var(--bg2);
+  border: 1px solid var(--bd);
+  border-radius: 6px;
+  flex-shrink: 0;
+  flex-wrap: wrap;
+}
+.info-item {
+  display: flex;
+  align-items: center;
+  gap: 6px;
+  padding: 0 14px;
+  border-right: 1px solid var(--bd);
+}
+.info-item:first-child { padding-left: 0; }
+.info-item:last-child  { border-right: none; }
+.info-lbl { font-size: 8px; color: var(--dim); letter-spacing: .1em; }
+.info-val { font-size: 10px; color: var(--hi); font-family: monospace; }
+
+/* ══════════════════ BOTTOM BAR ══════════════════ */
+#bottombar {
+  display: flex;
+  align-items: center;
+  padding: 6px 14px;
+  background: var(--bg1);
+  border-top: 1px solid var(--bd);
+  flex-shrink: 0;
+  gap: 12px;
+  flex-wrap: wrap;
+}
+
+#bottombar-left { display: flex; gap: 8px; align-items: center; }
+
+.estop-btn {
+  padding: 5px 18px;
+  border-radius: 5px;
+  border: 2px solid #7f1d1d;
+  background: #1c0505;
+  color: #fca5a5;
+  font-family: monospace;
+  font-size: 11px;
+  font-weight: bold;
+  cursor: pointer;
+  letter-spacing: .08em;
+  transition: background .15s, border-color .15s;
+}
+.estop-btn:hover { background: #3b0606; border-color: var(--red); }
+.estop-btn.active {
+  background: #7f1d1d;
+  border-color: var(--red);
+  color: #fff;
+  animation: blink .7s infinite;
+}
+
+.resume-btn {
+  padding: 5px 14px;
+  border-radius: 5px;
+  border: 1px solid #14532d;
+  background: #052010;
+  color: #86efac;
+  font-family: monospace;
+  font-size: 11px;
+  font-weight: bold;
+  cursor: pointer;
+  transition: background .15s;
+}
+.resume-btn:hover { background: #0a3a1a; }
+
+#bottombar-center {
+  display: flex;
+  align-items: center;
+  gap: 8px;
+  flex: 1;
+}
+.bb-lbl { font-size: 9px; color: var(--dim); letter-spacing: .1em; flex-shrink: 0; }
+
+#mode-btns { display: flex; gap: 4px; }
+.mode-btn {
+  padding: 3px 10px;
+  border-radius: 4px;
+  border: 1px solid var(--bd2);
+  background: var(--bg2);
+  color: var(--mid);
+  font-family: monospace;
+  font-size: 10px;
+  font-weight: bold;
+  cursor: pointer;
+  letter-spacing: .08em;
+  transition: background .15s, color .15s, border-color .15s;
+}
+.mode-btn:hover   { background: #0e4f69; color: var(--hi); }
+.mode-btn.active  { background: #0e4f69; border-color: var(--cyan); color: #fff; }
+
+#bottombar-right {
+  display: flex;
+  align-items: center;
+  gap: 10px;
+  margin-left: auto;
+}
+#uptime-display { display: flex; align-items: center; gap: 6px; }
+#session-time   { font-size: 11px; color: var(--mid); font-family: monospace; }
+
+/* ══════════════════ RESPONSIVE ══════════════════ */
+@media (max-width: 700px) {
+  #topbar-status { display: none; }
+  #grid { grid-template-columns: 1fr 1fr; }
+}
+@media (max-width: 480px) {
+  #grid { grid-template-columns: 1fr; }
+  #mode-btns .mode-btn:nth-child(n+3) { display: none; }
+}

ui/dashboard.js

@@ -0,0 +1,480 @@
+/* dashboard.js — Saltybot Main Dashboard (Issue #630) */
+'use strict';
+
+// ── Auto-detect candidates ─────────────────────────────────────────────────
+// Try current page's hostname first (if served from robot), then localhost
+const AUTO_CANDIDATES = (() => {
+  const candidates = [];
+  if (location.hostname && location.hostname !== '' && location.hostname !== 'localhost') {
+    candidates.push(`ws://${location.hostname}:9090`);
+  }
+  candidates.push('ws://localhost:9090');
+  candidates.push('ws://saltybot.local:9090');
+  return candidates;
+})();
+
+// ── Panel definitions (status tracking) ───────────────────────────────────
+const PANELS = [
+  { id: 'map',      watchTopic: '/saltybot/pose/fused',           msgType: 'geometry_msgs/PoseStamped' },
+  { id: 'gamepad',  watchTopic: null,                              msgType: null },  // output only
+  { id: 'diag',     watchTopic: '/diagnostics',                    msgType: 'diagnostic_msgs/DiagnosticArray' },
+  { id: 'events',   watchTopic: '/rosout',                         msgType: 'rcl_interfaces/msg/Log' },
+  { id: 'settings', watchTopic: null,                              msgType: null },  // service-based
+  { id: 'gimbal',   watchTopic: '/gimbal/state',                   msgType: 'geometry_msgs/Vector3' },
+];
+
+// ── State ──────────────────────────────────────────────────────────────────
+const state = {
+  connected:   false,
+  estop:       false,
+  driveMode:   'MANUAL',
+  battery:     null,   // 0..100 %
+  voltage:     null,
+  safetyState: null,   // 'CLEAR' | 'WARN' | 'DANGER' | 'ESTOP'
+  closestM:    null,
+  uptime:      0,      // seconds since ROS connection
+  sessionStart: null,  // Date for session timer
+  msgCount:    0,
+  lastMsgTs:   0,
+  msgRate:     0,
+  latency:     null,
+  // Per-panel last-message timestamps
+  panelTs: {},
+};
+
+// ── ROS ────────────────────────────────────────────────────────────────────
+let ros = null;
+let cmdVelTopic = null;
+let modeTopic = null;
+let uptimeInterval = null;
+let panelWatches = {};   // id → ROSLIB.Topic
+let pingTs = 0;
+let pingTimeout = null;
+
+function connect(url) {
+  if (ros) {
+    Object.values(panelWatches).forEach(t => { try { t.unsubscribe(); } catch(_){} });
+    panelWatches = {};
+    try { ros.close(); } catch(_){}
+  }
+
+  ros = new ROSLIB.Ros({ url });
+
+  ros.on('connection', () => {
+    state.connected = true;
+    state.sessionStart = state.sessionStart || new Date();
+    localStorage.setItem('dashboard_ws_url', url);
+    $connDot.className = 'connected';
+    $connLabel.style.color = '#22c55e';
+    $connLabel.textContent = 'Connected';
+    $wsInput.value = url;
+    document.getElementById('info-ws').textContent = url;
+
+    setupTopics();
+    schedulePing();
+    updateDetectBar('connected', `Connected to ${url}`);
+  });
+
+  ros.on('error', () => {
+    state.connected = false;
+    $connDot.className = 'error';
+    $connLabel.style.color = '#ef4444';
+    $connLabel.textContent = 'Error';
+    updateDetectBar('error', `Error connecting to ${url}`);
+  });
+
+  ros.on('close', () => {
+    state.connected = false;
+    $connDot.className = '';
+    $connLabel.style.color = '#6b7280';
+    $connLabel.textContent = 'Disconnected';
+  });
+}
+
+function setupTopics() {
+  // ── Battery & temps from /diagnostics ──
+  const diagTopic = new ROSLIB.Topic({
+    ros, name: '/diagnostics',
+    messageType: 'diagnostic_msgs/DiagnosticArray',
+    throttle_rate: 2000,
+  });
+  diagTopic.subscribe(msg => {
+    state.msgCount++;
+    state.lastMsgTs = performance.now();
+    (msg.status || []).forEach(s => {
+      (s.values || []).forEach(kv => {
+        if (kv.key === 'battery_voltage_v') {
+          state.voltage = parseFloat(kv.value);
+          // 4S LiPo: 12.0V empty, 16.8V full
+          state.battery = Math.max(0, Math.min(100,
+            ((state.voltage - 12.0) / (16.8 - 12.0)) * 100));
+        }
+        if (kv.key === 'battery_soc_pct') {
+          state.battery = parseFloat(kv.value);
+        }
+      });
+    });
+    markPanelLive('diag');
+    renderBattery();
+  });
+
+  // ── Safety zone ──
+  const safetyTopic = new ROSLIB.Topic({
+    ros, name: '/saltybot/safety_zone/status',
+    messageType: 'std_msgs/String',
+    throttle_rate: 500,
+  });
+  safetyTopic.subscribe(msg => {
+    state.msgCount++;
+    try {
+      const d = JSON.parse(msg.data);
+      state.safetyState = d.state || d.fwd_zone || 'CLEAR';
+      state.closestM    = d.closest_m != null ? d.closest_m : null;
+      if (d.estop !== undefined) {
+        // only auto-latch E-stop if ROS says it's active and we aren't tracking it locally
+        if (d.estop && !state.estop) activateEstop(false);
+      }
+    } catch(_) {
+      state.safetyState = msg.data;
+    }
+    renderSafety();
+  });
+
+  // ── Balance state / drive mode ──
+  const modeSub = new ROSLIB.Topic({
+    ros, name: '/saltybot/balance_state',
+    messageType: 'std_msgs/String',
+    throttle_rate: 1000,
+  });
+  modeSub.subscribe(msg => {
+    try {
+      const d = JSON.parse(msg.data);
+      if (d.mode) {
+        state.driveMode = d.mode.toUpperCase();
+        renderMode();
+      }
+    } catch(_) {}
+  });
+
+  // ── Pose (for map card liveness) ──
+  const poseTopic = new ROSLIB.Topic({
+    ros, name: '/saltybot/pose/fused',
+    messageType: 'geometry_msgs/PoseStamped',
+    throttle_rate: 1000,
+  });
+  poseTopic.subscribe(() => { markPanelLive('map'); });
+
+  // ── /rosout (for events card) ──
+  const rosoutTopic = new ROSLIB.Topic({
+    ros, name: '/rosout',
+    messageType: 'rcl_interfaces/msg/Log',
+    throttle_rate: 2000,
+  });
+  rosoutTopic.subscribe(() => { markPanelLive('events'); });
+
+  // ── Gimbal state ──
+  const gimbalTopic = new ROSLIB.Topic({
+    ros, name: '/gimbal/state',
+    messageType: 'geometry_msgs/Vector3',
+    throttle_rate: 1000,
+  });
+  gimbalTopic.subscribe(() => { markPanelLive('gimbal'); });
+
+  // ── cmd_vel monitor (for gamepad card liveness) ──
+  const cmdVelWatch = new ROSLIB.Topic({
+    ros, name: '/cmd_vel',
+    messageType: 'geometry_msgs/Twist',
+    throttle_rate: 500,
+  });
+  cmdVelWatch.subscribe(() => { markPanelLive('gamepad'); });
+
+  // ── Publisher topics ──
+  cmdVelTopic = new ROSLIB.Topic({
+    ros, name: '/cmd_vel',
+    messageType: 'geometry_msgs/Twist',
+  });
+
+  modeTopic = new ROSLIB.Topic({
+    ros, name: '/saltybot/drive_mode',
+    messageType: 'std_msgs/String',
+  });
+
+  // ── Robot uptime from /diagnostics or fallback to /rosapi ──
+  const upSvc = new ROSLIB.Service({
+    ros, name: '/rosapi/get_time',
+    serviceType: 'rosapi/GetTime',
+  });
+  upSvc.callService({}, () => {
+    state.uptime = 0;
+    if (uptimeInterval) clearInterval(uptimeInterval);
+    uptimeInterval = setInterval(() => { state.uptime++; renderUptime(); }, 1000);
+  });
+}
+
+// ── E-stop ─────────────────────────────────────────────────────────────────
+function activateEstop(sendCmd = true) {
+  state.estop = true;
+  if (sendCmd && cmdVelTopic) {
+    cmdVelTopic.publish(new ROSLIB.Message({
+      linear: { x: 0, y: 0, z: 0 }, angular: { x: 0, y: 0, z: 0 },
+    }));
+  }
+  document.getElementById('btn-estop').style.display  = 'none';
+  document.getElementById('btn-resume').style.display = '';
+  document.getElementById('btn-estop').classList.add('active');
+  document.getElementById('val-estop').textContent  = 'ACTIVE';
+  document.getElementById('val-estop').style.color  = '#ef4444';
+  // Flash all cards
+  document.querySelectorAll('.panel-card').forEach(c => c.classList.add('estop-flash'));
+}
+
+function resumeFromEstop() {
+  state.estop = false;
+  document.getElementById('btn-estop').style.display  = '';
+  document.getElementById('btn-resume').style.display = 'none';
+  document.getElementById('btn-estop').classList.remove('active');
+  document.getElementById('val-estop').textContent = 'OFF';
+  document.getElementById('val-estop').style.color = '#6b7280';
+  document.querySelectorAll('.panel-card').forEach(c => c.classList.remove('estop-flash'));
+}
+
+document.getElementById('btn-estop').addEventListener('click',  () => activateEstop(true));
+document.getElementById('btn-resume').addEventListener('click', resumeFromEstop);
+
+// Space bar = emergency E-stop from dashboard
+document.addEventListener('keydown', e => {
+  if (e.code === 'Space' && e.target.tagName !== 'INPUT') {
+    e.preventDefault();
+    if (state.estop) resumeFromEstop(); else activateEstop(true);
+  }
+});
+
+// ── Drive mode buttons ─────────────────────────────────────────────────────
+document.querySelectorAll('.mode-btn').forEach(btn => {
+  btn.addEventListener('click', () => {
+    state.driveMode = btn.dataset.mode;
+    if (modeTopic) {
+      modeTopic.publish(new ROSLIB.Message({ data: state.driveMode }));
+    }
+    renderMode();
+  });
+});
+
+// ── Panel liveness tracking ────────────────────────────────────────────────
+const LIVE_TIMEOUT_MS = 5000;
+
+function markPanelLive(id) {
+  state.panelTs[id] = Date.now();
+  renderPanelCard(id);
+}
+
+function renderPanelCard(id) {
+  const ts = state.panelTs[id];
+  const dot = document.getElementById(`dot-${id}`);
+  const statusEl = document.getElementById(`status-${id}`);
+  const msgEl    = document.getElementById(`msg-${id}`);
+  if (!dot || !statusEl) return;
+
+  if (!state.connected) {
+    dot.className = 'card-dot';
+    statusEl.className = 'card-status';
+    statusEl.textContent = 'OFFLINE';
+    if (msgEl) msgEl.textContent = 'Not connected';
+    return;
+  }
+
+  // Settings card is always config
+  if (id === 'settings') return;
+  // Gamepad is output only — show "READY" when connected
+  if (id === 'gamepad') {
+    const age = ts ? Date.now() - ts : Infinity;
+    if (age < LIVE_TIMEOUT_MS) {
+      dot.className = 'card-dot live';
+      statusEl.className = 'card-status live';
+      statusEl.textContent = 'ACTIVE';
+      if (msgEl) msgEl.textContent = 'Receiving /cmd_vel';
+    } else {
+      dot.className = 'card-dot';
+      statusEl.className = 'card-status';
+      statusEl.textContent = 'READY';
+      if (msgEl) msgEl.textContent = 'Awaiting input';
+    }
+    return;
+  }
+
+  if (!ts) {
+    dot.className = 'card-dot idle';
+    statusEl.className = 'card-status idle';
+    statusEl.textContent = 'IDLE';
+    if (msgEl) msgEl.textContent = 'No messages yet';
+    return;
+  }
+
+  const age = Date.now() - ts;
+  if (age < LIVE_TIMEOUT_MS) {
+    dot.className = 'card-dot live';
+    statusEl.className = 'card-status live';
+    statusEl.textContent = 'LIVE';
+    if (msgEl) msgEl.textContent = `${(age / 1000).toFixed(1)}s ago`;
+  } else {
+    dot.className = 'card-dot idle';
+    statusEl.className = 'card-status idle';
+    statusEl.textContent = 'IDLE';
+    if (msgEl) msgEl.textContent = `${Math.floor(age / 1000)}s ago`;
+  }
+}
+
+// ── Render helpers ─────────────────────────────────────────────────────────
+function renderBattery() {
+  const el   = document.getElementById('val-battery');
+  const fill = document.getElementById('batt-fill');
+  if (state.battery === null) { el.textContent = '—'; return; }
+  const pct = state.battery.toFixed(0);
+  el.textContent = `${pct}%${state.voltage ? ' · ' + state.voltage.toFixed(1) + 'V' : ''}`;
+  fill.style.width = pct + '%';
+  fill.style.background = pct > 50 ? '#22c55e' : pct > 20 ? '#f59e0b' : '#ef4444';
+  el.style.color = pct > 50 ? '#d1d5db' : pct > 20 ? '#f59e0b' : '#ef4444';
+}
+
+function renderSafety() {
+  const el = document.getElementById('val-safety');
+  if (!state.safetyState) { el.textContent = '—'; return; }
+  const s = state.safetyState.toUpperCase();
+  el.textContent = s + (state.closestM != null ? ` · ${state.closestM.toFixed(2)}m` : '');
+  el.style.color = s === 'DANGER' || s === 'ESTOP' ? '#ef4444' :
+                   s === 'WARN'                    ? '#f59e0b' : '#22c55e';
+}
+
+function renderMode() {
+  document.getElementById('val-mode').textContent = state.driveMode;
+  document.querySelectorAll('.mode-btn').forEach(b => {
+    b.classList.toggle('active', b.dataset.mode === state.driveMode);
+  });
+}
+
+function renderUptime() {
+  const el = document.getElementById('val-uptime');
+  if (!state.connected) { el.textContent = '—'; return; }
+  const s = state.uptime;
+  el.textContent = s < 60 ? `${s}s` : s < 3600 ? `${Math.floor(s/60)}m ${s%60}s` : `${Math.floor(s/3600)}h ${Math.floor((s%3600)/60)}m`;
+}
+
+function renderSession() {
+  if (!state.sessionStart) return;
+  const secs = Math.floor((Date.now() - state.sessionStart) / 1000);
+  const h = String(Math.floor(secs / 3600)).padStart(2, '0');
+  const m = String(Math.floor((secs % 3600) / 60)).padStart(2, '0');
+  const s = String(secs % 60).padStart(2, '0');
+  document.getElementById('session-time').textContent = `${h}:${m}:${s}`;
+}
+
+function renderMsgRate() {
+  const now = performance.now();
+  if (state.lastMsgTs) {
+    const dt = (now - state.lastMsgTs) / 1000;
+    document.getElementById('info-rate').textContent =
+      dt < 10 ? Math.round(1 / dt) + ' msg/s' : '—';
+  }
+}
+
+// ── Latency ping ───────────────────────────────────────────────────────────
+function schedulePing() {
+  if (pingTimeout) clearTimeout(pingTimeout);
+  pingTimeout = setTimeout(() => {
+    if (!ros || !state.connected) return;
+    const svc = new ROSLIB.Service({ ros, name: '/rosapi/get_time', serviceType: 'rosapi/GetTime' });
+    pingTs = performance.now();
+    svc.callService({}, () => {
+      state.latency = Math.round(performance.now() - pingTs);
+      document.getElementById('info-latency').textContent = state.latency + ' ms';
+      document.getElementById('info-ip').textContent = $wsInput.value.replace('ws://','').split(':')[0];
+      schedulePing();
+    }, () => { schedulePing(); });
+  }, 5000);
+}
+
+// ── Auto-detect ────────────────────────────────────────────────────────────
+const $detectStatus = document.getElementById('detect-status');
+const $detectDots   = document.getElementById('detect-dots');
+const $detectBar    = document.getElementById('detect-bar');
+
+function updateDetectBar(cls, msg) {
+  $detectBar.className = cls;
+  $detectStatus.textContent = msg;
+}
+
+function buildDetectDots() {
+  $detectDots.innerHTML = AUTO_CANDIDATES.map((_, i) =>
+    `<div class="detect-dot" id="ddot-${i}"></div>`
+  ).join('');
+}
+
+function tryAutoDetect(idx) {
+  if (idx >= AUTO_CANDIDATES.length) {
+    updateDetectBar('error', '✗ Auto-detect failed — enter URL manually');
+    return;
+  }
+  const url = AUTO_CANDIDATES[idx];
+  const dot = document.getElementById(`ddot-${idx}`);
+  if (dot) dot.className = 'detect-dot trying';
+  updateDetectBar('', `🔍 Trying ${url}…`);
+
+  const testRos = new ROSLIB.Ros({ url });
+  const timer = setTimeout(() => {
+    testRos.close();
+    if (dot) dot.className = 'detect-dot fail';
+    tryAutoDetect(idx + 1);
+  }, 2000);
+
+  testRos.on('connection', () => {
+    clearTimeout(timer);
+    if (dot) dot.className = 'detect-dot ok';
+    // Mark remaining as skipped
+    for (let j = idx + 1; j < AUTO_CANDIDATES.length; j++) {
+      const d = document.getElementById(`ddot-${j}`);
+      if (d) d.className = 'detect-dot';
+    }
+    testRos.close();
+    // Connect for real
+    connect(url);
+  });
+
+  testRos.on('error', () => {
+    clearTimeout(timer);
+    if (dot) dot.className = 'detect-dot fail';
+    tryAutoDetect(idx + 1);
+  });
+}
+
+// ── Connection bar ─────────────────────────────────────────────────────────
+const $connDot   = document.getElementById('conn-dot');
+const $connLabel = document.getElementById('conn-label');
+const $wsInput   = document.getElementById('ws-input');
+
+document.getElementById('btn-connect').addEventListener('click', () => {
+  connect($wsInput.value.trim() || 'ws://localhost:9090');
+});
+
+// ── Liveness refresh loop ──────────────────────────────────────────────────
+setInterval(() => {
+  PANELS.forEach(p => renderPanelCard(p.id));
+  renderMsgRate();
+  renderSession();
+}, 1000);
+
+// ── Session timer ──────────────────────────────────────────────────────────
+state.sessionStart = new Date();
+setInterval(renderSession, 1000);
+
+// ── Init ───────────────────────────────────────────────────────────────────
+buildDetectDots();
+
+// Restore saved URL or auto-detect
+const savedUrl = localStorage.getItem('dashboard_ws_url');
+if (savedUrl) {
+  $wsInput.value = savedUrl;
+  updateDetectBar('', `⟳ Reconnecting to ${savedUrl}…`);
+  connect(savedUrl);
+} else {
+  tryAutoDetect(0);
+}