feat: CAN bus driver for BLDC motor controllers (Issue #597)

- Add can_driver.h / can_driver.c: CAN2 on PB12/PB13 (AF9) at 500 kbps
  APB1=54 MHz, PSC=6, BS1=13tq, BS2=4tq, SJW=1tq → 18tq/bit, SP 77.8%
  Filter bank 14 (SlaveStartFilterBank=14); 32-bit mask; FIFO0
  Accept std IDs 0x200–0x21F (left/right feedback frames)
  TX: velocity+torque cmd (0x100+nid, DLC=4) at 100 Hz via main loop
  RX: velocity/current/position/temp/fault feedback (0x200+nid, DLC=8)
  AutoBusOff=ENABLE for HW recovery; can_driver_process() drains FIFO0
- Add JLINK_TLM_CAN_STATS (0x89, 16 bytes) + JLINK_CMD_CAN_STATS_GET (0x10)
  Also add JLINK_TLM_SLOPE (0x88) + jlink_tlm_slope_t missing from Issue #600
- Wire into main.c: init after jlink_init; 100Hz TX loop (differential drive
  speed_rpm ± steer_rpm/2); CAN enable follows arm state; 1Hz stats telemetry
- Add CAN_RPM_SCALE=10 and CAN_TLM_HZ=1 to config.h

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

chassis/rplidar_mount.scad

@@ -1,502 +1,343 @@
 // ============================================================
-// rplidar_mount.scad — RPLIDAR A1 Elevated Bracket for 2020 T-Slot Rail
-// Issue: #561  Agent: sl-mechanical  Date: 2026-03-14
-// (supersedes Rev A anti-vibration ring — ring integrated as Part 4)
-// ============================================================
-//
-// Complete elevated mount system for RPLIDAR A1 on 2020 aluminium T-slot
-// rail.  Scanner raised ELEV_H mm above rail attachment point so the
-// 360° laser scan plane clears the rover/tank chassis body.
-//
-// Architecture:
-//   T-nut base  → clamps to 2020 rail (standard thumbscrew interface)
-//   Column      → parametric-height hollow mast; USB cable routed inside
-//   Platform    → disc receives RPLIDAR via 4× M3 on Ø40 mm bolt circle
-//   Vibe ring   → anti-vibration isolation ring with silicone grommet seats
-//   Cable guide → snap-on clips along column for USB cable management
-//
+// RPLIDAR A1 Mount Bracket — Issue #596
+// Agent   : sl-mechanical
+// Date    : 2026-03-14
 // Part catalogue:
-//   Part 1 — tnut_base()      2020 T-nut rail base + column stub socket
-//   Part 2 — column()         Hollow elevation mast (parametric ELEV_H)
-//   Part 3 — scan_platform()  RPLIDAR mounting disc + motor connector slot
-//   Part 4 — vibe_ring()      Anti-vibration isolation ring (grommet seats)
-//   Part 5 — cable_guide()    Snap-on cable management clip for column
-//   Part 6 — assembly_preview()
+//   1. tnut_base     — 2020 T-slot rail interface plate with M5 T-nut captive pockets
+//   2. column        — hollow elevation column, 120 mm tall, 3 stiffening ribs, cable bore
+//   3. scan_platform — top plate with Ø40 mm BC M3 mounting pattern, vibration seats
+//   4. vibe_ring     — silicone FC-grommet isolation ring for scan_platform bolts
+//   5. cable_guide   — snap-on cable management clip for column body
 //
-// Hardware BOM (per mount):
-//   1× M3 × 16 mm SHCS + M3 hex nut   rail clamp thumbscrew
-//   4× M3 × 30 mm SHCS                RPLIDAR → vibe_ring → platform
-//   4× M3 silicone grommets (Ø6 mm)   anti-vibration isolators
-//   4× M3 hex nuts                    captured in platform underside
-//   2× M4 × 12 mm SHCS                column → base socket bolts
-//   2× M4 hex nuts                    captured in base socket
-//   1× USB-A cable (RPLIDAR → Jetson) routed through column bore
+// BOM:
+//   2 × M5×10 BHCS + M5 T-nuts   (tnut_base to rail)
+//   4 × M3×8  SHCS                (scan_platform to RPLIDAR A1)
+//   4 × M3 silicone FC grommets Ø8.5 OD / Ø3.2 bore  (anti-vibe)
+//   4 × M3 hex nuts               (captured in scan_platform)
 //
-// RPLIDAR A1 interface (caliper-verified Slamtec RPLIDAR A1):
-//   Body diameter     : Ø70 mm
-//   Bolt circle       : Ø40 mm, 4× M3, at 45°/135°/225°/315°
-//   USB connector     : micro-USB, right-rear quadrant, exits at 0° (front)
-//   Motor connector   : JST 2-pin, rear centreline
-//   Scan plane height : 19 mm above bolt mounting face
-//   Min clearance     : Ø80 mm cylinder around body for 360° scan
-//
-// Parametric constants (override for variants):
-//   ELEV_H     — scan elevation above rail face (default 120 mm)
-//   COL_OD     — column outer diameter (default 25 mm)
-//   RAIL choice — RAIL_W = 20 for 2020, = 40 for 4040 extrusion
-//
-// Print settings:
-//   Material   : PETG (all parts); vibe_ring optionally in TPU 95A
-//   Perimeters : 5 (tnut_base, column, platform), 3 (vibe_ring, cable_guide)
-//   Infill     : 40 % gyroid (structural), 20 % (vibe_ring, guide)
-//   Orientation:
-//     tnut_base    — face-plate flat on bed (no supports)
-//     column       — standing upright (no supports; hollow bore bridgeable)
-//     scan_platform — disc face down (no supports)
-//     vibe_ring    — flat on bed (no supports)
-//     cable_guide  — clip-open face down (no supports)
+// Print settings (PETG):
+//   tnut_base / column / scan_platform : 5 perimeters, 40 % gyroid, no supports
+//   vibe_ring                          : 3 perimeters, 20 % gyroid, no supports
+//   cable_guide                        : 3 perimeters, 30 % gyroid, no supports
 //
 // Export commands:
-//   openscad rplidar_mount.scad -D 'RENDER="tnut_base_stl"'    -o rpm_tnut_base.stl
-//   openscad rplidar_mount.scad -D 'RENDER="column_stl"'       -o rpm_column.stl
-//   openscad rplidar_mount.scad -D 'RENDER="platform_stl"'     -o rpm_platform.stl
-//   openscad rplidar_mount.scad -D 'RENDER="vibe_ring_stl"'    -o rpm_vibe_ring.stl
-//   openscad rplidar_mount.scad -D 'RENDER="cable_guide_stl"'  -o rpm_cable_guide.stl
+//   openscad -D 'RENDER="tnut_base"'     -o tnut_base.stl     rplidar_mount.scad
+//   openscad -D 'RENDER="column"'        -o column.stl        rplidar_mount.scad
+//   openscad -D 'RENDER="scan_platform"' -o scan_platform.stl rplidar_mount.scad
+//   openscad -D 'RENDER="vibe_ring"'     -o vibe_ring.stl     rplidar_mount.scad
+//   openscad -D 'RENDER="cable_guide"'   -o cable_guide.stl   rplidar_mount.scad
+//   openscad -D 'RENDER="assembly"'      -o assembly.png      rplidar_mount.scad
 // ============================================================
 
+// ── Render selector ─────────────────────────────────────────
+RENDER = "assembly"; // tnut_base | column | scan_platform | vibe_ring | cable_guide | assembly
+
+// ── Global constants ────────────────────────────────────────
 $fn = 64;
-e   = 0.01;
+EPS = 0.01;
 
-// ── Parametric elevation ──────────────────────────────────────────────────────
-ELEV_H     = 120.0;   // scan plane elevation above rail face (mm)
-                      // increase for taller chassis; min ~60 mm recommended
+// 2020 rail
+RAIL_W       = 20.0;   // extrusion cross-section
+RAIL_H       = 20.0;
+SLOT_NECK_H  =  3.2;   // T-slot opening width
+TNUT_W       =  9.8;   // M5 T-nut width
+TNUT_H       =  5.5;   // T-nut height (depth into slot)
+TNUT_L       = 12.0;   // T-nut body length
+M5_D         =  5.2;   // M5 clearance bore
+M5_HEAD_D    =  9.5;   // M5 BHCS head diameter
+M5_HEAD_H    =  4.0;   // M5 BHCS head height
 
-// ── RPLIDAR A1 interface constants ───────────────────────────────────────────
-RPL_BODY_D = 70.0;    // scanner body outer diameter
-RPL_BC_D   = 40.0;    // mounting bolt circle diameter (4× M3 at 45° offsets)
-RPL_BOLT_D =  3.3;    // M3 clearance bore
-RPL_SCAN_Z = 19.0;    // scan plane height above mount face
-RPL_CLEAR_D = 82.0;   // minimum radial clearance diameter for 360° scan
+// Base plate
+BASE_L       = 60.0;   // length along rail axis
+BASE_W       = 30.0;   // width across rail
+BASE_T       =  8.0;   // plate thickness
+BOLT_PITCH   = 40.0;   // M5 bolt pitch along rail (centre-to-centre)
 
-// ── Rail geometry (matches sensor_rail.scad) ─────────────────────────────────
-RAIL_W       = 20.0;
-SLOT_OPEN    =  6.0;
-SLOT_INNER_W = 10.2;
-SLOT_INNER_H =  5.8;
-SLOT_NECK_H  =  3.2;
-
-// ── T-nut geometry (matches sensor_rail_brackets.scad) ───────────────────────
-TNUT_W         =  9.8;
-TNUT_H         =  5.5;
-TNUT_L         = 12.0;
-TNUT_M3_NUT_AF =  5.5;
-TNUT_M3_NUT_H  =  2.5;
-TNUT_BOLT_D    =  3.3;
-
-// ── Base plate geometry ───────────────────────────────────────────────────────
-BASE_FACE_W = 38.0;   // wider than rail, provides column socket footprint
-BASE_FACE_H = 38.0;   // height along rail Z
-BASE_FACE_T = SLOT_NECK_H + 2.0;   // plate depth (Y)
-
-// ── Column geometry ───────────────────────────────────────────────────────────
+// Elevation column
 COL_OD       = 25.0;   // column outer diameter
-COL_ID      = 17.0;   // column inner bore (cable routing + weight saving)
-COL_SOCKET_D = COL_OD + 6.0;   // socket boss OD (column inserts into base)
-COL_SOCKET_L = 14.0;  // socket depth in base (14 mm engagement)
-COL_BOLT_BC  = COL_OD + 4.0;   // M4 column-lock bolt span (centre-to-centre)
-COL_SLOT_W   =  5.0;  // cable exit slot width in column base
-COL_SLOT_H   =  8.0;  // cable exit slot height
+COL_ID       = 17.0;   // inner bore (cable routing)
+ELEV_H       = 120.0;  // scan plane above rail top face
+COL_WALL     = (COL_OD - COL_ID) / 2;
+RIB_W        =  3.0;   // stiffening rib width
+RIB_H        =  3.5;   // rib radial height
+CABLE_SLOT_W =  8.0;   // cable entry slot width
+CABLE_SLOT_H =  5.0;   // cable entry slot height
 
-// ── Platform geometry ─────────────────────────────────────────────────────────
-PLAT_OD      = RPL_CLEAR_D + 4.0;   // platform disc OD (covers scan clear zone)
-PLAT_T       =  5.0;   // platform disc thickness
-PLAT_SOCKET_D = COL_OD + 0.3;       // column-top socket ID (slip fit)
-PLAT_SOCKET_L =  12.0; // socket depth on platform underside
-PLAT_RIM_T   =  3.5;   // rim wall thickness around RPLIDAR body
+// Scan platform
+PLAT_D       = 60.0;   // platform disc diameter (clears RPLIDAR body Ø100 mm well)
+PLAT_T       =  6.0;   // platform thickness
+RPL_BC_D     = 40.0;   // RPLIDAR M3 bolt circle diameter (4 bolts at 45 °)
+RPL_BORE_D   = 36.0;   // central pass-through for scan motor cable
+M3_D         =  3.2;   // M3 clearance bore
+M3_NUT_W     =  5.5;   // M3 hex nut across-flats
+M3_NUT_H     =  2.4;   // M3 hex nut height
+GROM_OD      =  8.5;   // FC silicone grommet OD
+GROM_ID      =  3.2;   // grommet bore
+GROM_H       =  3.0;   // grommet seat depth
+CONN_SLOT_W  = 12.0;   // connector side-exit slot width
+CONN_SLOT_H  =  5.0;   // connector slot height
 
-// ── Anti-vibration ring geometry ─────────────────────────────────────────────
-RING_OD      = RPL_BODY_D + 12.0;   // 82 mm (body + 6 mm rim)
-RING_ID      = 28.0;   // central bore (connector/cable access)
-RING_H       =  4.0;   // ring thickness
-GROMMET_D    =  7.0;   // silicone grommet OD pocket
-GROMMET_RECESS =  1.5; // grommet seating recess depth (bottom face)
+// Vibe ring
+VRING_OD     = GROM_OD + 1.6;  // printed retainer OD
+VRING_ID     = GROM_ID + 0.3;  // pass-through with grommet seated
+VRING_T      =  2.0;            // ring flange thickness
 
-// ── Cable guide clip geometry ─────────────────────────────────────────────────
-GUIDE_CABLE_D =  6.0;  // max cable OD (USB-A cable)
-GUIDE_T       =  2.0;  // clip wall thickness
-GUIDE_BODY_W  = 20.0;  // clip body width
-GUIDE_BODY_H  = 12.0;  // clip body height
+// Cable guide clip
+CLIP_W       = 14.0;
+CLIP_T       =  3.5;
+CLIP_GAP     = COL_OD + 0.4;   // snap-fit gap (slight interference)
+SNAP_T       =  1.8;
+CABLE_CH_W   =  8.0;
+CABLE_CH_H   =  5.0;
 
-// ── Fastener sizes ────────────────────────────────────────────────────────────
-M3_D = 3.3;
-M4_D = 4.3;
-M3_NUT_AF = 5.5;
-M3_NUT_H  = 2.4;
-M4_NUT_AF = 7.0;
-M4_NUT_H  = 3.2;
-
-// ============================================================
-// RENDER DISPATCH
-// ============================================================
-RENDER = "assembly";
-
-if      (RENDER == "assembly")       assembly_preview();
-else if (RENDER == "tnut_base_stl")  tnut_base();
-else if (RENDER == "column_stl")     column();
-else if (RENDER == "platform_stl")   scan_platform();
-else if (RENDER == "vibe_ring_stl")  vibe_ring();
-else if (RENDER == "cable_guide_stl") cable_guide();
-
-// ============================================================
-// ASSEMBLY PREVIEW
-// ============================================================
-module assembly_preview() {
-    // Ghost 2020 rail section (250 mm)
-    %color("Silver", 0.28)
-        translate([-RAIL_W/2, -RAIL_W/2, 0])
-            cube([RAIL_W, RAIL_W, 250]);
-
-    // T-nut base at Z=60 on rail
-    color("OliveDrab", 0.85)
-        translate([0, 0, 60])
-            tnut_base();
-
-    // Column rising from base
-    color("SteelBlue", 0.85)
-        translate([0, BASE_FACE_T + COL_OD/2, 60 + BASE_FACE_H/2])
-            column();
-
-    // Vibe ring on top of platform
-    color("Teal", 0.85)
-        translate([0, BASE_FACE_T + COL_OD/2,
-                   60 + BASE_FACE_H/2 + ELEV_H + PLAT_T])
-            vibe_ring();
-
-    // Scan platform at column top
-    color("DarkSlateGray", 0.85)
-        translate([0, BASE_FACE_T + COL_OD/2,
-                   60 + BASE_FACE_H/2 + ELEV_H])
-            scan_platform();
-
-    // RPLIDAR body ghost
-    %color("Black", 0.35)
-        translate([0, BASE_FACE_T + COL_OD/2,
-                   60 + BASE_FACE_H/2 + ELEV_H + PLAT_T + RING_H + 1])
-            cylinder(d = RPL_BODY_D, h = 30);
-
-    // Cable guides at 30 mm intervals along column
-    for (gz = [20, 50, 80])
-        color("DimGray", 0.75)
-            translate([COL_OD/2,
-                       BASE_FACE_T + COL_OD/2,
-                       60 + BASE_FACE_H/2 + gz])
-                rotate([0, -90, 0])
-                    cable_guide();
+// ── Utility modules ─────────────────────────────────────────
+module chamfer_cube(size, ch=1.0) {
+    // simple chamfered box (bottom edge only for printability)
+    hull() {
+        translate([ch, ch, 0])
+            cube([size[0]-2*ch, size[1]-2*ch, EPS]);
+        translate([0, 0, ch])
+            cube(size - [0, 0, ch]);
+    }
 }
 
-// ============================================================
-// PART 1 — T-NUT RAIL BASE
-// ============================================================
-// Standard 2020 rail T-nut attachment, matching interface used across
-// all SaltyLab sensor brackets (sensor_rail_brackets.scad convention).
-// Column socket boss on front face (+Y) receives column bottom.
-// Column locked with 2× M4 cross-bolts through socket boss.
-//
-// Cable exit slot at base of socket directs RPLIDAR USB cable
-// downward and rearward toward Jetson USB port.
-//
-// Print: face-plate flat on bed, PETG, 5 perims, 50 % gyroid.
+module hex_pocket(af, depth) {
+    // hex nut pocket (flat-to-flat af)
+    cylinder(d = af / cos(30), h = depth, $fn = 6);
+}
+
+// ── Part 1: tnut_base ───────────────────────────────────────
 module tnut_base() {
     difference() {
+        // Body
         union() {
-            // ── Face plate (flush against rail outer face, -Y) ───────────
-            translate([-BASE_FACE_W/2, -BASE_FACE_T, 0])
-                cube([BASE_FACE_W, BASE_FACE_T, BASE_FACE_H]);
-
-            // ── T-nut neck (enters rail slot) ────────────────────────────
-            translate([-TNUT_W/2, 0, (BASE_FACE_H - TNUT_L)/2])
-                cube([TNUT_W, SLOT_NECK_H + e, TNUT_L]);
-
-            // ── T-nut body (wider, locks in T-groove) ────────────────────
-            translate([-TNUT_W/2, SLOT_NECK_H - e, (BASE_FACE_H - TNUT_L)/2])
-                cube([TNUT_W, TNUT_H - SLOT_NECK_H + e, TNUT_L]);
-
-            // ── Column socket boss (front face, centred) ─────────────────
-            translate([0, -BASE_FACE_T, BASE_FACE_H/2])
-                rotate([-90, 0, 0])
-                    cylinder(d = COL_SOCKET_D, h = BASE_FACE_T + COL_SOCKET_L);
+            chamfer_cube([BASE_L, BASE_W, BASE_T], ch=1.5);
+            // Column socket boss centred on plate top face
+            translate([BASE_L/2, BASE_W/2, BASE_T])
+                cylinder(d=COL_OD + 4.0, h=8.0);
         }
 
-        // ── Rail clamp bolt bore (M3, centre of face plate) ──────────────
-        translate([0, -BASE_FACE_T - e, BASE_FACE_H/2])
-            rotate([-90, 0, 0])
-                cylinder(d = TNUT_BOLT_D, h = BASE_FACE_T + TNUT_H + 2*e);
+        // M5 bolt holes (counterbored for BHCS heads from underneath)
+        for (x = [BASE_L/2 - BOLT_PITCH/2, BASE_L/2 + BOLT_PITCH/2])
+            translate([x, BASE_W/2, -EPS]) {
+                cylinder(d=M5_D, h=BASE_T + 8.0 + 2*EPS);
+                // counterbore from bottom
+                cylinder(d=M5_HEAD_D, h=M5_HEAD_H + EPS);
+            }
 
-        // ── M3 hex nut pocket (inside T-nut body) ────────────────────────
-        translate([0, SLOT_NECK_H + 0.3, BASE_FACE_H/2])
-            rotate([-90, 0, 0])
-                cylinder(d = TNUT_M3_NUT_AF / cos(30),
-                         h = TNUT_M3_NUT_H + 0.3, $fn = 6);
+        // T-nut captive pockets (accessible from bottom)
+        for (x = [BASE_L/2 - BOLT_PITCH/2, BASE_L/2 + BOLT_PITCH/2])
+            translate([x - TNUT_L/2, BASE_W/2 - TNUT_W/2, BASE_T - TNUT_H])
+                cube([TNUT_L, TNUT_W, TNUT_H + EPS]);
 
-        // ── Column socket bore (column inserts from +Y side) ─────────────
-        translate([0, -BASE_FACE_T, BASE_FACE_H/2])
-            rotate([-90, 0, 0])
-                cylinder(d = COL_OD + 0.3, h = BASE_FACE_T + COL_SOCKET_L + e);
+        // Column bore into boss
+        translate([BASE_L/2, BASE_W/2, BASE_T - EPS])
+            cylinder(d=COL_OD + 0.3, h=8.0 + 2*EPS);
 
-        // ── Column lock bolt bores (2× M4, horizontal through socket boss) ─
-        // One bolt from +X, one from -X, on COL_SOCKET_L/2 depth
-        for (lx = [-1, 1])
-            translate([lx * (COL_SOCKET_D/2 + e), COL_SOCKET_L/2, BASE_FACE_H/2])
-                rotate([0, 90, 0])
-                    cylinder(d = M4_D, h = COL_SOCKET_D + 2*e,
-                             center = true);
+        // Cable exit slot through base (offset 5 mm from column centre)
+        translate([BASE_L/2 - CABLE_SLOT_W/2, BASE_W/2 + COL_OD/4, -EPS])
+            cube([CABLE_SLOT_W, CABLE_SLOT_H, BASE_T + 8.0 + 2*EPS]);
 
-        // ── M4 nut pockets (one side of socket boss for each bolt) ────────
-        for (lx = [-1, 1])
-            translate([lx * (COL_SOCKET_D/2 - M4_NUT_H - 1),
-                       COL_SOCKET_L/2,
-                       BASE_FACE_H/2])
-                rotate([0, 90, 0])
-                    cylinder(d = M4_NUT_AF / cos(30),
-                             h = M4_NUT_H + 0.5, $fn = 6);
-
-        // ── Cable exit slot (bottom of socket, cable exits downward) ──────
-        translate([0, COL_SOCKET_L * 0.6, BASE_FACE_H/2 - COL_SOCKET_D/2])
-            cube([COL_SLOT_W, COL_SOCKET_D + e, COL_SLOT_H], center = [true, false, false]);
-
-        // ── Lightening pockets in face plate ─────────────────────────────
-        translate([0, -BASE_FACE_T/2, BASE_FACE_H/2])
-            cube([BASE_FACE_W - 12, BASE_FACE_T - 2, BASE_FACE_H - 16],
-                 center = true);
+        // Weight relief pockets on underside
+        for (x = [BASE_L/2 - BOLT_PITCH/2 + 10, BASE_L/2 + BOLT_PITCH/2 - 10])
+            for (y = [7, BASE_W - 7])
+                translate([x - 5, y - 5, -EPS])
+                    cube([10, 10, BASE_T/2]);
     }
 }
 
-// ============================================================
-// PART 2 — ELEVATION COLUMN
-// ============================================================
-// Hollow cylindrical mast (ELEV_H tall) raising the RPLIDAR scan
-// plane above the chassis body for unobstructed 360° coverage.
-// Inner bore routes USB cable from scanner to base exit slot.
-// Bottom peg inserts into tnut_base socket; top peg inserts into
-// scan_platform socket.  Both ends are plain Ø(COL_OD) cylinders,
-// interference-free slip fit into Ø(COL_OD+0.3) sockets.
-//
-// Three longitudinal ribs on outer surface add torsional stiffness
-// without added diameter.  Cable slot on one rib for cable retention.
-//
-// Print: standing upright, PETG, 5 perims, 20 % gyroid (hollow).
+// ── Part 2: column ──────────────────────────────────────────
 module column() {
-    rib_w = 3.0;
-    rib_h = 2.0;   // rib protrusion from column OD
+    // Actual column height: ELEV_H minus base boss engagement (8 mm) and platform seating (6 mm)
+    col_h = ELEV_H - 8.0 - PLAT_T;
 
     difference() {
         union() {
-            // ── Hollow cylinder ───────────────────────────────────────────
-            cylinder(d = COL_OD, h = ELEV_H + COL_SOCKET_L);
+            // Hollow tube
+            cylinder(d=COL_OD, h=col_h);
 
-            // ── Three stiffening ribs (120° apart) ────────────────────────
-            for (ra = [0, 120, 240])
-                rotate([0, 0, ra])
-                    translate([COL_OD/2 - e, -rib_w/2, 0])
-                        cube([rib_h + e, rib_w, ELEV_H + COL_SOCKET_L]);
+            // Three 120°-spaced stiffening ribs along full height
+            for (a = [0, 120, 240])
+                rotate([0, 0, a])
+                    translate([COL_OD/2 - EPS, -RIB_W/2, 0])
+                        cube([RIB_H, RIB_W, col_h]);
+
+            // Bottom spigot (fits into base boss bore)
+            translate([0, 0, -6.0])
+                cylinder(d=COL_OD - 0.4, h=6.0 + EPS);
+
+            // Top spigot (seats into scan_platform recess)
+            translate([0, 0, col_h - EPS])
+                cylinder(d=COL_OD - 0.4, h=6.0);
         }
 
-        // ── Central cable bore (full length) ─────────────────────────────
-        translate([0, 0, -e])
-            cylinder(d = COL_ID, h = ELEV_H + COL_SOCKET_L + 2*e);
+        // Inner cable bore
+        translate([0, 0, -6.0 - EPS])
+            cylinder(d=COL_ID, h=col_h + 12.0 + 2*EPS);
 
-        // ── Cable entry slot at column base (aligns with base exit slot) ──
-        translate([-COL_SLOT_W/2, COL_OD/2 - e, -e])
-            cube([COL_SLOT_W, COL_ID/2 + rib_h + 2, COL_SLOT_H + 2]);
+        // Cable entry slot at bottom (aligns with base slot)
+        translate([-CABLE_SLOT_W/2, -COL_OD/2 - EPS, 2.0])
+            cube([CABLE_SLOT_W, CABLE_SLOT_H + EPS, CABLE_SLOT_H]);
 
-        // ── Cable exit slot at column top (USB exits to scanner) ──────────
-        translate([-COL_SLOT_W/2, COL_OD/2 - e,
-                   ELEV_H + COL_SOCKET_L - COL_SLOT_H - 2])
-            cube([COL_SLOT_W, COL_ID/2 + rib_h + 2, COL_SLOT_H + 2]);
+        // Cable exit slot at top (90° rotated for tidy routing)
+        rotate([0, 0, 90])
+            translate([-CABLE_SLOT_W/2, -COL_OD/2 - EPS, col_h - CABLE_SLOT_H - 4.0])
+                cube([CABLE_SLOT_W, CABLE_SLOT_H + EPS, CABLE_SLOT_H]);
 
-        // ── Column lock flat (prevents rotation in socket) ────────────────
-        // Two opposed flats at column base & top socket peg
-        for (peg_z = [0, ELEV_H]) {
-            translate([-COL_OD/2 - e, COL_OD/2 - 2.0, peg_z])
-                cube([COL_OD + 2*e, 2.5, COL_SOCKET_L]);
+        // Cable clip snap groove (at mid-height)
+        translate([0, 0, col_h / 2])
+            difference() {
+                cylinder(d=COL_OD + 2*RIB_H + 0.8, h=4.0, center=true);
+                cylinder(d=COL_OD - 0.2, h=4.0 + 2*EPS, center=true);
             }
     }
 }
 
-// ============================================================
-// PART 3 — SCAN PLATFORM
-// ============================================================
-// Disc that RPLIDAR A1 mounts to.  Matches RPLIDAR A1 bolt pattern:
-// 4× M3 on Ø40 mm bolt circle at 45°/135°/225°/315°.
-// M3 hex nuts captured in underside pockets (blind, tool-free install).
-// Column-top socket on underside receives column top peg (Ø25 slip fit).
-// Motor connector slot on rear edge for JST cable exit.
-// Vibe ring sits on top face between platform and RPLIDAR (separate part).
-//
-// Scan plane (19 mm above mount face) clears platform top by design;
-// minimum platform OD = RPL_CLEAR_D (82 mm) leaves scan plane open.
-//
-// Print: disc face down, PETG, 5 perims, 40 % gyroid.
+// ── Part 3: scan_platform ───────────────────────────────────
 module scan_platform() {
     difference() {
         union() {
-            // ── Platform disc ─────────────────────────────────────────────
-            cylinder(d = PLAT_OD, h = PLAT_T);
+            // Main disc
+            cylinder(d=PLAT_D, h=PLAT_T);
 
-            // ── Column socket boss (underside, -Z) ────────────────────────
-            translate([0, 0, -PLAT_SOCKET_L])
-                cylinder(d = COL_SOCKET_D, h = PLAT_SOCKET_L + e);
+            // Rim lip for stiffness
+            translate([0, 0, PLAT_T])
+                difference() {
+                    cylinder(d=PLAT_D, h=2.0);
+                    cylinder(d=PLAT_D - 4.0, h=2.0 + EPS);
+                }
         }
 
-        // ── Column socket bore (column top peg inserts from below) ────────
-        translate([0, 0, -PLAT_SOCKET_L - e])
-            cylinder(d = PLAT_SOCKET_D, h = PLAT_SOCKET_L + e + 1);
+        // Central cable pass-through
+        translate([0, 0, -EPS])
+            cylinder(d=RPL_BORE_D, h=PLAT_T + 4.0);
 
-        // ── Column lock bores (2× M4 through socket boss) ─────────────────
-        for (lx = [-1, 1])
-            translate([lx * (COL_SOCKET_D/2 + e), 0, -PLAT_SOCKET_L/2])
-                rotate([0, 90, 0])
-                    cylinder(d = M4_D, h = COL_SOCKET_D + 2*e, center = true);
+        // Column spigot socket (bottom recess)
+        translate([0, 0, -EPS])
+            cylinder(d=COL_OD - 0.4 + 0.4, h=6.0);
 
-        // ── M4 nut pockets (one side socket boss) ─────────────────────────
-        translate([COL_SOCKET_D/2 - M4_NUT_H - 1, 0, -PLAT_SOCKET_L/2])
-            rotate([0, 90, 0])
-                cylinder(d = M4_NUT_AF / cos(30), h = M4_NUT_H + 0.5,
-                         $fn = 6);
-
-        // ── 4× RPLIDAR mounting bolt holes (M3, Ø40 mm BC at 45°) ────────
+        // RPLIDAR M3 mounting holes — 4× on Ø40 BC at 45°/135°/225°/315°
         for (a = [45, 135, 225, 315])
-            translate([RPL_BC_D/2 * cos(a),
-                       RPL_BC_D/2 * sin(a), -e])
-                cylinder(d = RPL_BOLT_D, h = PLAT_T + 2*e);
+            rotate([0, 0, a])
+                translate([RPL_BC_D/2, 0, -EPS]) {
+                    // Through bore
+                    cylinder(d=M3_D, h=PLAT_T + 2*EPS);
+                    // Grommet seat (countersunk from top)
+                    translate([0, 0, PLAT_T - GROM_H])
+                        cylinder(d=GROM_OD + 0.3, h=GROM_H + EPS);
+                    // Captured M3 hex nut pocket (from bottom)
+                    translate([0, 0, 1.5])
+                        hex_pocket(M3_NUT_W + 0.3, M3_NUT_H + 0.2);
+                }
 
-        // ── M3 hex nut pockets on underside (captured, tool-free) ─────────
-        for (a = [45, 135, 225, 315])
-            translate([RPL_BC_D/2 * cos(a),
-                       RPL_BC_D/2 * sin(a), -e])
-                cylinder(d = M3_NUT_AF / cos(30),
-                         h = M3_NUT_H + 0.5, $fn = 6);
+        // Connector side-exit slots (2× opposing, at 0° and 180°)
+        for (a = [0, 180])
+            rotate([0, 0, a])
+                translate([-CONN_SLOT_W/2, PLAT_D/2 - CONN_SLOT_H, -EPS])
+                    cube([CONN_SLOT_W, CONN_SLOT_H + EPS, PLAT_T + 2*EPS]);
 
-        // ── Motor connector slot (JST rear centreline, 10×6 mm) ──────────
-        translate([0, PLAT_OD/2 - 8, -e])
-            cube([10, 10, PLAT_T + 2*e], center = [true, false, false]);
-
-        // ── USB connector slot (micro-USB, right-rear, 12×6 mm) ──────────
-        translate([PLAT_OD/4, PLAT_OD/2 - 8, -e])
-            cube([12, 10, PLAT_T + 2*e], center = [true, false, false]);
-
-        // ── Lightening pockets (between bolt holes) ────────────────────────
-        for (a = [0, 90, 180, 270])
-            translate([(RPL_BC_D/2 + 10) * cos(a),
-                       (RPL_BC_D/2 + 10) * sin(a), -e])
-                cylinder(d = 8, h = PLAT_T + 2*e);
-
-        // ── Central cable bore (USB from scanner routes down column) ──────
-        translate([0, 0, -e])
-            cylinder(d = COL_ID - 2, h = PLAT_T + 2*e);
+        // Weight relief pockets (2× lateral)
+        for (a = [90, 270])
+            rotate([0, 0, a])
+                translate([-10, 15, 1.5])
+                    cube([20, 8, PLAT_T - 3.0]);
     }
 }
 
-// ============================================================
-// PART 4 — VIBRATION ISOLATION RING
-// ============================================================
-// Flat ring sits between scan_platform top face and RPLIDAR bottom.
-// Anti-vibration isolation via 4× M3 silicone FC-style grommets
-// (Ø6 mm silicone, M3 bore — same type used on flight controllers).
-//
-// Bolt stack (bottom → top):
-//   M3 × 30 SHCS → platform (countersunk) → grommet (Ø7 seat) →
-//   ring (4 mm) → RPLIDAR threaded boss (~6 mm engagement)
-//
-// Grommet seats are recessed 1.5 mm into ring bottom face so grommets
-// are captured and self-locating.  Ring top face is flat for RPLIDAR.
-//
-// Print: flat on bed, PETG or TPU 95A, 3 perims, 20 % infill.
-// TPU 95A provides additional compliance in axial direction.
+// ── Part 4: vibe_ring ───────────────────────────────────────
+// Printed silicone-grommet retainer ring — press-fits over M3 bolt with grommet seated
 module vibe_ring() {
     difference() {
-        // ── Ring body ────────────────────────────────────────────────────
-        cylinder(d = RING_OD, h = RING_H);
-
-        // ── Central bore (cable / connector access) ───────────────────────
-        translate([0, 0, -e])
-            cylinder(d = RING_ID, h = RING_H + 2*e);
-
-        // ── 4× M3 clearance bores on Ø40 mm bolt circle ───────────────────
-        for (a = [45, 135, 225, 315])
-            translate([RPL_BC_D/2 * cos(a),
-                       RPL_BC_D/2 * sin(a), -e])
-                cylinder(d = RPL_BOLT_D, h = RING_H + 2*e);
-
-        // ── Grommet seating recesses (bottom face, Ø7 mm × 1.5 mm deep) ──
-        for (a = [45, 135, 225, 315])
-            translate([RPL_BC_D/2 * cos(a),
-                       RPL_BC_D/2 * sin(a), -e])
-                cylinder(d = GROMMET_D, h = GROMMET_RECESS + e);
-
-        // ── Motor connector notch (rear centreline, passes through ring) ──
-        translate([0, RING_OD/2 - 6, -e])
-            cube([10, 8, RING_H + 2*e], center = [true, false, false]);
-
-        // ── Lightening arcs ───────────────────────────────────────────────
-        for (a = [0, 90, 180, 270])
-            translate([(RPL_BC_D/2 + 9) * cos(a),
-                       (RPL_BC_D/2 + 9) * sin(a), -e])
-                cylinder(d = 7, h = RING_H + 2*e);
+        union() {
+            cylinder(d=VRING_OD, h=VRING_T + GROM_H);
+            // Flange
+            cylinder(d=VRING_OD + 2.0, h=VRING_T);
+        }
+        // Bore
+        translate([0, 0, -EPS])
+            cylinder(d=VRING_ID, h=VRING_T + GROM_H + 2*EPS);
     }
 }
 
-// ============================================================
-// PART 5 — CABLE GUIDE CLIP
-// ============================================================
-// Snap-on C-clip that presses onto column ribs to retain USB cable
-// along column exterior.  Cable sits in a semicircular channel;
-// snap tongue grips the rib.  No fasteners — push-fit on rib.
-// Print multiples: one every ~30 mm along column for clean routing.
-//
-// Print: clip-opening face down, PETG, 3 perims, 20 % infill.
-// Orientation matters — clip opening (-Y face) must face down for bridging.
+// ── Part 5: cable_guide ─────────────────────────────────────
+// Snap-on cable clip for column mid-section
 module cable_guide() {
-    snap_t   = 1.8;    // snap tongue thickness (springy PETG)
-    snap_oc  = GUIDE_CABLE_D + 2*GUIDE_T;   // channel outer cylinder OD
-    body_h   = GUIDE_BODY_H;
+    arm_t = SNAP_T;
+    gap   = CLIP_GAP;
 
     difference() {
         union() {
-            // ── Clip body (flat plate on column face) ─────────────────────
-            translate([-GUIDE_BODY_W/2, 0, 0])
-                cube([GUIDE_BODY_W, GUIDE_T, body_h]);
-
-            // ── Cable channel (C-shape, opens toward +Y) ──────────────────
-            translate([0, GUIDE_T + snap_oc/2, body_h/2])
-                rotate([0, 90, 0])
+            // Saddle body (U-shape wrapping column)
             difference() {
-                        cylinder(d = snap_oc, h = GUIDE_BODY_W,
-                                 center = true);
-                        cylinder(d = GUIDE_CABLE_D, h = GUIDE_BODY_W + 2*e,
-                                 center = true);
-                        // Open front slot for cable insertion
-                        translate([0, snap_oc/2 + e, 0])
-                            cube([GUIDE_CABLE_D * 0.85,
-                                  snap_oc + 2*e,
-                                  GUIDE_BODY_W + 2*e], center = true);
+                cylinder(d=gap + 2*CLIP_T, h=CLIP_W);
+                translate([0, 0, -EPS])
+                    cylinder(d=gap, h=CLIP_W + 2*EPS);
+                // Open front slot for snap insertion
+                translate([-gap/2, 0, -EPS])
+                    cube([gap, gap/2 + CLIP_T + EPS, CLIP_W + 2*EPS]);
             }
 
-            // ── Snap-fit tongue (grips column rib, -Y side of body) ───────
-            // Two flexible tabs that straddle column rib
-            for (tx = [-GUIDE_BODY_W/2 + 2, GUIDE_BODY_W/2 - 2 - snap_t])
-                translate([tx, -4, 0])
-                    cube([snap_t, 4 + GUIDE_T, body_h]);
+            // Snap arms
+            for (s = [-1, 1])
+                translate([s*(gap/2 - arm_t), 0, 0])
+                    mirror([s < 0 ? 1 : 0, 0, 0])
+                        translate([0, -arm_t/2, 0])
+                            cube([arm_t + 1.5, arm_t, CLIP_W]);
 
-            // Snap barbs (slight overhang engages rib back edge)
-            for (tx = [-GUIDE_BODY_W/2 + 2, GUIDE_BODY_W/2 - 2 - snap_t])
-                translate([tx + snap_t/2, -4, body_h/2])
-                    rotate([0, 90, 0])
-                        cylinder(d = 2, h = snap_t, center = true);
+            // Cable channel bracket (side-mounted)
+            translate([gap/2 + CLIP_T, -(CABLE_CH_W/2 + CLIP_T), 0])
+                cube([CLIP_T + CABLE_CH_H, CABLE_CH_W + 2*CLIP_T, CLIP_W]);
         }
 
-        // ── Rib slot (column rib passes through clip body) ─────────────────
-        translate([0, -2, body_h/2])
-            cube([3.5, GUIDE_T + 4 + e, body_h - 4], center = true);
+        // Cable channel cutout
+        translate([gap/2 + CLIP_T + CLIP_T - EPS, -CABLE_CH_W/2, -EPS])
+            cube([CABLE_CH_H + EPS, CABLE_CH_W, CLIP_W + 2*EPS]);
+
+        // Snap tip undercut (both arms)
+        for (s = [-1, 1])
+            translate([s*(gap/2 + CLIP_T + 1.0), -arm_t, -EPS])
+                rotate([0, 0, s*30])
+                    cube([2, arm_t*2, CLIP_W + 2*EPS]);
     }
 }
+
+// ── Assembly / render dispatch ───────────────────────────────
+module assembly() {
+    // tnut_base at origin
+    color("SteelBlue")
+        tnut_base();
+
+    // column rising from base boss
+    color("DodgerBlue")
+        translate([BASE_L/2, BASE_W/2, BASE_T + 8.0 - 6.0])
+            column();
+
+    // scan_platform at top of column
+    col_h_actual = ELEV_H - 8.0 - PLAT_T;
+    color("CornflowerBlue")
+        translate([BASE_L/2, BASE_W/2, BASE_T + 8.0 - 6.0 + col_h_actual + 6.0 - EPS])
+            scan_platform();
+
+    // vibe rings (4×) seated in platform holes
+    for (a = [45, 135, 225, 315])
+        color("Gray", 0.7)
+            translate([BASE_L/2, BASE_W/2,
+                       BASE_T + 8.0 - 6.0 + col_h_actual + 6.0 + PLAT_T - GROM_H])
+                rotate([0, 0, a])
+                    translate([RPL_BC_D/2, 0, 0])
+                        vibe_ring();
+
+    // cable_guide clipped at column mid-height
+    color("LightSteelBlue")
+        translate([BASE_L/2, BASE_W/2,
+                   BASE_T + 8.0 - 6.0 + (ELEV_H - 8.0 - PLAT_T)/2 - CLIP_W/2])
+            cable_guide();
+}
+
+// ── Dispatch ────────────────────────────────────────────────
+if      (RENDER == "tnut_base")     tnut_base();
+else if (RENDER == "column")        column();
+else if (RENDER == "scan_platform") scan_platform();
+else if (RENDER == "vibe_ring")     vibe_ring();
+else if (RENDER == "cable_guide")   cable_guide();
+else                                assembly();

esp32/uwb_anchor/src/main.cpp

@@ -27,6 +27,8 @@
  *   AT+RANGE_ADDR=<hex_addr>   → pair with specific tag (filter others)
  *   AT+RANGE_ADDR=             → clear pairing (accept all tags)
  *   AT+ID?                     → returns +ID:<anchor_id>
+ *   AT+PEER_RANGE=<id>         → inter-anchor DS-TWR (for auto-calibration)
+ *                                → +PEER_RANGE:<my>,<peer>,<mm>,<rssi>
  *
  * Pin mapping — Makerfabs ESP32 UWB Pro
  * ──────────────────────────────────────
@@ -188,6 +190,116 @@ static float rx_power_dbm(void) {
     return 10.0f * log10f((f * f) / (n * n)) - 121.74f;
 }
 
+/* ── Peer-anchor ranging (initiator role, for auto-calibration) ─── */
+
+/* Timeout waiting for peer's RESP during inter-anchor ranging */
+#define PEER_RX_RESP_TIMEOUT_US   3500
+/* Block up to this many ms waiting for the interrupt flags */
+#define PEER_WAIT_MS              20
+
+/* Initiate a single DS-TWR exchange toward peer anchor `peer_id`.
+ * Returns range in mm (>=0) on success, or -1 on timeout/error.
+ * RSSI is stored in *rssi_out if non-null.
+ *
+ * Exchange:
+ *   This anchor  → peer  POLL
+ *   peer         → This  RESP  (carries T_poll_rx, T_resp_tx)
+ *   This anchor  → peer  FINAL (carries Ra, Da)
+ *   This side computes its own range estimate from Ra/Da.
+ */
+static int32_t peer_range_once(uint8_t peer_id, float *rssi_out) {
+    /* ── Reset interrupt flags ── */
+    g_tx_done = g_rx_ok = g_rx_err = g_rx_to = false;
+
+    /* ── Build POLL frame ── */
+    uint8_t poll_buf[FRAME_HDR + FCS_LEN] = {
+        FTYPE_POLL,
+        (uint8_t)ANCHOR_ID,
+        peer_id,
+    };
+
+    dwt_writetxdata(sizeof(poll_buf), poll_buf, 0);
+    dwt_writetxfctrl(sizeof(poll_buf), 0, 1);
+    dwt_setrxtimeout(PEER_RX_RESP_TIMEOUT_US);
+    dwt_rxenable(DWT_START_RX_IMMEDIATE);
+    if (dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED) != DWT_SUCCESS)
+        return -1;
+
+    /* Wait for TX done */
+    uint32_t t0 = millis();
+    while (!g_tx_done && !g_rx_err && !g_rx_to) {
+        if (millis() - t0 > (uint32_t)PEER_WAIT_MS) return -1;
+    }
+    if (g_rx_err || g_rx_to) return -1;
+    g_tx_done = false;
+
+    /* Capture T_poll_tx */
+    uint64_t T_poll_tx;
+    dwt_readtxtimestamp((uint8_t *)&T_poll_tx);
+    T_poll_tx &= DWT_TS_MASK;
+
+    /* Wait for RESP */
+    t0 = millis();
+    while (!g_rx_ok && !g_rx_err && !g_rx_to) {
+        if (millis() - t0 > (uint32_t)PEER_WAIT_MS) return -1;
+    }
+    if (!g_rx_ok) return -1;
+
+    /* Validate RESP */
+    if (g_rx_len < (uint32_t)(FRAME_HDR + RESP_PAYLOAD + FCS_LEN)) return -1;
+    if (g_rx_buf[0] != FTYPE_RESP)              return -1;
+    if (g_rx_buf[1] != peer_id)                  return -1;
+    if (g_rx_buf[2] != (uint8_t)ANCHOR_ID)       return -1;
+
+    uint64_t T_resp_rx;
+    dwt_readrxtimestamp((uint8_t *)&T_resp_rx);
+    T_resp_rx &= DWT_TS_MASK;
+
+    /* Extract peer timestamps from RESP payload */
+    const uint8_t *pl = g_rx_buf + FRAME_HDR;
+    uint64_t T_poll_rx_peer = ts_read(pl);
+    uint64_t T_resp_tx_peer = ts_read(pl + 5);
+
+    /* DS-TWR Ra, Da */
+    uint64_t Ra = ts_diff(T_resp_rx, T_poll_tx);
+    uint64_t Da = ts_diff(T_resp_tx_peer, T_poll_rx_peer);
+
+    g_rx_ok = g_rx_err = g_rx_to = false;
+
+    /* ── Build FINAL frame ── */
+    uint8_t final_buf[FRAME_HDR + FINAL_PAYLOAD + FCS_LEN];
+    final_buf[0] = FTYPE_FINAL;
+    final_buf[1] = (uint8_t)ANCHOR_ID;
+    final_buf[2] = peer_id;
+    ts_write(final_buf + FRAME_HDR,      Ra);
+    ts_write(final_buf + FRAME_HDR + 5,  Da);
+    ts_write(final_buf + FRAME_HDR + 10, (uint64_t)0);  /* Db placeholder */
+
+    dwt_setrxtimeout(0);
+    dwt_writetxdata(sizeof(final_buf), final_buf, 0);
+    dwt_writetxfctrl(sizeof(final_buf), 0, 1);
+    if (dwt_starttx(DWT_START_TX_IMMEDIATE) != DWT_SUCCESS) return -1;
+
+    t0 = millis();
+    while (!g_tx_done && !g_rx_err) {
+        if (millis() - t0 > (uint32_t)PEER_WAIT_MS) return -1;
+    }
+    g_tx_done = false;
+
+    /* Simplified one-sided range estimate: tof = Ra - Da/2 */
+    double tof_s = ticks_to_s(Ra) - ticks_to_s(Da) / 2.0;
+    if (tof_s < 0.0) tof_s = 0.0;
+    int32_t range_mm = (int32_t)(tof_s * SPEED_OF_LIGHT * 1000.0);
+
+    if (rssi_out) *rssi_out = rx_power_dbm();
+
+    /* Re-enable normal RX for tag ranging */
+    dwt_setrxtimeout(0);
+    dwt_rxenable(DWT_START_RX_IMMEDIATE);
+
+    return range_mm;
+}
+
 /* ── AT command handler ─────────────────────────────────────────── */
 
 static char g_at_buf[64];
@@ -212,6 +324,23 @@ static void at_dispatch(const char *cmd) {
             g_paired_tag_id = (uint8_t)strtoul(v, nullptr, 0);
             Serial.printf("+OK:PAIRED=0x%02X\r\n", g_paired_tag_id);
         }
+
+    } else if (strncmp(cmd, "AT+PEER_RANGE=", 14) == 0) {
+        /* Inter-anchor ranging for calibration.
+         * Usage: AT+PEER_RANGE=<peer_anchor_id>
+         * Response: +PEER_RANGE:<my_id>,<peer_id>,<range_mm>,<rssi_dbm>
+         *       or: +PEER_RANGE:ERR,<peer_id>,TIMEOUT
+         */
+        uint8_t peer_id = (uint8_t)strtoul(cmd + 14, nullptr, 0);
+        float rssi = 0.0f;
+        int32_t mm = peer_range_once(peer_id, &rssi);
+        if (mm >= 0) {
+            Serial.printf("+PEER_RANGE:%d,%d,%ld,%.1f\r\n",
+                          ANCHOR_ID, peer_id, mm, (double)rssi);
+        } else {
+            Serial.printf("+PEER_RANGE:ERR,%d,TIMEOUT\r\n", peer_id);
+        }
+
     } else {
         Serial.println("+ERR:UNKNOWN_CMD");
     }

include/balance.h

@@ -3,6 +3,7 @@
 
 #include <stdint.h>
 #include "mpu6000.h"
+#include "slope_estimator.h"
 
 /*
  * SaltyLab Balance Controller
@@ -36,6 +37,9 @@ typedef struct {
     /* Safety */
     float max_tilt;         /* Cutoff angle (degrees) */
     int16_t max_speed;      /* Speed limit */
+
+    /* Slope compensation (Issue #600) */
+    slope_estimator_t slope;
 } balance_t;
 
 void balance_init(balance_t *b);

include/jlink.h

@@ -170,6 +170,14 @@ typedef struct __attribute__((packed)) {
     uint8_t  _pad;         /* reserved                                               */
 } jlink_tlm_motor_current_t;  /* 8 bytes */
 
+/* ---- Telemetry SLOPE payload (4 bytes, packed) Issue #600 ---- */
+/* Sent at SLOPE_TLM_HZ (1 Hz) by slope_estimator_send_tlm(). */
+typedef struct __attribute__((packed)) {
+    int16_t slope_x100;  /* terrain slope estimate (degrees x100; + = nose-up) */
+    uint8_t active;      /* 1 = slope estimation enabled */
+    uint8_t _pad;
+} jlink_tlm_slope_t;  /* 4 bytes */
+
 /* ---- Telemetry FAULT_LOG payload (20 bytes, packed) Issue #565 ---- */
 /* Sent on boot (if last fault != NONE) and in response to FAULT_LOG_GET. */
 typedef struct __attribute__((packed)) {
@@ -184,14 +192,6 @@ typedef struct __attribute__((packed)) {
     uint32_t hfsr;          /* SCB->HFSR */
 } jlink_tlm_fault_log_t;    /* 20 bytes */
 
-/* ---- Telemetry SLOPE payload (4 bytes, packed) Issue #600 ---- */
-/* Sent at SLOPE_TLM_HZ (1 Hz) by slope_estimator_send_tlm(). */
-typedef struct __attribute__((packed)) {
-    int16_t slope_x100;  /* terrain slope estimate (degrees x100; + = nose-up) */
-    uint8_t active;      /* 1 = slope estimation enabled */
-    uint8_t _pad;
-} jlink_tlm_slope_t;  /* 4 bytes */
-
 /* ---- Telemetry CAN_STATS payload (16 bytes, packed) Issue #597 ---- */
 /* Sent at CAN_TLM_HZ (1 Hz) and in response to CAN_STATS_GET. */
 typedef struct __attribute__((packed)) {

include/slope_estimator.h

@@ -0,0 +1,101 @@
+#ifndef SLOPE_ESTIMATOR_H
+#define SLOPE_ESTIMATOR_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * slope_estimator — slow-adapting terrain slope estimator for Issue #600.
+ *
+ * On a slope the robot must lean slightly into the hill to stay balanced.
+ * The IMU pitch reading therefore includes both the robot's balance offset
+ * and the ground incline.  This module decouples the two by tracking the
+ * slowly-changing DC component of the pitch signal with a first-order IIR
+ * low-pass filter (time constant SLOPE_TAU_S = 5 s).
+ *
+ * HOW IT WORKS:
+ *   Every call to slope_estimator_update(pitch_deg, dt):
+ *
+ *     alpha    = dt / (SLOPE_TAU_S + dt)       // ≈ 0.0002 at 1 kHz
+ *     raw      = slope * (1 - alpha) + pitch * alpha
+ *     slope    = clamp(raw, -SLOPE_MAX_DEG, +SLOPE_MAX_DEG)
+ *
+ *   The IIR converges to the steady-state pitch in ~5 s.  Fast tilt
+ *   transients (balance corrections, steps, bumps) are attenuated by
+ *   the long time constant and do not corrupt the estimate.
+ *
+ * INTEGRATION IN BALANCE PID:
+ *   Subtract slope_estimate from the measured pitch before computing
+ *   the PID error so the controller balances around the slope surface
+ *   rather than absolute vertical:
+ *
+ *     tilt_corrected = pitch_deg - slope_estimate_deg
+ *     error = setpoint - tilt_corrected
+ *
+ *   This is equivalent to continuously adjusting the balance setpoint
+ *   to track the incline.
+ *
+ * SAFETY:
+ *   - Estimate is clamped to ±SLOPE_MAX_DEG (15°) to prevent drift from
+ *     extreme falls being mistaken for genuine slopes.
+ *   - slope_estimator_reset() zeroes the state; call on disarm or after
+ *     a tilt fault so re-arming starts fresh.
+ *
+ * TELEMETRY:
+ *   JLINK_TLM_SLOPE (0x88) published at SLOPE_TLM_HZ (1 Hz):
+ *     jlink_tlm_slope_t { int16 slope_x100, uint8 active, uint8 _pad }
+ */
+
+/* ---- Configuration ---- */
+#define SLOPE_TAU_S         5.0f    /* IIR time constant (seconds)          */
+#define SLOPE_MAX_DEG      15.0f    /* Maximum estimate magnitude (degrees) */
+#define SLOPE_TLM_HZ        1u      /* JLINK_TLM_SLOPE publish rate (Hz)    */
+
+/* ---- State ---- */
+typedef struct {
+    float    estimate_deg;   /* current slope estimate (degrees, + = nose-up) */
+    bool     enabled;        /* compensation on/off; off = estimate frozen     */
+    uint32_t last_tlm_ms;   /* timestamp of last TLM transmission             */
+} slope_estimator_t;
+
+/* ---- API ---- */
+
+/*
+ * slope_estimator_init(se) — zero state, enable estimation.
+ * Call once during system init.
+ */
+void slope_estimator_init(slope_estimator_t *se);
+
+/*
+ * slope_estimator_reset(se) — zero estimate without changing enabled flag.
+ * Call on disarm or after BALANCE_TILT_FAULT to avoid stale state on rearm.
+ */
+void slope_estimator_reset(slope_estimator_t *se);
+
+/*
+ * slope_estimator_update(se, pitch_deg, dt) — advance the IIR filter.
+ *   pitch_deg : current fused pitch angle from IMU (degrees)
+ *   dt        : loop interval (seconds)
+ * No-op if se->enabled == false or dt <= 0.
+ */
+void slope_estimator_update(slope_estimator_t *se, float pitch_deg, float dt);
+
+/*
+ * slope_estimator_get_deg(se) — return current estimate (degrees).
+ * Returns 0 if disabled.
+ */
+float slope_estimator_get_deg(const slope_estimator_t *se);
+
+/*
+ * slope_estimator_set_enabled(se, en) — enable or disable compensation.
+ * Disabling freezes the estimate at its current value.
+ */
+void slope_estimator_set_enabled(slope_estimator_t *se, bool en);
+
+/*
+ * slope_estimator_send_tlm(se, now_ms) — transmit JLINK_TLM_SLOPE (0x88)
+ * frame to Jetson.  Rate-limited to SLOPE_TLM_HZ; safe to call every tick.
+ */
+void slope_estimator_send_tlm(const slope_estimator_t *se, uint32_t now_ms);
+
+#endif /* SLOPE_ESTIMATOR_H */

jetson/ros2_ws/src/saltybot_nav2_uwb/config/nav2_uwb_params.yaml

@@ -0,0 +1,315 @@
+# nav2_uwb_params.yaml — Nav2 configuration for UWB-based localization (Issue #599).
+#
+# Key differences from the AMCL/SLAM config (saltybot_nav2_slam):
+#   • AMCL is NOT used.  The uwb_pose_bridge_node broadcasts map→odom TF directly
+#     from the UWB-IMU EKF fused pose (/saltybot/pose/fused_cov).
+#   • DWB controller capped at 1.0 m/s for safe differential-drive operation.
+#   • Global costmap uses a rolling window — no pre-built static map required.
+#     UWB provides the global (map-frame) position.
+#   • Costmap obstacle layer: /scan (RPLIDAR A1/A2) only.
+#   • lifecycle_manager_localization is intentionally empty; the bridge node
+#     keeps the TF alive without Nav2 lifecycle management.
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: false
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    enable_groot_monitoring: false
+    default_nav_to_pose_bt_xml: >-
+      $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
+    default_nav_through_poses_bt_xml: >-
+      $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: false
+
+# ── Controller (DWB — differential drive, max 1.0 m/s) ─────────────────────────
+controller_server:
+  ros__parameters:
+    use_sim_time: false
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["general_goal_checker"]
+    controller_plugins: ["FollowPath"]
+
+    progress_checker:
+      plugin: "nav2_core::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 15.0
+
+    general_goal_checker:
+      stateful: true
+      plugin: "nav2_core::SimpleGoalChecker"
+      xy_goal_tolerance: 0.20
+      yaw_goal_tolerance: 0.15
+
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: false
+      # ── Velocity limits (1.0 m/s max per Issue #599) ──────────────────
+      min_vel_x: 0.0
+      max_vel_x: 1.0          # ← hard cap for safe indoor operation
+      min_vel_y: 0.0
+      max_vel_y: 0.0           # differential drive — no lateral motion
+      min_vel_theta: -1.5
+      max_vel_theta: 1.5
+      min_speed_xy: 0.0
+      max_speed_xy: 1.0
+      min_speed_theta: 0.0
+      # ── Acceleration limits ────────────────────────────────────────────
+      acc_lim_x: 1.0
+      acc_lim_y: 0.0
+      acc_lim_theta: 2.0
+      decel_lim_x: -1.0
+      decel_lim_y: 0.0
+      decel_lim_theta: -2.0
+      # ── Trajectory sampling ────────────────────────────────────────────
+      vx_samples: 20
+      vy_samples: 1            # differential drive — only 1 lateral sample
+      vtheta_samples: 40
+      sim_time: 1.5
+      linear_granularity: 0.05
+      angular_granularity: 0.05
+      transform_tolerance: 0.3
+      trans_stopped_velocity: 0.25
+      short_circuit_trajectory_evaluation: true
+      stateful: true
+      # ── Critic weights ─────────────────────────────────────────────────
+      critics:
+        - "RotateToGoal"
+        - "Oscillation"
+        - "BaseObstacle"
+        - "GoalAlign"
+        - "PathAlign"
+        - "PathDist"
+        - "GoalDist"
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: false
+
+# ── Global planner (NavFn A*) ────────────────────────────────────────────────
+planner_server:
+  ros__parameters:
+    use_sim_time: false
+    expected_planner_frequency: 10.0
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.3
+      use_astar: true
+      allow_unknown: true
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: false
+
+# ── Recovery behaviors ───────────────────────────────────────────────────────
+behavior_server:
+  ros__parameters:
+    use_sim_time: false
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+    drive_on_heading:
+      plugin: "nav2_behaviors/DriveOnHeading"
+    wait:
+      plugin: "nav2_behaviors/Wait"
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_timeout: 0.1
+    simulate_ahead_time: 2.0
+    max_rotations: 1.0
+    max_backup_dist: 0.3
+    backup_speed: 0.15
+
+behavior_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: false
+
+# ── Costmaps ─────────────────────────────────────────────────────────────────
+#
+# Both costmaps use only /scan (RPLIDAR).  No depth camera source here —
+# saltybot_depth_costmap adds that layer separately when enabled.
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      use_sim_time: false
+      update_frequency: 10.0
+      publish_frequency: 5.0
+      global_frame: odom
+      robot_base_frame: base_link
+      rolling_window: true
+      width: 4
+      height: 4
+      resolution: 0.05
+      robot_radius: 0.35
+      plugins: ["obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: true
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          min_obstacle_height: 0.0
+          clearing: true
+          marking: true
+          data_type: "LaserScan"
+          raytrace_max_range: 8.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 6.0
+          obstacle_min_range: 0.0
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        enabled: true
+        inflation_radius: 0.55
+        cost_scaling_factor: 10.0
+        inflate_unknown: false
+        inflate_around_unknown: true
+      track_unknown_space: false
+      always_send_full_costmap: true
+
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: false
+
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: false
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: false
+      update_frequency: 2.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      # Rolling window — UWB provides global position, no static map needed.
+      rolling_window: true
+      width: 20
+      height: 20
+      resolution: 0.05
+      robot_radius: 0.35
+      plugins: ["obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: true
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          min_obstacle_height: 0.0
+          clearing: true
+          marking: true
+          data_type: "LaserScan"
+          raytrace_max_range: 8.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 6.0
+          obstacle_min_range: 0.0
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        enabled: true
+        inflation_radius: 0.55
+        cost_scaling_factor: 10.0
+        inflate_unknown: false
+        inflate_around_unknown: true
+      track_unknown_space: false
+      always_send_full_costmap: false
+
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: false
+
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: false
+
+# ── Velocity smoother ────────────────────────────────────────────────────────
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: false
+    smoothing_frequency: 20.0
+    scale_velocities: false
+    feedback: "OPEN_LOOP"
+    max_velocity: [1.0, 0.0, 1.5]     # [x, y, theta] — capped at 1.0 m/s
+    min_velocity: [-0.3, 0.0, -1.5]
+    max_accel: [1.0, 0.0, 2.0]
+    max_decel: [-1.0, 0.0, -2.0]
+    odom_topic: /odom
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0
+
+# ── Lifecycle managers ───────────────────────────────────────────────────────
+#
+# Localization lifecycle is intentionally empty — the uwb_pose_bridge_node
+# manages the map→odom TF independently (no AMCL, no SLAM Toolbox).
+
+lifecycle_manager_navigation:
+  ros__parameters:
+    use_sim_time: false
+    autostart: true
+    node_names:
+      - "controller_server"
+      - "planner_server"
+      - "behavior_server"
+      - "bt_navigator"
+      - "velocity_smoother"
+
+lifecycle_manager_localization:
+  ros__parameters:
+    use_sim_time: false
+    autostart: true
+    node_names: []   # UWB pose bridge handles TF directly — no AMCL
+
+map_server:
+  ros__parameters:
+    use_sim_time: false
+    yaml_filename: ""    # No static map — global costmap is rolling window

jetson/ros2_ws/src/saltybot_nav2_uwb/launch/nav2_uwb.launch.py

@@ -0,0 +1,168 @@
+"""nav2_uwb.launch.py — Full Nav2 stack with UWB localization (Issue #599).
+
+Launch sequence
+───────────────
+  1. uwb_pose_bridge_node    — broadcasts map→odom TF from UWB-IMU EKF pose
+  2. nav2_bringup navigation — controller, planner, BT navigator, costmaps
+  3. lifecycle_manager_navigation — autostart all Nav2 nodes
+  4. waypoint_follower_node  — sequential waypoint execution action server
+
+Localization is handled by the UWB bridge; AMCL is NOT launched.
+
+Arguments
+─────────
+  use_sim_time   (bool)   default: false
+  params_file    (str)    default: <package>/config/nav2_uwb_params.yaml
+  uwb_pose_topic (str)    default: /saltybot/pose/fused_cov
+  odom_topic     (str)    default: /odom
+  map_frame      (str)    default: map
+  odom_frame     (str)    default: odom
+  base_frame     (str)    default: base_link
+  max_vel_x      (float)  default: 1.0  — DWB hard cap (m/s)
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    GroupAction,
+    IncludeLaunchDescription,
+    LogInfo,
+    TimerAction,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import (
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    PythonExpression,
+)
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    pkg_nav2_uwb  = get_package_share_directory("saltybot_nav2_uwb")
+    pkg_nav2_bringup = get_package_share_directory("nav2_bringup")
+
+    default_params = os.path.join(pkg_nav2_uwb, "config", "nav2_uwb_params.yaml")
+
+    # ── Declare arguments ──────────────────────────────────────────────────
+    args = [
+        DeclareLaunchArgument(
+            "use_sim_time", default_value="false",
+            description="Use simulation clock"),
+        DeclareLaunchArgument(
+            "params_file", default_value=default_params,
+            description="Full path to nav2_uwb_params.yaml"),
+        DeclareLaunchArgument(
+            "uwb_pose_topic",  default_value="/saltybot/pose/fused_cov",
+            description="UWB-IMU EKF fused pose topic (PoseWithCovarianceStamped)"),
+        DeclareLaunchArgument(
+            "odom_topic",  default_value="/odom",
+            description="Wheel odometry topic (Odometry)"),
+        DeclareLaunchArgument(
+            "map_frame",   default_value="map",
+            description="Global map frame id"),
+        DeclareLaunchArgument(
+            "odom_frame",  default_value="odom",
+            description="Odometry frame id"),
+        DeclareLaunchArgument(
+            "base_frame",  default_value="base_link",
+            description="Robot base frame id"),
+        DeclareLaunchArgument(
+            "autostart",   default_value="true",
+            description="Automatically start Nav2 lifecycle nodes"),
+    ]
+
+    # ── UWB pose bridge — starts immediately ──────────────────────────────
+    uwb_bridge = Node(
+        package="saltybot_nav2_uwb",
+        executable="uwb_pose_bridge_node",
+        name="uwb_pose_bridge",
+        output="screen",
+        parameters=[{
+            "use_sim_time":    LaunchConfiguration("use_sim_time"),
+            "uwb_pose_topic":  LaunchConfiguration("uwb_pose_topic"),
+            "odom_topic":      LaunchConfiguration("odom_topic"),
+            "map_frame":       LaunchConfiguration("map_frame"),
+            "odom_frame":      LaunchConfiguration("odom_frame"),
+            "base_frame":      LaunchConfiguration("base_frame"),
+            "tf_publish_hz":   20.0,
+            "tf_timeout_s":    0.5,
+        }],
+    )
+
+    uwb_bridge_log = LogInfo(
+        msg="[nav2_uwb] UWB pose bridge launched — map→odom TF will be "
+            "broadcast once UWB + odom data arrive."
+    )
+
+    # ── Nav2 navigation stack (t=2s: allow bridge to initialise first) ─────
+    nav2_navigation = TimerAction(
+        period=2.0,
+        actions=[
+            LogInfo(msg="[nav2_uwb] Starting Nav2 navigation stack..."),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(pkg_nav2_bringup, "launch", "navigation_launch.py")
+                ),
+                launch_arguments={
+                    "use_sim_time":                  LaunchConfiguration("use_sim_time"),
+                    "params_file":                   LaunchConfiguration("params_file"),
+                    "autostart":                     LaunchConfiguration("autostart"),
+                    # No map_subscribe_transient_local — no static map
+                    "use_lifecycle_mgr":             "true",
+                    "use_sim_time":                  LaunchConfiguration("use_sim_time"),
+                }.items(),
+            ),
+        ],
+    )
+
+    # ── Waypoint follower (t=4s: Nav2 needs time to come up) ──────────────
+    waypoint_follower = TimerAction(
+        period=4.0,
+        actions=[
+            LogInfo(msg="[nav2_uwb] Starting waypoint follower action server..."),
+            Node(
+                package="saltybot_nav2_uwb",
+                executable="waypoint_follower_node",
+                name="waypoint_follower",
+                output="screen",
+                parameters=[{
+                    "use_sim_time":       LaunchConfiguration("use_sim_time"),
+                    "map_frame":          LaunchConfiguration("map_frame"),
+                    "goal_xy_tolerance":  0.20,
+                    "goal_yaw_tolerance": 0.15,
+                    "nav_timeout_s":      60.0,
+                    "status_hz":          2.0,
+                }],
+            ),
+        ],
+    )
+
+    ready_log = TimerAction(
+        period=4.5,
+        actions=[
+            LogInfo(
+                msg="[nav2_uwb] Stack ready.\n"
+                    "  Localization : UWB-IMU EKF bridge (no AMCL)\n"
+                    "  Controller   : DWB ≤ 1.0 m/s  (differential drive)\n"
+                    "  Waypoints    : /saltybot/nav/follow_waypoints  (action)\n"
+                    "  Status       : /saltybot/nav/waypoint_status   (JSON)\n"
+                    "  Cancel       : /saltybot/nav/cancel            (topic)"
+            ),
+        ],
+    )
+
+    return LaunchDescription(
+        args + [
+            uwb_bridge_log,
+            uwb_bridge,
+            nav2_navigation,
+            waypoint_follower,
+            ready_log,
+        ]
+    )

jetson/ros2_ws/src/saltybot_nav2_uwb/package.xml

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>saltybot_nav2_uwb</name>
+  <version>0.1.0</version>
+  <description>
+    Nav2 integration with UWB-based localization for SaltyBot (Issue #599).
+    Replaces AMCL with a UWB pose bridge that broadcasts the map-&gt;odom TF
+    directly from the UWB-IMU EKF fused pose.  Includes a DWB controller
+    capped at 1.0 m/s for safe differential-drive operation and a simple
+    sequential waypoint-follower action server.
+  </description>
+  <maintainer email="sl-jetson@saltylab.local">sl-jetson</maintainer>
+  <license>Apache-2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>nav2_msgs</depend>
+
+  <exec_depend>nav2_bringup</exec_depend>
+  <exec_depend>nav2_bt_navigator</exec_depend>
+  <exec_depend>nav2_controller</exec_depend>
+  <exec_depend>nav2_planner</exec_depend>
+  <exec_depend>nav2_behaviors</exec_depend>
+  <exec_depend>nav2_lifecycle_manager</exec_depend>
+  <exec_depend>nav2_costmap_2d</exec_depend>
+  <exec_depend>nav2_navfn_planner</exec_depend>
+  <exec_depend>dwb_core</exec_depend>
+  <exec_depend>dwb_critics</exec_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_nav2_uwb/saltybot_nav2_uwb/uwb_pose_bridge_node.py

@@ -0,0 +1,218 @@
+"""uwb_pose_bridge_node.py — UWB→Nav2 localization bridge (Issue #599).
+
+Replaces AMCL by listening to the UWB-IMU EKF fused pose and broadcasting
+the map→odom TF that Nav2 requires for global localization.
+
+TF computation (2D, ground robot):
+──────────────────────────────────
+    map → base_link   := UWB-IMU EKF   (/saltybot/pose/fused_cov)
+    odom → base_link  := wheel odometry  (/odom)
+
+    T_map_odom = T_map_base_link  ⊗  T_odom_base_link⁻¹
+
+Published TF:  map → odom  (at UWB rate, ≥ 10 Hz)
+Published topic: /initialpose  (once on first valid pose, for Nav2 boot)
+
+Subscribed topics:
+    /saltybot/pose/fused_cov  (PoseWithCovarianceStamped) — UWB-IMU EKF
+    /odom                     (Odometry)                  — wheel odometry
+"""
+
+from __future__ import annotations
+
+import math
+from typing import Optional
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import (
+    PoseWithCovarianceStamped,
+    TransformStamped,
+)
+from nav_msgs.msg import Odometry
+from tf2_ros import TransformBroadcaster
+
+
+# ── Pure-Python 2-D transform helpers (no numpy / tf_transformations needed) ──
+
+def _yaw_from_q(qx: float, qy: float, qz: float, qw: float) -> float:
+    """Extract yaw (rad) from unit quaternion."""
+    return math.atan2(2.0 * (qw * qz + qx * qy),
+                      1.0 - 2.0 * (qy * qy + qz * qz))
+
+
+def _q_from_yaw(yaw: float):
+    """Return (qx, qy, qz, qw) for a pure-yaw rotation."""
+    return 0.0, 0.0, math.sin(yaw * 0.5), math.cos(yaw * 0.5)
+
+
+def _invert_2d(x: float, y: float, th: float):
+    """Invert a 2-D SE(2) transform."""
+    c, s = math.cos(th), math.sin(th)
+    xi = -(c * x + s * y)
+    yi = -(-s * x + c * y)
+    return xi, yi, -th
+
+
+def _compose_2d(x1: float, y1: float, th1: float,
+                x2: float, y2: float, th2: float):
+    """Compose two 2-D SE(2) transforms:  T1 ⊗ T2."""
+    c1, s1 = math.cos(th1), math.sin(th1)
+    xr = x1 + c1 * x2 - s1 * y2
+    yr = y1 + s1 * x2 + c1 * y2
+    thr = th1 + th2
+    return xr, yr, thr
+
+
+def compute_map_odom_tf(
+    map_bl_x: float, map_bl_y: float, map_bl_th: float,
+    odom_bl_x: float, odom_bl_y: float, odom_bl_th: float,
+):
+    """Return (x, y, yaw) for the map→odom transform.
+
+    T_map_odom = T_map_base_link ⊗ inv(T_odom_base_link)
+    """
+    ix, iy, ith = _invert_2d(odom_bl_x, odom_bl_y, odom_bl_th)
+    return _compose_2d(map_bl_x, map_bl_y, map_bl_th, ix, iy, ith)
+
+
+# ── ROS2 node ─────────────────────────────────────────────────────────────────
+
+class UwbPoseBridgeNode(Node):
+    """Broadcasts map→odom TF derived from UWB-IMU fused pose + wheel odom.
+
+    Parameters (ROS):
+        uwb_pose_topic   (str)   default: /saltybot/pose/fused_cov
+        odom_topic       (str)   default: /odom
+        tf_publish_hz    (float) default: 20.0   — TF broadcast rate
+        tf_timeout_s     (float) default: 0.5    — stop publishing if UWB
+                                                   older than this
+        map_frame        (str)   default: map
+        odom_frame       (str)   default: odom
+        base_frame       (str)   default: base_link
+    """
+
+    def __init__(self):
+        super().__init__("uwb_pose_bridge")
+
+        self.declare_parameter("uwb_pose_topic",  "/saltybot/pose/fused_cov")
+        self.declare_parameter("odom_topic",      "/odom")
+        self.declare_parameter("tf_publish_hz",   20.0)
+        self.declare_parameter("tf_timeout_s",    0.5)
+        self.declare_parameter("map_frame",       "map")
+        self.declare_parameter("odom_frame",      "odom")
+        self.declare_parameter("base_frame",      "base_link")
+
+        self._map_frame  = self.get_parameter("map_frame").value
+        self._odom_frame = self.get_parameter("odom_frame").value
+        self._base_frame = self.get_parameter("base_frame").value
+        self._tf_timeout = self.get_parameter("tf_timeout_s").value
+
+        self._uwb_pose:  Optional[PoseWithCovarianceStamped] = None
+        self._odom_pose: Optional[Odometry]                  = None
+        self._init_pose_sent: bool = False
+
+        self._tf_broadcaster = TransformBroadcaster(self)
+
+        # /initialpose: Nav2 uses this to seed the robot's initial position
+        self._init_pub = self.create_publisher(
+            PoseWithCovarianceStamped, "/initialpose", 1)
+
+        self.create_subscription(
+            PoseWithCovarianceStamped,
+            self.get_parameter("uwb_pose_topic").value,
+            self._on_uwb_pose, 10)
+
+        self.create_subscription(
+            Odometry,
+            self.get_parameter("odom_topic").value,
+            self._on_odom, 20)
+
+        hz = self.get_parameter("tf_publish_hz").value
+        self.create_timer(1.0 / hz, self._publish_tf)
+
+        self.get_logger().info(
+            f"UWB pose bridge ready | "
+            f"uwb={self.get_parameter('uwb_pose_topic').value}  "
+            f"odom={self.get_parameter('odom_topic').value}  "
+            f"tf={self._map_frame}→{self._odom_frame} @ {hz:.0f} Hz"
+        )
+
+    # ── Callbacks ──────────────────────────────────────────────────────────
+
+    def _on_uwb_pose(self, msg: PoseWithCovarianceStamped) -> None:
+        self._uwb_pose = msg
+        if not self._init_pose_sent:
+            self._init_pub.publish(msg)
+            self._init_pose_sent = True
+            self.get_logger().info(
+                f"Initial UWB pose published to /initialpose  "
+                f"({msg.pose.pose.position.x:.2f}, "
+                f"{msg.pose.pose.position.y:.2f})"
+            )
+
+    def _on_odom(self, msg: Odometry) -> None:
+        self._odom_pose = msg
+
+    # ── TF broadcast ───────────────────────────────────────────────────────
+
+    def _publish_tf(self) -> None:
+        if self._uwb_pose is None or self._odom_pose is None:
+            return
+
+        # Staleness guard: stop broadcasting if UWB data is too old
+        now = self.get_clock().now()
+        uwb_stamp = rclpy.time.Time.from_msg(self._uwb_pose.header.stamp)
+        age_s = (now - uwb_stamp).nanoseconds * 1e-9
+        if age_s > self._tf_timeout:
+            self.get_logger().warn(
+                f"UWB pose stale ({age_s:.2f}s > {self._tf_timeout}s) — "
+                "map→odom TF suppressed",
+                throttle_duration_sec=5.0,
+            )
+            return
+
+        # Extract map→base_link from UWB pose
+        p = self._uwb_pose.pose.pose
+        map_bl_th = _yaw_from_q(p.orientation.x, p.orientation.y,
+                                 p.orientation.z, p.orientation.w)
+        map_bl_x, map_bl_y = p.position.x, p.position.y
+
+        # Extract odom→base_link from wheel odometry
+        o = self._odom_pose.pose.pose
+        odom_bl_th = _yaw_from_q(o.orientation.x, o.orientation.y,
+                                  o.orientation.z, o.orientation.w)
+        odom_bl_x, odom_bl_y = o.position.x, o.position.y
+
+        # Compute map→odom
+        mo_x, mo_y, mo_th = compute_map_odom_tf(
+            map_bl_x, map_bl_y, map_bl_th,
+            odom_bl_x, odom_bl_y, odom_bl_th,
+        )
+
+        # Broadcast
+        tf_msg = TransformStamped()
+        tf_msg.header.stamp    = now.to_msg()
+        tf_msg.header.frame_id = self._map_frame
+        tf_msg.child_frame_id  = self._odom_frame
+        tf_msg.transform.translation.x = mo_x
+        tf_msg.transform.translation.y = mo_y
+        tf_msg.transform.translation.z = 0.0
+        qx, qy, qz, qw = _q_from_yaw(mo_th)
+        tf_msg.transform.rotation.x = qx
+        tf_msg.transform.rotation.y = qy
+        tf_msg.transform.rotation.z = qz
+        tf_msg.transform.rotation.w = qw
+        self._tf_broadcaster.sendTransform(tf_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = UwbPoseBridgeNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()

jetson/ros2_ws/src/saltybot_nav2_uwb/saltybot_nav2_uwb/waypoint_follower_node.py

@@ -0,0 +1,349 @@
+"""waypoint_follower_node.py — Simple sequential waypoint follower (Issue #599).
+
+Exposes a ROS2 action server that accepts a list of waypoints and drives the
+robot through them one by one via Nav2's NavigateToPose action.
+
+Action server:  /saltybot/nav/follow_waypoints  (nav2_msgs/FollowWaypoints)
+Nav2 client:    /navigate_to_pose               (nav2_msgs/NavigateToPose)
+
+Waypoints topic (JSON, for operator / WebUI override):
+    /saltybot/nav/waypoints  (std_msgs/String)
+    Payload: JSON array of {x, y, yaw_deg} objects.
+    Example: [{"x": 1.0, "y": 2.0, "yaw_deg": 90.0}, ...]
+
+Status topic (JSON):
+    /saltybot/nav/waypoint_status  (std_msgs/String)
+    Published every cycle with WaypointSequencer.status_dict().
+
+Cancel topic:
+    /saltybot/nav/cancel  (std_msgs/Empty)
+    Cancels the active mission.
+"""
+
+from __future__ import annotations
+
+import json
+import math
+import threading
+from typing import List, Optional
+
+import rclpy
+from rclpy.action import ActionClient, ActionServer, CancelResponse, GoalResponse
+from rclpy.node import Node
+from rclpy.callback_groups import ReentrantCallbackGroup
+
+from geometry_msgs.msg import PoseStamped
+from nav2_msgs.action import FollowWaypoints, NavigateToPose
+from std_msgs.msg import Empty, String
+
+from saltybot_nav2_uwb.waypoint_sequencer import Waypoint, WaypointSequencer
+
+
+def _yaw_to_quaternion(yaw_rad: float):
+    """Return (qx, qy, qz, qw) for a heading-only rotation."""
+    return 0.0, 0.0, math.sin(yaw_rad * 0.5), math.cos(yaw_rad * 0.5)
+
+
+def waypoint_to_pose_stamped(wp: Waypoint, frame: str = "map") -> PoseStamped:
+    """Convert a Waypoint to a PoseStamped in the given frame."""
+    msg = PoseStamped()
+    msg.header.frame_id = frame
+    msg.pose.position.x = wp.x
+    msg.pose.position.y = wp.y
+    msg.pose.position.z = 0.0
+    qx, qy, qz, qw = _yaw_to_quaternion(math.radians(wp.yaw_deg))
+    msg.pose.orientation.x = qx
+    msg.pose.orientation.y = qy
+    msg.pose.orientation.z = qz
+    msg.pose.orientation.w = qw
+    return msg
+
+
+class WaypointFollowerNode(Node):
+    """Sequential waypoint follower backed by Nav2 NavigateToPose.
+
+    Parameters (ROS):
+        map_frame          (str)   default: map
+        goal_xy_tolerance  (float) default: 0.20  (m)  — passed to Nav2
+        goal_yaw_tolerance (float) default: 0.15  (rad) — passed to Nav2
+        nav_timeout_s      (float) default: 60.0  — abort if Nav2 takes longer
+        status_hz          (float) default: 2.0   — status publish rate
+    """
+
+    def __init__(self):
+        super().__init__("waypoint_follower")
+
+        self.declare_parameter("map_frame",          "map")
+        self.declare_parameter("goal_xy_tolerance",  0.20)
+        self.declare_parameter("goal_yaw_tolerance", 0.15)
+        self.declare_parameter("nav_timeout_s",      60.0)
+        self.declare_parameter("status_hz",          2.0)
+
+        self._map_frame    = self.get_parameter("map_frame").value
+        self._nav_timeout  = self.get_parameter("nav_timeout_s").value
+
+        self._seq   = WaypointSequencer()
+        self._lock  = threading.Lock()
+        self._cb_group = ReentrantCallbackGroup()
+
+        # ── Publishers ─────────────────────────────────────────────────
+        self._status_pub = self.create_publisher(
+            String, "/saltybot/nav/waypoint_status", 10)
+
+        # ── Subscriptions ──────────────────────────────────────────────
+        self.create_subscription(
+            String, "/saltybot/nav/waypoints",
+            self._on_waypoints_json, 10,
+            callback_group=self._cb_group)
+
+        self.create_subscription(
+            Empty, "/saltybot/nav/cancel",
+            self._on_cancel, 10,
+            callback_group=self._cb_group)
+
+        # ── Nav2 action client ─────────────────────────────────────────
+        self._nav_client: ActionClient = ActionClient(
+            self, NavigateToPose, "/navigate_to_pose",
+            callback_group=self._cb_group)
+
+        # ── Action server (nav2_msgs/FollowWaypoints) ──────────────────
+        self._action_server = ActionServer(
+            self,
+            FollowWaypoints,
+            "/saltybot/nav/follow_waypoints",
+            execute_callback=self._execute_follow_waypoints,
+            goal_callback=self._goal_callback,
+            cancel_callback=self._cancel_callback,
+            callback_group=self._cb_group,
+        )
+
+        hz = self.get_parameter("status_hz").value
+        self.create_timer(1.0 / hz, self._publish_status,
+                          callback_group=self._cb_group)
+
+        self.get_logger().info(
+            f"WaypointFollower ready | "
+            f"action=/saltybot/nav/follow_waypoints  "
+            f"nav2=/navigate_to_pose  frame={self._map_frame}"
+        )
+
+    # ── Action server callbacks ────────────────────────────────────────────
+
+    def _goal_callback(self, goal_request):
+        if not self._nav_client.wait_for_server(timeout_sec=2.0):
+            self.get_logger().error(
+                "Nav2 NavigateToPose server not available — rejecting goal")
+            return GoalResponse.REJECT
+        if len(goal_request.poses) == 0:
+            self.get_logger().warn("Empty waypoint list — rejecting goal")
+            return GoalResponse.REJECT
+        return GoalResponse.ACCEPT
+
+    def _cancel_callback(self, _goal_handle):
+        return CancelResponse.ACCEPT
+
+    async def _execute_follow_waypoints(self, goal_handle) -> FollowWaypoints.Result:
+        """Execute the action: drive through each PoseStamped in sequence."""
+        result = FollowWaypoints.Result()
+        feedback_msg = FollowWaypoints.Feedback()
+
+        poses: List[PoseStamped] = list(goal_handle.request.poses)
+        waypoints = [
+            Waypoint(
+                x=p.pose.position.x,
+                y=p.pose.position.y,
+                yaw_deg=math.degrees(
+                    math.atan2(
+                        2.0 * (p.pose.orientation.w * p.pose.orientation.z
+                               + p.pose.orientation.x * p.pose.orientation.y),
+                        1.0 - 2.0 * (p.pose.orientation.y ** 2
+                                     + p.pose.orientation.z ** 2),
+                    )
+                ),
+            )
+            for p in poses
+        ]
+
+        with self._lock:
+            self._seq.start(waypoints)
+
+        self.get_logger().info(
+            f"Starting waypoint mission: {len(waypoints)} waypoints")
+
+        missed_waypoints: List[int] = []
+
+        while True:
+            # Check cancellation
+            if goal_handle.is_cancel_requested:
+                with self._lock:
+                    self._seq.cancel()
+                goal_handle.canceled()
+                result.missed_waypoints = missed_waypoints
+                return result
+
+            with self._lock:
+                if self._seq.is_done:
+                    break
+                wp = self._seq.current_waypoint
+                idx = self._seq.current_index
+
+            if wp is None:
+                break
+
+            # Send goal to Nav2
+            nav_goal = NavigateToPose.Goal()
+            nav_goal.pose = waypoint_to_pose_stamped(wp, self._map_frame)
+
+            self.get_logger().info(
+                f"Sending waypoint {idx + 1}/{len(waypoints)}: "
+                f"({wp.x:.2f}, {wp.y:.2f}, {wp.yaw_deg:.1f}°)"
+            )
+
+            if not self._nav_client.wait_for_server(timeout_sec=5.0):
+                self.get_logger().error("Nav2 server unavailable — aborting")
+                with self._lock:
+                    self._seq.abort("Nav2 server unavailable")
+                missed_waypoints.extend(range(idx, len(waypoints)))
+                break
+
+            send_future = self._nav_client.send_goal_async(
+                nav_goal,
+                feedback_callback=lambda fb: None,  # suppress individual feedback
+            )
+            # Spin until goal accepted
+            rclpy.spin_until_future_complete(
+                self, send_future, timeout_sec=self._nav_timeout)
+
+            if not send_future.done():
+                self.get_logger().error(
+                    f"Waypoint {idx + 1}: goal send timed out — skipping")
+                missed_waypoints.append(idx)
+                with self._lock:
+                    self._seq.advance()
+                continue
+
+            goal_res = send_future.result()
+            if not goal_res.accepted:
+                self.get_logger().warn(
+                    f"Waypoint {idx + 1}: goal rejected by Nav2 — skipping")
+                missed_waypoints.append(idx)
+                with self._lock:
+                    self._seq.advance()
+                continue
+
+            # Wait for Nav2 to reach the goal
+            result_future = goal_res.get_result_async()
+            rclpy.spin_until_future_complete(
+                self, result_future, timeout_sec=self._nav_timeout)
+
+            if not result_future.done():
+                self.get_logger().error(
+                    f"Waypoint {idx + 1}: Nav2 timed out after "
+                    f"{self._nav_timeout:.0f}s — skipping")
+                goal_res.cancel_goal_async()
+                missed_waypoints.append(idx)
+            else:
+                nav_result = result_future.result()
+                from action_msgs.msg import GoalStatus
+                if nav_result.status == GoalStatus.STATUS_SUCCEEDED:
+                    self.get_logger().info(
+                        f"Waypoint {idx + 1}/{len(waypoints)} reached ✓")
+                else:
+                    self.get_logger().warn(
+                        f"Waypoint {idx + 1} failed (Nav2 status "
+                        f"{nav_result.status}) — skipping")
+                    missed_waypoints.append(idx)
+
+            with self._lock:
+                self._seq.advance()
+
+            # Publish feedback
+            feedback_msg.current_waypoint = self._seq.current_index
+            goal_handle.publish_feedback(feedback_msg)
+
+        # Mission complete
+        with self._lock:
+            final_state = self._seq.state
+
+        if final_state == WaypointSequencer.SUCCEEDED and not missed_waypoints:
+            self.get_logger().info(
+                f"Waypoint mission SUCCEEDED ({len(waypoints)} waypoints)")
+            goal_handle.succeed()
+        elif final_state == WaypointSequencer.CANCELED:
+            self.get_logger().info("Waypoint mission CANCELED")
+            goal_handle.canceled()
+        else:
+            self.get_logger().warn(
+                f"Waypoint mission ended: state={final_state} "
+                f"missed={missed_waypoints}")
+            goal_handle.succeed()  # FollowWaypoints always succeeds; missed list carries info
+
+        result.missed_waypoints = missed_waypoints
+        return result
+
+    # ── JSON topic handler ─────────────────────────────────────────────────
+
+    def _on_waypoints_json(self, msg: String) -> None:
+        """Accept a JSON waypoint list and start a new mission immediately.
+
+        This is a convenience interface for WebUI / operator use.
+        For production use prefer the action server (supports feedback/cancel).
+        """
+        try:
+            raw = json.loads(msg.data)
+        except json.JSONDecodeError as exc:
+            self.get_logger().error(f"Invalid waypoints JSON: {exc}")
+            return
+
+        if not isinstance(raw, list) or len(raw) == 0:
+            self.get_logger().error(
+                "Waypoints JSON must be a non-empty array of {x, y, yaw_deg}")
+            return
+
+        try:
+            waypoints = [Waypoint.from_dict(d) for d in raw]
+        except (KeyError, ValueError) as exc:
+            self.get_logger().error(f"Malformed waypoint in list: {exc}")
+            return
+
+        if not self._nav_client.wait_for_server(timeout_sec=2.0):
+            self.get_logger().error(
+                "Nav2 server not ready — cannot execute JSON waypoints")
+            return
+
+        self.get_logger().info(
+            f"JSON mission: {len(waypoints)} waypoints received via topic")
+        with self._lock:
+            self._seq.start(waypoints)
+        # Execution driven by action server; JSON topic just seeds the sequencer.
+        # For full feedback, use the FollowWaypoints action server directly.
+
+    # ── Cancel topic handler ───────────────────────────────────────────────
+
+    def _on_cancel(self, _msg: Empty) -> None:
+        with self._lock:
+            if self._seq.is_running:
+                self._seq.cancel()
+                self.get_logger().info(
+                    "Mission cancelled via /saltybot/nav/cancel")
+
+    # ── Status publish ─────────────────────────────────────────────────────
+
+    def _publish_status(self) -> None:
+        with self._lock:
+            d = self._seq.status_dict()
+        self._status_pub.publish(String(data=json.dumps(d)))
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = WaypointFollowerNode()
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()

jetson/ros2_ws/src/saltybot_nav2_uwb/saltybot_nav2_uwb/waypoint_sequencer.py

@@ -0,0 +1,176 @@
+"""waypoint_sequencer.py — Pure-Python waypoint sequencing logic (Issue #599).
+
+No ROS2 dependencies — importable and fully unit-testable without a live
+ROS2 install.
+
+Design
+──────
+WaypointSequencer tracks a list of (x, y, yaw_deg) waypoints and the index of
+the currently active goal.  The ROS2 node (waypoint_follower_node.py) drives
+this state machine by calling:
+
+    sequencer.start(waypoints)          — load a new mission
+    sequencer.advance()                 — mark current waypoint reached
+    sequencer.abort(reason)             — mark mission failed
+    sequencer.cancel()                  — mark mission cancelled
+
+State transitions:
+    IDLE → RUNNING  (on start())
+    RUNNING → SUCCEEDED  (on advance() when last waypoint reached)
+    RUNNING → ABORTED    (on abort())
+    RUNNING → CANCELED   (on cancel())
+    ABORTED / CANCELED / SUCCEEDED → any state via start()
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import List, Optional, Tuple
+
+
+# ── Waypoint data type ────────────────────────────────────────────────────────
+
+@dataclass
+class Waypoint:
+    """A single navigation goal in the map frame."""
+    x: float
+    y: float
+    yaw_deg: float = 0.0
+
+    def __post_init__(self):
+        if not (-360.0 <= self.yaw_deg <= 360.0):
+            raise ValueError(
+                f"yaw_deg must be in [-360, 360], got {self.yaw_deg}"
+            )
+
+    @classmethod
+    def from_dict(cls, d: dict) -> "Waypoint":
+        """Construct from a plain dict (e.g. parsed from JSON)."""
+        return cls(
+            x=float(d["x"]),
+            y=float(d["y"]),
+            yaw_deg=float(d.get("yaw_deg", 0.0)),
+        )
+
+    def to_dict(self) -> dict:
+        return {"x": self.x, "y": self.y, "yaw_deg": self.yaw_deg}
+
+
+# ── State machine ─────────────────────────────────────────────────────────────
+
+class WaypointSequencer:
+    """Tracks state for sequential waypoint execution.
+
+    Thread-safety: external callers (ROS2 callbacks, action server) should
+    hold their own lock when calling multiple methods atomically.
+    """
+
+    IDLE      = "idle"
+    RUNNING   = "running"
+    SUCCEEDED = "succeeded"
+    ABORTED   = "aborted"
+    CANCELED  = "canceled"
+
+    def __init__(self) -> None:
+        self._waypoints:  List[Waypoint] = []
+        self._current:    int = 0
+        self._state:      str = self.IDLE
+        self._abort_reason: Optional[str] = None
+
+    # ── Control methods ────────────────────────────────────────────────────
+
+    def start(self, waypoints: List[Waypoint]) -> None:
+        """Load a new mission and set state to RUNNING.
+
+        Raises:
+            ValueError: if waypoints is empty.
+        """
+        if not waypoints:
+            raise ValueError("Waypoint list must not be empty")
+        self._waypoints   = list(waypoints)
+        self._current     = 0
+        self._state       = self.RUNNING
+        self._abort_reason = None
+
+    def advance(self) -> None:
+        """Mark the current waypoint as reached and move to the next.
+
+        If the last waypoint is reached the state transitions to SUCCEEDED.
+
+        Raises:
+            RuntimeError: if not RUNNING.
+        """
+        if self._state != self.RUNNING:
+            raise RuntimeError(
+                f"advance() called in state {self._state!r} (must be RUNNING)"
+            )
+        self._current += 1
+        if self._current >= len(self._waypoints):
+            self._state = self.SUCCEEDED
+
+    def abort(self, reason: str = "") -> None:
+        """Mark the mission as aborted (e.g. Nav2 returned FAILED)."""
+        self._state = self.ABORTED
+        self._abort_reason = reason
+
+    def cancel(self) -> None:
+        """Mark the mission as cancelled (e.g. operator preempt)."""
+        self._state = self.CANCELED
+
+    # ── Query methods ──────────────────────────────────────────────────────
+
+    @property
+    def state(self) -> str:
+        return self._state
+
+    @property
+    def is_running(self) -> bool:
+        return self._state == self.RUNNING
+
+    @property
+    def is_done(self) -> bool:
+        return self._state in (self.SUCCEEDED, self.ABORTED, self.CANCELED)
+
+    @property
+    def current_waypoint(self) -> Optional[Waypoint]:
+        """The active goal, or None if not RUNNING or already done."""
+        if self._state != self.RUNNING:
+            return None
+        if self._current >= len(self._waypoints):
+            return None
+        return self._waypoints[self._current]
+
+    @property
+    def current_index(self) -> int:
+        """0-based index of the active waypoint (0 if not started)."""
+        return self._current
+
+    @property
+    def total(self) -> int:
+        """Total number of waypoints in the current mission."""
+        return len(self._waypoints)
+
+    @property
+    def progress(self) -> Tuple[int, int]:
+        """Return (completed_count, total_count)."""
+        return self._current, len(self._waypoints)
+
+    @property
+    def remaining(self) -> int:
+        return max(0, len(self._waypoints) - self._current)
+
+    @property
+    def abort_reason(self) -> Optional[str]:
+        return self._abort_reason
+
+    def status_dict(self) -> dict:
+        """Serialisable status snapshot for MQTT / diagnostics."""
+        wp = self.current_waypoint
+        return {
+            "state":         self._state,
+            "current_index": self._current,
+            "total":         len(self._waypoints),
+            "remaining":     self.remaining,
+            "current_wp":    wp.to_dict() if wp else None,
+            "abort_reason":  self._abort_reason,
+        }

jetson/ros2_ws/src/saltybot_nav2_uwb/setup.cfg

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

jetson/ros2_ws/src/saltybot_nav2_uwb/setup.py

@@ -0,0 +1,35 @@
+from setuptools import setup
+
+package_name = "saltybot_nav2_uwb"
+
+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/nav2_uwb.launch.py",
+        ]),
+        (f"share/{package_name}/config", [
+            "config/nav2_uwb_params.yaml",
+        ]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="sl-jetson",
+    maintainer_email="sl-jetson@saltylab.local",
+    description=(
+        "Nav2 with UWB localization: pose bridge (map→odom TF from UWB-IMU EKF) "
+        "and sequential waypoint follower (Issue #599)"
+    ),
+    license="Apache-2.0",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "uwb_pose_bridge_node  = saltybot_nav2_uwb.uwb_pose_bridge_node:main",
+            "waypoint_follower_node = saltybot_nav2_uwb.waypoint_follower_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_nav2_uwb/test/test_waypoint_follower.py

@@ -0,0 +1,459 @@
+"""test_waypoint_follower.py — Unit tests for Nav2-UWB waypoint logic (Issue #599).
+
+Tests are ROS2-free.  Run with:
+    python3 -m pytest \
+        jetson/ros2_ws/src/saltybot_nav2_uwb/test/test_waypoint_follower.py -v
+
+Coverage:
+    • Waypoint dataclass construction, validation, serialisation
+    • WaypointSequencer state machine (all transitions)
+    • Progress tracking
+    • UWB pose-bridge 2-D TF maths
+    • waypoint_to_pose_stamped conversion (quaternion from yaw)
+"""
+
+from __future__ import annotations
+
+import math
+import sys
+import os
+
+# Allow import without ROS2 install
+_pkg_dir = os.path.join(os.path.dirname(__file__), "..", "saltybot_nav2_uwb")
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
+sys.path.insert(0, _pkg_dir)
+
+import pytest
+
+# ── Stub ROS2 modules before importing any node files ────────────────────────
+
+def _install_stubs():
+    """Inject minimal stubs so node modules are importable without ROS2."""
+    from unittest.mock import MagicMock
+    import types
+
+    for mod_name in [
+        "rclpy", "rclpy.node", "rclpy.action", "rclpy.time",
+        "rclpy.callback_groups", "rclpy.executors",
+        "geometry_msgs", "geometry_msgs.msg",
+        "nav_msgs", "nav_msgs.msg",
+        "nav2_msgs", "nav2_msgs.action",
+        "std_msgs", "std_msgs.msg",
+        "tf2_ros", "action_msgs", "action_msgs.msg",
+    ]:
+        sys.modules.setdefault(mod_name, MagicMock())
+
+    # Make geometry_msgs.msg.PoseWithCovarianceStamped / TransformStamped real
+    # enough to be used as type hints
+    sys.modules["geometry_msgs.msg"].PoseWithCovarianceStamped = MagicMock
+    sys.modules["geometry_msgs.msg"].TransformStamped          = MagicMock
+    sys.modules["nav_msgs.msg"].Odometry                       = MagicMock
+    sys.modules["std_msgs.msg"].Empty                          = MagicMock
+    sys.modules["std_msgs.msg"].String                         = MagicMock
+    sys.modules["tf2_ros"].TransformBroadcaster                = MagicMock
+
+_install_stubs()
+
+from waypoint_sequencer import Waypoint, WaypointSequencer
+
+# ── helpers from uwb_pose_bridge_node (pure-python section) ──────────────────
+
+from uwb_pose_bridge_node import (
+    _yaw_from_q,
+    _q_from_yaw,
+    _invert_2d,
+    _compose_2d,
+    compute_map_odom_tf,
+)
+
+
+# ─── Waypoint dataclass ────────────────────────────────────────────────────────
+
+class TestWaypointConstruction:
+    def test_basic_construction(self):
+        wp = Waypoint(x=1.0, y=2.0, yaw_deg=45.0)
+        assert wp.x == 1.0
+        assert wp.y == 2.0
+        assert wp.yaw_deg == 45.0
+
+    def test_default_yaw_is_zero(self):
+        wp = Waypoint(x=0.0, y=0.0)
+        assert wp.yaw_deg == 0.0
+
+    def test_negative_coordinates(self):
+        wp = Waypoint(x=-3.5, y=-1.2, yaw_deg=-90.0)
+        assert wp.x == -3.5
+        assert wp.yaw_deg == -90.0
+
+    def test_yaw_boundary_360(self):
+        wp = Waypoint(x=0.0, y=0.0, yaw_deg=360.0)
+        assert wp.yaw_deg == 360.0
+
+    def test_yaw_boundary_neg360(self):
+        wp = Waypoint(x=0.0, y=0.0, yaw_deg=-360.0)
+        assert wp.yaw_deg == -360.0
+
+    def test_yaw_out_of_range_raises(self):
+        with pytest.raises(ValueError, match="yaw_deg"):
+            Waypoint(x=0.0, y=0.0, yaw_deg=400.0)
+
+    def test_yaw_out_of_range_negative_raises(self):
+        with pytest.raises(ValueError, match="yaw_deg"):
+            Waypoint(x=0.0, y=0.0, yaw_deg=-400.0)
+
+
+class TestWaypointFromDict:
+    def test_full_dict(self):
+        wp = Waypoint.from_dict({"x": 1.5, "y": -2.5, "yaw_deg": 30.0})
+        assert wp.x == 1.5
+        assert wp.y == -2.5
+        assert wp.yaw_deg == 30.0
+
+    def test_missing_yaw_defaults_zero(self):
+        wp = Waypoint.from_dict({"x": 0.0, "y": 1.0})
+        assert wp.yaw_deg == 0.0
+
+    def test_string_values_coerced(self):
+        wp = Waypoint.from_dict({"x": "1.0", "y": "2.0", "yaw_deg": "45"})
+        assert wp.x == 1.0
+
+    def test_missing_x_raises(self):
+        with pytest.raises(KeyError):
+            Waypoint.from_dict({"y": 1.0})
+
+    def test_missing_y_raises(self):
+        with pytest.raises(KeyError):
+            Waypoint.from_dict({"x": 1.0})
+
+
+class TestWaypointToDict:
+    def test_roundtrip(self):
+        wp = Waypoint(x=1.0, y=2.0, yaw_deg=90.0)
+        d = wp.to_dict()
+        wp2 = Waypoint.from_dict(d)
+        assert wp2.x == wp.x
+        assert wp2.y == wp.y
+        assert wp2.yaw_deg == wp.yaw_deg
+
+    def test_keys_present(self):
+        d = Waypoint(x=0.0, y=0.0).to_dict()
+        assert "x" in d and "y" in d and "yaw_deg" in d
+
+
+# ─── WaypointSequencer state machine ─────────────────────────────────────────
+
+def _three_wp():
+    return [Waypoint(float(i), 0.0) for i in range(3)]
+
+
+class TestSequencerInitialState:
+    def setup_method(self):
+        self.seq = WaypointSequencer()
+
+    def test_initial_state_idle(self):
+        assert self.seq.state == WaypointSequencer.IDLE
+
+    def test_is_not_running(self):
+        assert not self.seq.is_running
+
+    def test_is_not_done(self):
+        assert not self.seq.is_done
+
+    def test_no_current_waypoint(self):
+        assert self.seq.current_waypoint is None
+
+    def test_total_zero(self):
+        assert self.seq.total == 0
+
+    def test_progress_zero(self):
+        assert self.seq.progress == (0, 0)
+
+    def test_no_abort_reason(self):
+        assert self.seq.abort_reason is None
+
+
+class TestSequencerStart:
+    def setup_method(self):
+        self.seq = WaypointSequencer()
+
+    def test_transitions_to_running(self):
+        self.seq.start(_three_wp())
+        assert self.seq.state == WaypointSequencer.RUNNING
+
+    def test_is_running_true(self):
+        self.seq.start(_three_wp())
+        assert self.seq.is_running
+
+    def test_is_done_false(self):
+        self.seq.start(_three_wp())
+        assert not self.seq.is_done
+
+    def test_current_index_zero(self):
+        self.seq.start(_three_wp())
+        assert self.seq.current_index == 0
+
+    def test_total_set(self):
+        self.seq.start(_three_wp())
+        assert self.seq.total == 3
+
+    def test_first_waypoint_returned(self):
+        wps = _three_wp()
+        self.seq.start(wps)
+        assert self.seq.current_waypoint is wps[0]
+
+    def test_empty_list_raises(self):
+        with pytest.raises(ValueError, match="empty"):
+            self.seq.start([])
+
+    def test_restart_resets_state(self):
+        self.seq.start(_three_wp())
+        self.seq.advance()
+        self.seq.start([Waypoint(9.0, 9.0)])
+        assert self.seq.current_index == 0
+        assert self.seq.total == 1
+
+
+class TestSequencerAdvance:
+    def setup_method(self):
+        self.seq = WaypointSequencer()
+
+    def test_advance_moves_to_next(self):
+        self.seq.start(_three_wp())
+        self.seq.advance()
+        assert self.seq.current_index == 1
+
+    def test_advance_twice(self):
+        self.seq.start(_three_wp())
+        self.seq.advance()
+        self.seq.advance()
+        assert self.seq.current_index == 2
+
+    def test_advance_last_sets_succeeded(self):
+        self.seq.start(_three_wp())
+        for _ in range(3):
+            self.seq.advance()
+        assert self.seq.state == WaypointSequencer.SUCCEEDED
+
+    def test_advance_last_sets_done(self):
+        self.seq.start(_three_wp())
+        for _ in range(3):
+            self.seq.advance()
+        assert self.seq.is_done
+
+    def test_advance_on_idle_raises(self):
+        with pytest.raises(RuntimeError, match="RUNNING"):
+            self.seq.advance()
+
+    def test_advance_on_aborted_raises(self):
+        self.seq.start(_three_wp())
+        self.seq.abort()
+        with pytest.raises(RuntimeError, match="RUNNING"):
+            self.seq.advance()
+
+    def test_no_current_waypoint_after_succeed(self):
+        self.seq.start(_three_wp())
+        for _ in range(3):
+            self.seq.advance()
+        assert self.seq.current_waypoint is None
+
+
+class TestSequencerAbort:
+    def setup_method(self):
+        self.seq = WaypointSequencer()
+
+    def test_abort_transitions_state(self):
+        self.seq.start(_three_wp())
+        self.seq.abort("Nav2 failed")
+        assert self.seq.state == WaypointSequencer.ABORTED
+
+    def test_abort_sets_reason(self):
+        self.seq.start(_three_wp())
+        self.seq.abort("timeout")
+        assert self.seq.abort_reason == "timeout"
+
+    def test_abort_sets_done(self):
+        self.seq.start(_three_wp())
+        self.seq.abort()
+        assert self.seq.is_done
+
+    def test_abort_no_reason(self):
+        self.seq.start(_three_wp())
+        self.seq.abort()
+        assert self.seq.abort_reason == ""
+
+
+class TestSequencerCancel:
+    def setup_method(self):
+        self.seq = WaypointSequencer()
+
+    def test_cancel_transitions_state(self):
+        self.seq.start(_three_wp())
+        self.seq.cancel()
+        assert self.seq.state == WaypointSequencer.CANCELED
+
+    def test_cancel_sets_done(self):
+        self.seq.start(_three_wp())
+        self.seq.cancel()
+        assert self.seq.is_done
+
+    def test_cancel_clears_current_waypoint(self):
+        self.seq.start(_three_wp())
+        self.seq.cancel()
+        assert self.seq.current_waypoint is None
+
+
+class TestSequencerProgress:
+    def setup_method(self):
+        self.seq = WaypointSequencer()
+        self.seq.start(_three_wp())
+
+    def test_initial_progress(self):
+        assert self.seq.progress == (0, 3)
+
+    def test_progress_after_one(self):
+        self.seq.advance()
+        assert self.seq.progress == (1, 3)
+
+    def test_remaining_initial(self):
+        assert self.seq.remaining == 3
+
+    def test_remaining_after_advance(self):
+        self.seq.advance()
+        assert self.seq.remaining == 2
+
+    def test_remaining_after_all(self):
+        for _ in range(3):
+            self.seq.advance()
+        assert self.seq.remaining == 0
+
+
+class TestSequencerStatusDict:
+    def setup_method(self):
+        self.seq = WaypointSequencer()
+
+    def test_idle_status_dict(self):
+        d = self.seq.status_dict()
+        assert d["state"] == "idle"
+        assert d["total"] == 0
+        assert d["current_wp"] is None
+
+    def test_running_status_dict(self):
+        self.seq.start(_three_wp())
+        d = self.seq.status_dict()
+        assert d["state"] == "running"
+        assert d["total"] == 3
+        assert d["current_wp"] is not None
+        assert "x" in d["current_wp"]
+
+    def test_succeeded_status_dict(self):
+        self.seq.start(_three_wp())
+        for _ in range(3):
+            self.seq.advance()
+        d = self.seq.status_dict()
+        assert d["state"] == "succeeded"
+        assert d["remaining"] == 0
+
+
+# ─── 2-D TF maths (uwb_pose_bridge_node) ──────────────────────────────────────
+
+EPS = 1e-9
+
+
+class TestYawFromQ:
+    def test_zero_yaw(self):
+        qx, qy, qz, qw = _q_from_yaw(0.0)
+        assert abs(_yaw_from_q(qx, qy, qz, qw)) < EPS
+
+    def test_90_degrees(self):
+        yaw = math.pi / 2
+        qx, qy, qz, qw = _q_from_yaw(yaw)
+        assert abs(_yaw_from_q(qx, qy, qz, qw) - yaw) < 1e-6
+
+    def test_minus_90_degrees(self):
+        yaw = -math.pi / 2
+        qx, qy, qz, qw = _q_from_yaw(yaw)
+        assert abs(_yaw_from_q(qx, qy, qz, qw) - yaw) < 1e-6
+
+    def test_180_degrees(self):
+        yaw = math.pi
+        qx, qy, qz, qw = _q_from_yaw(yaw)
+        # atan2 wraps to -π at exactly π; tolerate either
+        result = _yaw_from_q(qx, qy, qz, qw)
+        assert abs(abs(result) - math.pi) < 1e-6
+
+    def test_roundtrip_arbitrary(self):
+        for deg in (0, 30, 60, 90, 120, 150, 170, -45, -120):
+            yaw = math.radians(deg)
+            qx, qy, qz, qw = _q_from_yaw(yaw)
+            back = _yaw_from_q(qx, qy, qz, qw)
+            assert abs(back - yaw) < 1e-6, f"roundtrip failed for {deg}°"
+
+
+class TestInvert2D:
+    def test_identity(self):
+        xi, yi, thi = _invert_2d(0.0, 0.0, 0.0)
+        assert abs(xi) < EPS and abs(yi) < EPS and abs(thi) < EPS
+
+    def test_translation_only(self):
+        # invert pure translation: inv(T(3,4,0)) = T(-3,-4,0)
+        xi, yi, thi = _invert_2d(3.0, 4.0, 0.0)
+        assert abs(xi + 3.0) < EPS
+        assert abs(yi + 4.0) < EPS
+        assert abs(thi) < EPS
+
+    def test_rotation_only(self):
+        # invert pure rotation by 90°
+        xi, yi, thi = _invert_2d(0.0, 0.0, math.pi / 2)
+        assert abs(thi + math.pi / 2) < EPS
+
+    def test_compose_with_inverse_is_identity(self):
+        x, y, th = 2.5, -1.3, math.radians(47)
+        ix, iy, ith = _invert_2d(x, y, th)
+        rx, ry, rth = _compose_2d(x, y, th, ix, iy, ith)
+        assert abs(rx) < 1e-9
+        assert abs(ry) < 1e-9
+        assert abs(rth % (2 * math.pi)) < 1e-9 or abs(rth) < 1e-9
+
+
+class TestComputeMapOdomTf:
+    def test_identity_pose(self):
+        """If both poses are identity, map→odom is identity."""
+        mx, my, mth = compute_map_odom_tf(0, 0, 0, 0, 0, 0)
+        assert abs(mx) < EPS and abs(my) < EPS and abs(mth) < EPS
+
+    def test_odom_is_map(self):
+        """If robot is at same position in both frames, map→odom is identity."""
+        pose = (1.0, 2.0, math.radians(45))
+        mx, my, mth = compute_map_odom_tf(*pose, *pose)
+        assert abs(mx) < 1e-9 and abs(my) < 1e-9 and abs(mth) < 1e-9
+
+    def test_pure_translation_offset(self):
+        """map_bl at (5,0,0), odom_bl at (2,0,0) → map_odom at (3,0,0)."""
+        mx, my, mth = compute_map_odom_tf(5.0, 0.0, 0.0, 2.0, 0.0, 0.0)
+        assert abs(mx - 3.0) < 1e-9
+        assert abs(my) < 1e-9
+        assert abs(mth) < 1e-9
+
+    def test_tf_reconstruction(self):
+        """T_map_odom ⊗ T_odom_bl == T_map_bl (round-trip check)."""
+        map_bl  = (3.0, 1.5, math.radians(30))
+        odom_bl = (1.0, 0.5, math.radians(10))
+        mo = compute_map_odom_tf(*map_bl, *odom_bl)
+        # Recompose: T_map_odom ⊗ T_odom_bl should equal T_map_bl
+        rx, ry, rth = _compose_2d(*mo, *odom_bl)
+        assert abs(rx - map_bl[0]) < 1e-9
+        assert abs(ry - map_bl[1]) < 1e-9
+        assert abs(rth - map_bl[2]) < 1e-9
+
+    def test_rotated_frame(self):
+        """Verify correctness with a 90° heading offset."""
+        # map: robot at (1,0) facing 90°; odom: robot at (0,1) facing 0°
+        mo_x, mo_y, mo_th = compute_map_odom_tf(
+            1.0, 0.0, math.pi / 2,
+            0.0, 1.0, 0.0,
+        )
+        # Recompose and check
+        rx, ry, rth = _compose_2d(mo_x, mo_y, mo_th, 0.0, 1.0, 0.0)
+        assert abs(rx - 1.0) < 1e-9
+        assert abs(ry - 0.0) < 1e-9
+        assert abs(rth - math.pi / 2) < 1e-9

jetson/ros2_ws/src/saltybot_phone/launch/mqtt_bridge.launch.py

@@ -0,0 +1,69 @@
+#!/usr/bin/env python3
+"""
+mqtt_bridge.launch.py — Launch the MQTT-to-ROS2 phone sensor bridge (Issue #601)
+
+Usage:
+  ros2 launch saltybot_phone mqtt_bridge.launch.py
+  ros2 launch saltybot_phone mqtt_bridge.launch.py mqtt_host:=192.168.1.100
+  ros2 launch saltybot_phone mqtt_bridge.launch.py use_phone_timestamp:=true warn_drift_s:=0.5
+"""
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    args = [
+        DeclareLaunchArgument(
+            "mqtt_host", default_value="localhost",
+            description="MQTT broker IP/hostname",
+        ),
+        DeclareLaunchArgument(
+            "mqtt_port", default_value="1883",
+            description="MQTT broker port",
+        ),
+        DeclareLaunchArgument(
+            "reconnect_delay_s", default_value="5.0",
+            description="Seconds between MQTT reconnect attempts",
+        ),
+        DeclareLaunchArgument(
+            "use_phone_timestamp", default_value="false",
+            description="Use phone epoch ts for ROS2 header stamp",
+        ),
+        DeclareLaunchArgument(
+            "warn_drift_s", default_value="1.0",
+            description="Clock drift threshold for warning (s)",
+        ),
+        DeclareLaunchArgument(
+            "imu_accel_cov", default_value="0.01",
+            description="IMU accelerometer diagonal covariance (m/s²)²",
+        ),
+        DeclareLaunchArgument(
+            "imu_gyro_cov", default_value="0.001",
+            description="IMU gyroscope diagonal covariance (rad/s)²",
+        ),
+    ]
+
+    bridge_node = Node(
+        package="saltybot_phone",
+        executable="mqtt_ros2_bridge",
+        name="mqtt_ros2_bridge",
+        parameters=[{
+            "mqtt_host":           LaunchConfiguration("mqtt_host"),
+            "mqtt_port":           LaunchConfiguration("mqtt_port"),
+            "reconnect_delay_s":   LaunchConfiguration("reconnect_delay_s"),
+            "frame_id_imu":        "phone_imu",
+            "frame_id_gps":        "phone_gps",
+            "use_phone_timestamp": LaunchConfiguration("use_phone_timestamp"),
+            "warn_drift_s":        LaunchConfiguration("warn_drift_s"),
+            "imu_accel_cov":       LaunchConfiguration("imu_accel_cov"),
+            "imu_gyro_cov":        LaunchConfiguration("imu_gyro_cov"),
+            "gps_alt_cov_factor":  4.0,
+            "status_hz":           0.2,
+        }],
+        output="screen",
+        emulate_tty=True,
+    )
+
+    return LaunchDescription(args + [bridge_node])

jetson/ros2_ws/src/saltybot_phone/package.xml

@@ -15,6 +15,7 @@
   <exec_depend>python3-numpy</exec_depend>
   <exec_depend>python3-opencv</exec_depend>
   <exec_depend>python3-launch-ros</exec_depend>
+  <exec_depend>python3-paho-mqtt</exec_depend>
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

jetson/ros2_ws/src/saltybot_phone/saltybot_phone/mqtt_ros2_bridge_node.py

@@ -0,0 +1,485 @@
+#!/usr/bin/env python3
+"""
+mqtt_ros2_bridge_node.py — MQTT-to-ROS2 bridge for Termux phone sensors (Issue #601)
+
+Subscribes to the three MQTT topics published by phone/sensor_dashboard.py and
+republishes each as a proper ROS2 message type on the Jetson.
+
+MQTT topics consumed (sensor_dashboard.py JSON format)
+───────────────────────────────────────────────────────
+  saltybot/phone/imu      → sensor_msgs/Imu
+  saltybot/phone/gps      → sensor_msgs/NavSatFix
+  saltybot/phone/battery  → sensor_msgs/BatteryState
+
+ROS2 topics published
+──────────────────────
+  /saltybot/phone/imu               sensor_msgs/Imu
+  /saltybot/phone/gps               sensor_msgs/NavSatFix
+  /saltybot/phone/battery           sensor_msgs/BatteryState
+  /saltybot/phone/bridge/status     std_msgs/String  (JSON health/stats)
+
+Timestamp alignment
+────────────────────
+  Header stamp = ROS2 wall clock (monotonic, consistent with rest of system).
+  Phone epoch ts from JSON is cross-checked to detect clock drift > warn_drift_s;
+  if drift is within tolerance the phone timestamp can be used instead by
+  setting use_phone_timestamp:=true.
+
+Parameters
+──────────
+  mqtt_host          str   Broker IP/hostname         (default: localhost)
+  mqtt_port          int   Broker port                (default: 1883)
+  mqtt_keepalive     int   Keepalive seconds          (default: 60)
+  reconnect_delay_s  float Delay between reconnects   (default: 5.0)
+  frame_id_imu       str   TF frame for IMU           (default: phone_imu)
+  frame_id_gps       str   TF frame for GPS           (default: phone_gps)
+  use_phone_timestamp bool Use phone ts for header    (default: false)
+  warn_drift_s       float Clock drift warning thresh  (default: 1.0)
+  imu_accel_cov      float Accel diagonal covariance  (default: 0.01)
+  imu_gyro_cov       float Gyro diagonal covariance   (default: 0.001)
+  gps_alt_cov_factor float Altitude cov = acc*factor  (default: 4.0)
+  status_hz          float Bridge status publish rate  (default: 0.2)
+
+Dependencies
+────────────
+  pip install paho-mqtt   (on Jetson)
+"""
+
+import json
+import math
+import queue
+import threading
+import time
+from typing import Any
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import BatteryState, Imu, NavSatFix, NavSatStatus
+from std_msgs.msg import Header, String
+
+try:
+    import paho.mqtt.client as mqtt
+    _MQTT_OK = True
+except ImportError:
+    _MQTT_OK = False
+
+# ── MQTT topic constants (must match sensor_dashboard.py) ────────────────────
+
+_TOPIC_IMU     = "saltybot/phone/imu"
+_TOPIC_GPS     = "saltybot/phone/gps"
+_TOPIC_BATTERY = "saltybot/phone/battery"
+
+# ── BatteryState health/power-supply constants ───────────────────────────────
+
+_HEALTH_MAP = {
+    "GOOD":              BatteryState.POWER_SUPPLY_HEALTH_GOOD,
+    "OVERHEAT":          BatteryState.POWER_SUPPLY_HEALTH_OVERHEAT,
+    "DEAD":              BatteryState.POWER_SUPPLY_HEALTH_DEAD,
+    "OVER_VOLTAGE":      BatteryState.POWER_SUPPLY_HEALTH_OVERVOLTAGE,
+    "UNSPECIFIED_FAILURE": BatteryState.POWER_SUPPLY_HEALTH_UNSPEC_FAILURE,
+    "COLD":              BatteryState.POWER_SUPPLY_HEALTH_COLD,
+    "WATCHDOG_TIMER_EXPIRE": BatteryState.POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE,
+    "SAFETY_TIMER_EXPIRE":   BatteryState.POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE,
+}
+
+_PLUGGED_MAP = {
+    "AC":      BatteryState.POWER_SUPPLY_STATUS_CHARGING,
+    "USB":     BatteryState.POWER_SUPPLY_STATUS_CHARGING,
+    "WIRELESS": BatteryState.POWER_SUPPLY_STATUS_CHARGING,
+    "UNPLUGGED": BatteryState.POWER_SUPPLY_STATUS_DISCHARGING,
+}
+
+
+class MqttRos2BridgeNode(Node):
+    """
+    Bridges phone sensor MQTT topics into ROS2.
+
+    Architecture:
+      - paho-mqtt loop_start() runs the MQTT network thread independently.
+      - on_message callback enqueues raw (topic, payload) pairs.
+      - A ROS2 timer drains the queue and publishes ROS2 messages, keeping
+        all ROS2 operations on the executor thread.
+    """
+
+    def __init__(self) -> None:
+        super().__init__("mqtt_ros2_bridge")
+
+        # ── Parameters ────────────────────────────────────────────────────────
+        self.declare_parameter("mqtt_host",          "localhost")
+        self.declare_parameter("mqtt_port",          1883)
+        self.declare_parameter("mqtt_keepalive",     60)
+        self.declare_parameter("reconnect_delay_s",  5.0)
+        self.declare_parameter("frame_id_imu",       "phone_imu")
+        self.declare_parameter("frame_id_gps",       "phone_gps")
+        self.declare_parameter("use_phone_timestamp", False)
+        self.declare_parameter("warn_drift_s",       1.0)
+        self.declare_parameter("imu_accel_cov",      0.01)
+        self.declare_parameter("imu_gyro_cov",       0.001)
+        self.declare_parameter("gps_alt_cov_factor", 4.0)
+        self.declare_parameter("status_hz",          0.2)
+
+        p = self._p = self._load_params()
+
+        if not _MQTT_OK:
+            self.get_logger().error(
+                "paho-mqtt not installed. Run: pip install paho-mqtt"
+            )
+
+        # ── Publishers ────────────────────────────────────────────────────────
+        self._imu_pub     = self.create_publisher(Imu,          "/saltybot/phone/imu",             10)
+        self._gps_pub     = self.create_publisher(NavSatFix,    "/saltybot/phone/gps",             10)
+        self._bat_pub     = self.create_publisher(BatteryState, "/saltybot/phone/battery",         10)
+        self._status_pub  = self.create_publisher(String,       "/saltybot/phone/bridge/status",   10)
+
+        # ── Message queue (MQTT thread → ROS2 executor thread) ────────────────
+        self._q: queue.Queue[tuple[str, bytes]] = queue.Queue(maxsize=200)
+
+        # ── Stats ─────────────────────────────────────────────────────────────
+        self._rx_count     = {_TOPIC_IMU: 0, _TOPIC_GPS: 0, _TOPIC_BATTERY: 0}
+        self._pub_count    = {_TOPIC_IMU: 0, _TOPIC_GPS: 0, _TOPIC_BATTERY: 0}
+        self._err_count    = {_TOPIC_IMU: 0, _TOPIC_GPS: 0, _TOPIC_BATTERY: 0}
+        self._last_rx_ts   = {_TOPIC_IMU: 0.0, _TOPIC_GPS: 0.0, _TOPIC_BATTERY: 0.0}
+        self._mqtt_connected = False
+
+        # ── Timers ────────────────────────────────────────────────────────────
+        # Drain queue at 50 Hz (>> sensor rates so latency stays low)
+        self.create_timer(0.02, self._drain_queue)
+        # Status publisher
+        self.create_timer(1.0 / max(p["status_hz"], 0.01), self._publish_status)
+
+        # ── MQTT client ───────────────────────────────────────────────────────
+        if _MQTT_OK:
+            self._mqtt = mqtt.Client(
+                client_id="saltybot-mqtt-ros2-bridge", clean_session=True
+            )
+            self._mqtt.on_connect    = self._on_mqtt_connect
+            self._mqtt.on_disconnect = self._on_mqtt_disconnect
+            self._mqtt.on_message    = self._on_mqtt_message
+            self._mqtt.reconnect_delay_set(
+                min_delay=int(p["reconnect_delay_s"]),
+                max_delay=int(p["reconnect_delay_s"]) * 4,
+            )
+            self._mqtt_connect()
+
+        self.get_logger().info(
+            "MQTT→ROS2 bridge started — broker=%s:%d  use_phone_ts=%s",
+            p["mqtt_host"], p["mqtt_port"], p["use_phone_timestamp"],
+        )
+
+    # ── Param helper ─────────────────────────────────────────────────────────
+
+    def _load_params(self) -> dict:
+        return {
+            "mqtt_host":          self.get_parameter("mqtt_host").value,
+            "mqtt_port":          self.get_parameter("mqtt_port").value,
+            "mqtt_keepalive":     self.get_parameter("mqtt_keepalive").value,
+            "reconnect_delay_s":  self.get_parameter("reconnect_delay_s").value,
+            "frame_id_imu":       self.get_parameter("frame_id_imu").value,
+            "frame_id_gps":       self.get_parameter("frame_id_gps").value,
+            "use_phone_ts":       self.get_parameter("use_phone_timestamp").value,
+            "warn_drift_s":       self.get_parameter("warn_drift_s").value,
+            "imu_accel_cov":      self.get_parameter("imu_accel_cov").value,
+            "imu_gyro_cov":       self.get_parameter("imu_gyro_cov").value,
+            "gps_alt_cov_factor": self.get_parameter("gps_alt_cov_factor").value,
+            "status_hz":          self.get_parameter("status_hz").value,
+        }
+
+    # ── MQTT connection ───────────────────────────────────────────────────────
+
+    def _mqtt_connect(self) -> None:
+        try:
+            self._mqtt.connect_async(
+                self._p["mqtt_host"],
+                self._p["mqtt_port"],
+                keepalive=self._p["mqtt_keepalive"],
+            )
+            self._mqtt.loop_start()
+        except Exception as e:
+            self.get_logger().warning("MQTT initial connect error: %s", str(e))
+
+    def _on_mqtt_connect(self, client, userdata, flags, rc) -> None:
+        if rc == 0:
+            self._mqtt_connected = True
+            # Subscribe to all phone sensor topics
+            for topic in (_TOPIC_IMU, _TOPIC_GPS, _TOPIC_BATTERY):
+                client.subscribe(topic, qos=0)
+            self.get_logger().info(
+                "MQTT connected to %s:%d — subscribed to 3 phone topics",
+                self._p["mqtt_host"], self._p["mqtt_port"],
+            )
+        else:
+            self.get_logger().warning("MQTT connect failed rc=%d", rc)
+
+    def _on_mqtt_disconnect(self, client, userdata, rc) -> None:
+        self._mqtt_connected = False
+        if rc != 0:
+            self.get_logger().warning(
+                "MQTT disconnected (rc=%d) — paho will reconnect", rc
+            )
+
+    def _on_mqtt_message(self, client, userdata, message) -> None:
+        """Called in paho network thread — only enqueue, never publish here."""
+        topic = message.topic
+        if topic in self._rx_count:
+            self._rx_count[topic] += 1
+            self._last_rx_ts[topic] = time.time()
+        try:
+            self._q.put_nowait((topic, message.payload))
+        except queue.Full:
+            self.get_logger().debug("MQTT queue full — dropping %s", topic)
+
+    # ── Queue drain (ROS2 executor thread) ────────────────────────────────────
+
+    def _drain_queue(self) -> None:
+        """Drain up to 50 queued messages per timer tick to bound latency."""
+        for _ in range(50):
+            try:
+                topic, payload = self._q.get_nowait()
+            except queue.Empty:
+                break
+            self._dispatch(topic, payload)
+
+    def _dispatch(self, topic: str, payload: bytes) -> None:
+        try:
+            data = json.loads(payload)
+        except (json.JSONDecodeError, UnicodeDecodeError) as e:
+            self._err_count.get(topic, {})  # just access
+            if topic in self._err_count:
+                self._err_count[topic] += 1
+            self.get_logger().debug("JSON decode error on %s: %s", topic, e)
+            return
+
+        try:
+            if topic == _TOPIC_IMU:
+                self._handle_imu(data)
+            elif topic == _TOPIC_GPS:
+                self._handle_gps(data)
+            elif topic == _TOPIC_BATTERY:
+                self._handle_battery(data)
+        except (KeyError, TypeError, ValueError) as e:
+            if topic in self._err_count:
+                self._err_count[topic] += 1
+            self.get_logger().debug("Payload error on %s: %s — %s", topic, e, data)
+
+    # ── Timestamp helper ──────────────────────────────────────────────────────
+
+    def _make_header(self, data: dict, frame_id: str) -> Header:
+        """
+        Build a ROS2 Header.
+
+        By default uses the ROS2 wall clock so timestamps are consistent with
+        other nodes.  If use_phone_ts is true and the phone's `ts` is within
+        warn_drift_s of wall time, uses the phone's epoch timestamp instead.
+        """
+        hdr = Header()
+        hdr.frame_id = frame_id
+
+        phone_ts: float = float(data.get("ts", 0.0))
+        now_epoch = time.time()
+
+        if self._p["use_phone_ts"] and phone_ts > 0.0:
+            drift = abs(now_epoch - phone_ts)
+            if drift > self._p["warn_drift_s"]:
+                self.get_logger().warning(
+                    "Phone clock drift %.2f s (frame_id=%s) — using ROS2 clock",
+                    drift, frame_id,
+                )
+                hdr.stamp = self.get_clock().now().to_msg()
+            else:
+                # Convert phone epoch to ROS2 Time
+                sec  = int(phone_ts)
+                nsec = int((phone_ts - sec) * 1e9)
+                hdr.stamp.sec     = sec
+                hdr.stamp.nanosec = nsec
+        else:
+            hdr.stamp = self.get_clock().now().to_msg()
+
+        return hdr
+
+    # ── IMU handler ───────────────────────────────────────────────────────────
+
+    def _handle_imu(self, data: dict) -> None:
+        """
+        JSON: {"ts":..., "accel":{"x","y","z"}, "gyro":{"x","y","z"}}
+        Accel units: m/s²  Gyro units: rad/s
+        """
+        accel = data["accel"]
+        gyro  = data["gyro"]
+
+        # Validate finite values
+        for v in (accel["x"], accel["y"], accel["z"],
+                  gyro["x"],  gyro["y"],  gyro["z"]):
+            if not math.isfinite(float(v)):
+                raise ValueError(f"non-finite IMU value: {v}")
+
+        cov_a = self._p["imu_accel_cov"]
+        cov_g = self._p["imu_gyro_cov"]
+
+        msg = Imu()
+        msg.header = self._make_header(data, self._p["frame_id_imu"])
+
+        msg.linear_acceleration.x = float(accel["x"])
+        msg.linear_acceleration.y = float(accel["y"])
+        msg.linear_acceleration.z = float(accel["z"])
+        msg.linear_acceleration_covariance = [
+            cov_a, 0.0,   0.0,
+            0.0,   cov_a, 0.0,
+            0.0,   0.0,   cov_a,
+        ]
+
+        msg.angular_velocity.x = float(gyro["x"])
+        msg.angular_velocity.y = float(gyro["y"])
+        msg.angular_velocity.z = float(gyro["z"])
+        msg.angular_velocity_covariance = [
+            cov_g, 0.0,   0.0,
+            0.0,   cov_g, 0.0,
+            0.0,   0.0,   cov_g,
+        ]
+
+        # Orientation unknown — set covariance[0] = -1 per REP-145
+        msg.orientation_covariance[0] = -1.0
+
+        self._imu_pub.publish(msg)
+        self._pub_count[_TOPIC_IMU] += 1
+
+    # ── GPS handler ───────────────────────────────────────────────────────────
+
+    def _handle_gps(self, data: dict) -> None:
+        """
+        JSON: {"ts":..., "lat","lon","alt_m","accuracy_m","speed_ms","bearing_deg","provider"}
+        """
+        lat = float(data["lat"])
+        lon = float(data["lon"])
+        alt = float(data.get("alt_m", 0.0))
+        acc = float(data.get("accuracy_m", -1.0))
+
+        if not (-90.0 <= lat <= 90.0):
+            raise ValueError(f"invalid latitude: {lat}")
+        if not (-180.0 <= lon <= 180.0):
+            raise ValueError(f"invalid longitude: {lon}")
+
+        msg = NavSatFix()
+        msg.header = self._make_header(data, self._p["frame_id_gps"])
+
+        msg.latitude  = lat
+        msg.longitude = lon
+        msg.altitude  = alt
+
+        # Status
+        provider = data.get("provider", "unknown").lower()
+        msg.status.service = NavSatStatus.SERVICE_GPS
+        if provider in ("gps", "fused"):
+            msg.status.status = NavSatStatus.STATUS_FIX
+        elif provider == "network":
+            msg.status.status = NavSatStatus.STATUS_FIX
+            msg.status.service = NavSatStatus.SERVICE_COMPASS
+        else:
+            msg.status.status = NavSatStatus.STATUS_NO_FIX
+
+        # Covariance
+        if acc > 0.0:
+            h_var = acc * acc
+            v_var = h_var * self._p["gps_alt_cov_factor"]
+            msg.position_covariance = [
+                h_var, 0.0,   0.0,
+                0.0,   h_var, 0.0,
+                0.0,   0.0,   v_var,
+            ]
+            msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
+        else:
+            msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
+
+        self._gps_pub.publish(msg)
+        self._pub_count[_TOPIC_GPS] += 1
+
+    # ── Battery handler ───────────────────────────────────────────────────────
+
+    def _handle_battery(self, data: dict) -> None:
+        """
+        JSON: {"ts":..., "pct","charging","temp_c","health","plugged"}
+        """
+        pct      = int(data["pct"])
+        charging = bool(data.get("charging", False))
+        temp_c   = float(data.get("temp_c", float("nan")))
+        health   = str(data.get("health", "UNKNOWN")).upper()
+        plugged  = str(data.get("plugged", "UNPLUGGED")).upper()
+
+        if not (0 <= pct <= 100):
+            raise ValueError(f"invalid battery percentage: {pct}")
+
+        msg = BatteryState()
+        msg.header = self._make_header(data, "phone_battery")
+
+        msg.percentage      = float(pct) / 100.0
+        msg.temperature     = temp_c + 273.15 if math.isfinite(temp_c) else float("nan")
+        msg.present         = True
+        msg.power_supply_technology = BatteryState.POWER_SUPPLY_TECHNOLOGY_LION
+
+        msg.power_supply_status = (
+            BatteryState.POWER_SUPPLY_STATUS_CHARGING
+            if charging else
+            BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
+        )
+
+        msg.power_supply_health = _HEALTH_MAP.get(
+            health, BatteryState.POWER_SUPPLY_HEALTH_UNKNOWN
+        )
+
+        # Voltage / current unknown on Android
+        msg.voltage = float("nan")
+        msg.current = float("nan")
+        msg.charge  = float("nan")
+        msg.capacity = float("nan")
+        msg.design_capacity = float("nan")
+
+        msg.location = "phone"
+        msg.serial_number = ""
+
+        self._bat_pub.publish(msg)
+        self._pub_count[_TOPIC_BATTERY] += 1
+
+    # ── Status publisher ─────────────────────────────────────────────────────
+
+    def _publish_status(self) -> None:
+        now = time.time()
+        ages = {
+            t: round(now - self._last_rx_ts[t], 1) if self._last_rx_ts[t] > 0 else -1.0
+            for t in (_TOPIC_IMU, _TOPIC_GPS, _TOPIC_BATTERY)
+        }
+        body = {
+            "mqtt_connected": self._mqtt_connected,
+            "rx":  dict(self._rx_count),
+            "pub": dict(self._pub_count),
+            "err": dict(self._err_count),
+            "age_s": {t.split("/")[-1]: ages[t] for t in ages},
+            "queue_depth": self._q.qsize(),
+        }
+        msg = String()
+        msg.data = json.dumps(body, separators=(",", ":"))
+        self._status_pub.publish(msg)
+
+    # ── Cleanup ───────────────────────────────────────────────────────────────
+
+    def destroy_node(self) -> None:
+        if _MQTT_OK and hasattr(self, "_mqtt"):
+            self._mqtt.loop_stop()
+            self._mqtt.disconnect()
+        super().destroy_node()
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main(args: Any = None) -> None:
+    rclpy.init(args=args)
+    node = MqttRos2BridgeNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_phone/setup.py

@@ -29,6 +29,7 @@ setup(
             'openclaw_chat = saltybot_phone.openclaw_chat_node:main',
             'phone_bridge = saltybot_phone.ws_bridge:main',
             'phone_camera_node = saltybot_phone.phone_camera_node:main',
+            'mqtt_ros2_bridge = saltybot_phone.mqtt_ros2_bridge_node:main',
         ],
     },
 )

jetson/ros2_ws/src/saltybot_pose_fusion/config/pose_fusion_params.yaml

@@ -0,0 +1,30 @@
+pose_fusion:
+  ros__parameters:
+    # ── Sensor weights ──────────────────────────────────────────────────────
+    # UWB+IMU fused pose (/saltybot/pose/fused_cov)
+    sigma_uwb_pos_m:      0.10    # position 1-σ (m) — used when use_uwb_covariance=false
+    use_uwb_covariance:   true    # if true, extract σ from PoseWithCovarianceStamped.covariance
+    sigma_uwb_head_rad:   0.15    # heading 1-σ (rad) — used when cov unavailable
+
+    # Visual odometry (/saltybot/visual_odom)
+    sigma_vo_vel_m_s:     0.05    # linear-velocity update 1-σ (m/s)
+    sigma_vo_omega_r_s:   0.03    # yaw-rate update 1-σ (rad/s)
+
+    # IMU process noise (/imu/data)
+    sigma_imu_accel:      0.05    # accelerometer noise (m/s²)
+    sigma_imu_gyro:       0.003   # gyroscope noise (rad/s)
+    sigma_vel_drift:      0.10    # velocity random-walk process noise (m/s)
+    dropout_vel_damp:     0.95    # velocity damping factor per second during UWB dropout
+
+    # ── Dropout thresholds ──────────────────────────────────────────────────
+    uwb_dropout_warn_s:   2.0     # log warning if UWB silent > this (s)
+    uwb_dropout_max_s:    10.0    # suppress pose output if UWB silent > this (s)
+    vo_dropout_warn_s:    1.0     # log warning if VO silent > this (s)
+
+    # ── Topic / frame configuration ─────────────────────────────────────────
+    imu_topic:            /imu/data
+    uwb_pose_topic:       /saltybot/pose/fused_cov   # from saltybot_uwb_imu_fusion
+    vo_topic:             /saltybot/visual_odom       # from saltybot_visual_odom
+
+    map_frame:            map
+    base_frame:           base_link

jetson/ros2_ws/src/saltybot_pose_fusion/launch/pose_fusion.launch.py

@@ -0,0 +1,29 @@
+"""pose_fusion.launch.py — Launch the multi-sensor pose fusion node (Issue #595)."""
+
+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_pose_fusion')
+    default_params = os.path.join(pkg_share, 'config', 'pose_fusion_params.yaml')
+
+    params_arg = DeclareLaunchArgument(
+        'params_file',
+        default_value=default_params,
+        description='Path to pose_fusion_params.yaml',
+    )
+
+    pose_fusion_node = Node(
+        package='saltybot_pose_fusion',
+        executable='pose_fusion',
+        name='pose_fusion',
+        output='screen',
+        parameters=[LaunchConfiguration('params_file')],
+    )
+
+    return LaunchDescription([params_arg, pose_fusion_node])

jetson/ros2_ws/src/saltybot_pose_fusion/package.xml

@@ -0,0 +1,35 @@
+<?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_pose_fusion</name>
+  <version>0.1.0</version>
+  <description>
+    Multi-sensor EKF pose fusion node (Issue #595).
+    Fuses UWB+IMU absolute pose (/saltybot/pose/fused_cov), visual odometry
+    (/saltybot/visual_odom), and raw IMU (/imu/data) into a single authoritative
+    map-frame PoseWithCovarianceStamped on /saltybot/pose/authoritative with
+    TF2 map→base_link broadcast.
+  </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>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2_ros</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_pose_fusion/saltybot_pose_fusion/pose_fusion_ekf.py

@@ -0,0 +1,377 @@
+"""
+pose_fusion_ekf.py — Extended Kalman Filter for multi-sensor pose fusion (Issue #595).
+
+No ROS2 dependencies — pure Python + numpy, fully unit-testable.
+
+Fuses three sensor streams
+──────────────────────────
+  1. IMU (200 Hz) — predict step (body-frame accel + gyro)
+  2. UWB+IMU fused pose (10 Hz) — absolute position and heading update
+  3. Visual odometry (30 Hz) — velocity-domain update (drift-free short-term motion)
+
+State vector [x, y, θ, vx, vy, ω]ᵀ
+────────────────────────────────────
+  x   — position, forward    (m, map frame)
+  y   — position, lateral    (m, map frame)
+  θ   — heading, yaw         (rad, CCW positive)
+  vx  — velocity x           (m/s, map frame)
+  vy  — velocity y           (m/s, map frame)
+  ω   — yaw rate             (rad/s)
+
+Process model (IMU as control input):
+  θ_{k+1}  = θ_k + ω_imu * dt
+  vx_{k+1} = vx_k + (ax_body*cos θ - ay_body*sin θ) * dt
+  vy_{k+1} = vy_k + (ax_body*sin θ + ay_body*cos θ) * dt
+  x_{k+1}  = x_k  + vx_k * dt
+  y_{k+1}  = y_k  + vy_k * dt
+  ω_{k+1}  = ω_imu
+
+Measurement models
+──────────────────
+  UWB position:  z = [x, y]             H[2×6] = [[1,0,0,0,0,0],[0,1,0,0,0,0]]
+  UWB heading:   z = [θ]                H[1×6] = [[0,0,1,0,0,0]]
+  VO velocity:   z = [vx_w, vy_w, ω]   H[3×6] = diag of [vx,vy,ω] rows in state
+
+  VO velocity measurement: VO gives vx_body (forward speed) and ω.
+  We rotate to world frame: vx_w = vx_body*cos(θ), vy_w = vx_body*sin(θ)
+  then update [vx, vy, ω] state directly (linear measurement step).
+
+Sensor dropout handling
+───────────────────────
+  _uwb_dropout_s: seconds since last UWB update
+  _vo_dropout_s:  seconds since last VO update
+  Both increment during predict(), reset to 0 on update.
+  When uwb_dropout_s > uwb_max_s, velocity damping activates and output
+  is suppressed by the node.
+
+Sensor weights (configurable via constructor)
+─────────────────────────────────────────────
+  sigma_uwb_pos_m    — UWB+IMU fused position σ (m)        default 0.10
+  sigma_uwb_head_rad — UWB+IMU fused heading σ (rad)       default 0.15
+  sigma_vo_vel_m_s   — VO linear velocity σ (m/s)          default 0.05
+  sigma_vo_omega_r_s — VO yaw-rate σ (rad/s)               default 0.03
+  sigma_imu_accel    — IMU accel noise σ (m/s²)            default 0.05
+  sigma_imu_gyro     — IMU gyro noise σ (rad/s)            default 0.003
+  sigma_vel_drift    — velocity process noise σ (m/s)       default 0.10
+  dropout_vel_damp   — velocity damping/s during UWB drop   default 0.95
+"""
+
+from __future__ import annotations
+
+import math
+from typing import Tuple
+
+import numpy as np
+
+
+# ── State indices ──────────────────────────────────────────────────────────────
+N = 6
+IX, IY, IT, IVX, IVY, IW = range(N)
+
+
+def _wrap(a: float) -> float:
+    """Wrap angle to (−π, π]."""
+    return float((a + math.pi) % (2.0 * math.pi) - math.pi)
+
+
+class PoseFusionEKF:
+    """
+    EKF fusing UWB+IMU absolute pose (10 Hz), visual-odometry velocity (30 Hz),
+    and raw IMU (200 Hz) into a single authoritative map-frame pose.
+
+    Thread model: single-threaded — caller must serialise access.
+    """
+
+    def __init__(
+        self,
+        sigma_uwb_pos_m:    float = 0.10,
+        sigma_uwb_head_rad: float = 0.15,
+        sigma_vo_vel_m_s:   float = 0.05,
+        sigma_vo_omega_r_s: float = 0.03,
+        sigma_imu_accel:    float = 0.05,
+        sigma_imu_gyro:     float = 0.003,
+        sigma_vel_drift:    float = 0.10,
+        dropout_vel_damp:   float = 0.95,
+    ) -> None:
+        # Measurement noise variances
+        self._R_uwb_pos  = sigma_uwb_pos_m    ** 2
+        self._R_uwb_head = sigma_uwb_head_rad ** 2
+        self._R_vo_vel   = sigma_vo_vel_m_s   ** 2
+        self._R_vo_omega = sigma_vo_omega_r_s ** 2
+
+        # Process noise parameters
+        self._q_a    = sigma_imu_accel  ** 2
+        self._q_g    = sigma_imu_gyro   ** 2
+        self._q_v    = sigma_vel_drift  ** 2
+        self._damp   = dropout_vel_damp
+
+        # State & covariance
+        self._x    = np.zeros(N, dtype=np.float64)
+        self._P    = np.eye(N, dtype=np.float64) * 9.0
+
+        # Simple accel bias estimator
+        self._accel_bias  = np.zeros(2, dtype=np.float64)
+        self._bias_alpha  = 0.005
+
+        self._init           = False
+        self._uwb_dropout_s  = 0.0
+        self._vo_dropout_s   = 0.0
+
+    # ── Initialisation ─────────────────────────────────────────────────────────
+
+    def initialise(self, x: float, y: float, heading: float = 0.0) -> None:
+        """Seed filter with known position (called on first UWB measurement)."""
+        self._x[:] = 0.0
+        self._x[IX] = x
+        self._x[IY] = y
+        self._x[IT] = heading
+        self._P     = np.diag([0.25, 0.25, 0.1, 1.0, 1.0, 0.1])
+        self._init  = True
+
+    # ── IMU predict step (200 Hz) ──────────────────────────────────────────────
+
+    def predict_imu(
+        self,
+        ax_body: float,
+        ay_body: float,
+        omega:   float,
+        dt:      float,
+    ) -> None:
+        """
+        EKF predict driven by body-frame IMU.
+
+        Parameters
+        ----------
+        ax_body : forward accel (m/s², body frame)
+        ay_body : lateral accel (m/s², body frame, left positive)
+        omega   : yaw rate (rad/s, CCW positive)
+        dt      : timestep (s)
+        """
+        if not self._init:
+            return
+
+        # Bias-compensated acceleration
+        ax_c = ax_body - self._accel_bias[0]
+        ay_c = ay_body - self._accel_bias[1]
+
+        x   = self._x
+        th  = x[IT]
+        ct  = math.cos(th)
+        st  = math.sin(th)
+
+        # World-frame acceleration
+        ax_w = ax_c * ct - ay_c * st
+        ay_w = ax_c * st + ay_c * ct
+
+        # Propagate state
+        xp       = x.copy()
+        xp[IX]   = x[IX]  + x[IVX] * dt
+        xp[IY]   = x[IY]  + x[IVY] * dt
+        xp[IT]   = _wrap(x[IT] + omega * dt)
+        xp[IVX]  = x[IVX] + ax_w * dt
+        xp[IVY]  = x[IVY] + ay_w * dt
+        xp[IW]   = omega
+
+        # Jacobian F = ∂f/∂x
+        F        = np.eye(N, dtype=np.float64)
+        F[IX, IVX] = dt
+        F[IY, IVY] = dt
+        F[IVX, IT] = (-ax_c * st - ay_c * ct) * dt
+        F[IVY, IT] = ( ax_c * ct - ay_c * st) * dt
+
+        # Process noise Q
+        dt2 = dt * dt
+        Q = np.diag([
+            0.5 * self._q_a * dt2 * dt2,
+            0.5 * self._q_a * dt2 * dt2,
+            self._q_g * dt2,
+            self._q_v * dt + self._q_a * dt2,
+            self._q_v * dt + self._q_a * dt2,
+            self._q_g,
+        ])
+
+        self._x = xp
+        self._P = F @ self._P @ F.T + Q
+
+        # Advance dropout clocks
+        self._uwb_dropout_s += dt
+        self._vo_dropout_s  += dt
+
+        # Velocity damping during extended UWB dropout
+        if self._uwb_dropout_s > 2.0:
+            d = self._damp ** dt
+            self._x[IVX] *= d
+            self._x[IVY] *= d
+
+        # Running accel bias estimate
+        self._accel_bias[0] += self._bias_alpha * (ax_body - self._accel_bias[0])
+        self._accel_bias[1] += self._bias_alpha * (ay_body - self._accel_bias[1])
+
+    # ── UWB+IMU position update (10 Hz) ───────────────────────────────────────
+
+    def update_uwb_position(
+        self,
+        x_m: float,
+        y_m: float,
+        sigma_m: float | None = None,
+    ) -> None:
+        """
+        Update from UWB+IMU fused position measurement [x, y].
+
+        Parameters
+        ----------
+        x_m, y_m : measured position (m, map frame)
+        sigma_m  : std-dev override; uses constructor default if None
+        """
+        if not self._init:
+            self.initialise(x_m, y_m)
+            return
+
+        R_val = (sigma_m ** 2) if sigma_m is not None else self._R_uwb_pos
+        R     = np.eye(2, dtype=np.float64) * R_val
+
+        H     = np.zeros((2, N), dtype=np.float64)
+        H[0, IX] = 1.0
+        H[1, IY] = 1.0
+
+        innov = np.array([x_m - self._x[IX], y_m - self._x[IY]])
+        _ekf_update(self._x, self._P, H, R, innov)
+        self._x[IT] = _wrap(self._x[IT])
+
+        self._uwb_dropout_s = 0.0
+
+    # ── UWB+IMU heading update (10 Hz) ────────────────────────────────────────
+
+    def update_uwb_heading(
+        self,
+        heading_rad: float,
+        sigma_rad:   float | None = None,
+    ) -> None:
+        """Update heading from UWB+IMU fused estimate."""
+        if not self._init:
+            return
+
+        R_val = (sigma_rad ** 2) if sigma_rad is not None else self._R_uwb_head
+        R     = np.array([[R_val]])
+
+        H     = np.zeros((1, N), dtype=np.float64)
+        H[0, IT] = 1.0
+
+        innov = np.array([_wrap(heading_rad - self._x[IT])])
+        _ekf_update(self._x, self._P, H, R, innov)
+        self._x[IT] = _wrap(self._x[IT])
+
+    # ── Visual-odometry velocity update (30 Hz) ───────────────────────────────
+
+    def update_vo_velocity(
+        self,
+        vx_body: float,
+        omega:   float,
+        sigma_vel:   float | None = None,
+        sigma_omega: float | None = None,
+    ) -> None:
+        """
+        Update from visual-odometry velocity measurement.
+
+        VO gives forward speed (vx_body, m/s) in the robot body frame, and
+        yaw rate (omega, rad/s).  We rotate vx_body to world frame using the
+        current heading estimate, then update [vx, vy, ω] state.
+
+        Parameters
+        ----------
+        vx_body   : VO forward linear speed (m/s)
+        omega     : VO yaw rate (rad/s)
+        sigma_vel : std-dev for linear velocity component (m/s)
+        sigma_omega: std-dev for yaw rate (rad/s)
+        """
+        if not self._init:
+            return
+
+        R_vel   = (sigma_vel   ** 2) if sigma_vel   is not None else self._R_vo_vel
+        R_omega = (sigma_omega ** 2) if sigma_omega is not None else self._R_vo_omega
+
+        th = self._x[IT]
+        ct = math.cos(th)
+        st = math.sin(th)
+
+        # Rotate body-frame vx to world-frame [vx_w, vy_w]
+        vx_w = vx_body * ct
+        vy_w = vx_body * st
+
+        # Measurement: [vx_w, vy_w, ω]
+        z = np.array([vx_w, vy_w, omega])
+
+        H = np.zeros((3, N), dtype=np.float64)
+        H[0, IVX] = 1.0
+        H[1, IVY] = 1.0
+        H[2, IW]  = 1.0
+
+        R = np.diag([R_vel, R_vel, R_omega])
+
+        innov = z - H @ self._x
+        _ekf_update(self._x, self._P, H, R, innov)
+        self._x[IT] = _wrap(self._x[IT])
+
+        self._vo_dropout_s = 0.0
+
+    # ── Accessors ─────────────────────────────────────────────────────────────
+
+    @property
+    def position(self) -> Tuple[float, float]:
+        return float(self._x[IX]), float(self._x[IY])
+
+    @property
+    def heading(self) -> float:
+        return float(self._x[IT])
+
+    @property
+    def velocity(self) -> Tuple[float, float]:
+        return float(self._x[IVX]), float(self._x[IVY])
+
+    @property
+    def yaw_rate(self) -> float:
+        return float(self._x[IW])
+
+    @property
+    def covariance_position(self) -> np.ndarray:
+        """2×2 position sub-covariance [x, y]."""
+        return self._P[:2, :2].copy()
+
+    @property
+    def covariance_full(self) -> np.ndarray:
+        """Full 6×6 state covariance."""
+        return self._P.copy()
+
+    @property
+    def is_initialised(self) -> bool:
+        return self._init
+
+    @property
+    def uwb_dropout_s(self) -> float:
+        return self._uwb_dropout_s
+
+    @property
+    def vo_dropout_s(self) -> float:
+        return self._vo_dropout_s
+
+    def position_uncertainty_m(self) -> float:
+        """Approx 1-σ position uncertainty (m)."""
+        return float(math.sqrt((self._P[IX, IX] + self._P[IY, IY]) / 2.0))
+
+
+# ── EKF update helper ──────────────────────────────────────────────────────────
+
+def _ekf_update(
+    x: np.ndarray,
+    P: np.ndarray,
+    H: np.ndarray,
+    R: np.ndarray,
+    innov: np.ndarray,
+) -> None:
+    """Standard linear EKF update step — modifies x and P in place."""
+    S = H @ P @ H.T + R
+    K = P @ H.T @ np.linalg.inv(S)
+    x += K @ innov
+    I_KH = np.eye(N, dtype=np.float64) - K @ H
+    # Joseph form for numerical stability
+    P[:] = I_KH @ P @ I_KH.T + K @ R @ K.T

jetson/ros2_ws/src/saltybot_pose_fusion/saltybot_pose_fusion/pose_fusion_node.py

@@ -0,0 +1,396 @@
+"""
+pose_fusion_node.py — Multi-sensor pose fusion node (Issue #595).
+
+Fuses three sensor streams into a single authoritative map-frame pose:
+
+  IMU (200 Hz)               /imu/data
+  UWB + IMU fused pose       /saltybot/pose/fused_cov   (10 Hz)
+  Visual odometry            /saltybot/visual_odom       (30 Hz)
+
+The UWB+IMU stream (from saltybot_uwb_imu_fusion) provides absolute position
+anchoring every ~100 ms.  Visual odometry fills in smooth relative motion
+between UWB updates at 30 Hz.  Raw IMU drives the predict step at 200 Hz.
+
+Publishes
+─────────
+  /saltybot/pose/authoritative  geometry_msgs/PoseWithCovarianceStamped
+  /saltybot/pose/status         std_msgs/String  (JSON, 10 Hz)
+
+TF2
+───
+  map → base_link  (broadcast at IMU rate when filter is healthy)
+
+Parameters (see config/pose_fusion_params.yaml)
+───────────────────────────────────────────────
+  Sensor weights:
+    sigma_uwb_pos_m      float  0.10   UWB position 1-σ (m)
+    use_uwb_covariance   bool   true   extract σ from PoseWithCovarianceStamped
+    sigma_uwb_head_rad   float  0.15   UWB heading 1-σ (rad)
+    sigma_vo_vel_m_s     float  0.05   VO linear-velocity 1-σ (m/s)
+    sigma_vo_omega_r_s   float  0.03   VO yaw-rate 1-σ (rad/s)
+    sigma_imu_accel      float  0.05   IMU accel noise (m/s²)
+    sigma_imu_gyro       float  0.003  IMU gyro noise (rad/s)
+    sigma_vel_drift      float  0.10   velocity process-noise (m/s)
+    dropout_vel_damp     float  0.95   velocity damping/s during UWB dropout
+
+  Dropout thresholds:
+    uwb_dropout_warn_s   float  2.0    log warning after this many s without UWB
+    uwb_dropout_max_s    float  10.0   suppress output after this many s
+    vo_dropout_warn_s    float  1.0    log warning after this many s without VO
+
+  Frame IDs:
+    map_frame            str    map
+    base_frame           str    base_link
+
+  Topics:
+    imu_topic            str    /imu/data
+    uwb_pose_topic       str    /saltybot/pose/fused_cov
+    vo_topic             str    /saltybot/visual_odom
+"""
+
+from __future__ import annotations
+
+import json
+import math
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import (
+    QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+)
+
+import tf2_ros
+from geometry_msgs.msg import (
+    PoseWithCovarianceStamped, TransformStamped
+)
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import Imu
+from std_msgs.msg import String
+
+from .pose_fusion_ekf import PoseFusionEKF
+
+
+# ── QoS ───────────────────────────────────────────────────────────────────────
+_SENSOR_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=5,
+)
+_RELIABLE_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.RELIABLE,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=5,
+)
+
+
+def _quat_to_yaw(qx: float, qy: float, qz: float, qw: float) -> float:
+    """Extract yaw (Z-axis rotation) from quaternion."""
+    return math.atan2(2.0 * (qw * qz + qx * qy),
+                      1.0 - 2.0 * (qy * qy + qz * qz))
+
+
+def _yaw_to_qz_qw(yaw: float):
+    half = yaw * 0.5
+    return math.sin(half), math.cos(half)
+
+
+def _cov36_pos_sigma(cov36: list) -> float | None:
+    """Extract average xy position sigma from 6×6 covariance row-major list."""
+    try:
+        p_xx = cov36[0]
+        p_yy = cov36[7]
+        if p_xx > 0.0 and p_yy > 0.0:
+            return float(math.sqrt((p_xx + p_yy) / 2.0))
+    except (IndexError, ValueError):
+        pass
+    return None
+
+
+def _cov36_heading_sigma(cov36: list) -> float | None:
+    """Extract heading sigma from 6×6 covariance row-major list (index 35 = yaw)."""
+    try:
+        p_yaw = cov36[35]
+        if p_yaw > 0.0:
+            return float(math.sqrt(p_yaw))
+    except (IndexError, ValueError):
+        pass
+    return None
+
+
+class PoseFusionNode(Node):
+    """Multi-sensor EKF pose fusion — see module docstring for details."""
+
+    def __init__(self) -> None:
+        super().__init__('pose_fusion')
+
+        # ── Parameters ────────────────────────────────────────────────────────
+        self.declare_parameter('sigma_uwb_pos_m',    0.10)
+        self.declare_parameter('use_uwb_covariance', True)
+        self.declare_parameter('sigma_uwb_head_rad', 0.15)
+        self.declare_parameter('sigma_vo_vel_m_s',   0.05)
+        self.declare_parameter('sigma_vo_omega_r_s', 0.03)
+        self.declare_parameter('sigma_imu_accel',    0.05)
+        self.declare_parameter('sigma_imu_gyro',     0.003)
+        self.declare_parameter('sigma_vel_drift',    0.10)
+        self.declare_parameter('dropout_vel_damp',   0.95)
+        self.declare_parameter('uwb_dropout_warn_s', 2.0)
+        self.declare_parameter('uwb_dropout_max_s',  10.0)
+        self.declare_parameter('vo_dropout_warn_s',  1.0)
+        self.declare_parameter('map_frame',          'map')
+        self.declare_parameter('base_frame',         'base_link')
+        self.declare_parameter('imu_topic',          '/imu/data')
+        self.declare_parameter('uwb_pose_topic',     '/saltybot/pose/fused_cov')
+        self.declare_parameter('vo_topic',           '/saltybot/visual_odom')
+
+        self._map_frame      = self.get_parameter('map_frame').value
+        self._base_frame     = self.get_parameter('base_frame').value
+        self._use_uwb_cov    = self.get_parameter('use_uwb_covariance').value
+        self._sigma_uwb_pos  = self.get_parameter('sigma_uwb_pos_m').value
+        self._sigma_uwb_head = self.get_parameter('sigma_uwb_head_rad').value
+        self._uwb_warn       = self.get_parameter('uwb_dropout_warn_s').value
+        self._uwb_max        = self.get_parameter('uwb_dropout_max_s').value
+        self._vo_warn        = self.get_parameter('vo_dropout_warn_s').value
+
+        # ── EKF ───────────────────────────────────────────────────────────────
+        self._ekf = PoseFusionEKF(
+            sigma_uwb_pos_m    = self._sigma_uwb_pos,
+            sigma_uwb_head_rad = self._sigma_uwb_head,
+            sigma_vo_vel_m_s   = self.get_parameter('sigma_vo_vel_m_s').value,
+            sigma_vo_omega_r_s = self.get_parameter('sigma_vo_omega_r_s').value,
+            sigma_imu_accel    = self.get_parameter('sigma_imu_accel').value,
+            sigma_imu_gyro     = self.get_parameter('sigma_imu_gyro').value,
+            sigma_vel_drift    = self.get_parameter('sigma_vel_drift').value,
+            dropout_vel_damp   = self.get_parameter('dropout_vel_damp').value,
+        )
+
+        self._last_imu_t: float | None = None
+
+        # ── Publishers ─────────────────────────────────────────────────────────
+        self._pose_pub   = self.create_publisher(
+            PoseWithCovarianceStamped, '/saltybot/pose/authoritative', 10
+        )
+        self._status_pub = self.create_publisher(
+            String, '/saltybot/pose/status', 10
+        )
+        self._tf_br = tf2_ros.TransformBroadcaster(self)
+
+        # ── Subscriptions ──────────────────────────────────────────────────────
+        self.create_subscription(
+            Imu,
+            self.get_parameter('imu_topic').value,
+            self._on_imu,
+            _SENSOR_QOS,
+        )
+        self.create_subscription(
+            PoseWithCovarianceStamped,
+            self.get_parameter('uwb_pose_topic').value,
+            self._on_uwb_pose,
+            _RELIABLE_QOS,
+        )
+        self.create_subscription(
+            Odometry,
+            self.get_parameter('vo_topic').value,
+            self._on_vo,
+            _SENSOR_QOS,
+        )
+
+        # ── Status timer (10 Hz) ────────────────────────────────────────────────
+        self.create_timer(0.1, self._on_status_timer)
+
+        self.get_logger().info(
+            f'pose_fusion ready — '
+            f'IMU:{self.get_parameter("imu_topic").value}  '
+            f'UWB:{self.get_parameter("uwb_pose_topic").value}  '
+            f'VO:{self.get_parameter("vo_topic").value}  '
+            f'map:{self._map_frame}→{self._base_frame}'
+        )
+
+    # ── IMU callback — predict step ────────────────────────────────────────────
+
+    def _on_imu(self, msg: Imu) -> None:
+        now = self.get_clock().now().nanoseconds * 1e-9
+
+        if self._last_imu_t is None:
+            self._last_imu_t = now
+            return
+
+        dt = now - self._last_imu_t
+        self._last_imu_t = now
+
+        if dt <= 0.0 or dt > 0.5:
+            return
+
+        self._ekf.predict_imu(
+            ax_body = msg.linear_acceleration.x,
+            ay_body = msg.linear_acceleration.y,
+            omega   = msg.angular_velocity.z,
+            dt      = dt,
+        )
+
+        if not self._ekf.is_initialised:
+            return
+
+        # Warn / suppress on UWB dropout
+        if self._ekf.uwb_dropout_s > self._uwb_max:
+            self.get_logger().warn(
+                f'UWB dropout {self._ekf.uwb_dropout_s:.1f}s '
+                '> max — pose output suppressed',
+                throttle_duration_sec=5.0,
+            )
+            return
+
+        if self._ekf.uwb_dropout_s > self._uwb_warn:
+            self.get_logger().warn(
+                f'UWB dropout {self._ekf.uwb_dropout_s:.1f}s — '
+                'dead-reckoning only',
+                throttle_duration_sec=2.0,
+            )
+
+        self._publish_pose(msg.header.stamp)
+
+    # ── UWB+IMU fused pose callback — position+heading update ─────────────────
+
+    def _on_uwb_pose(self, msg: PoseWithCovarianceStamped) -> None:
+        x = msg.pose.pose.position.x
+        y = msg.pose.pose.position.y
+
+        # Position sigma: from message covariance or parameter
+        sigma_pos: float | None = None
+        if self._use_uwb_cov:
+            sigma_pos = _cov36_pos_sigma(list(msg.pose.covariance))
+        if sigma_pos is None or sigma_pos <= 0.0:
+            sigma_pos = self._sigma_uwb_pos
+
+        self._ekf.update_uwb_position(x, y, sigma_m=sigma_pos)
+
+        # Heading update from quaternion
+        q  = msg.pose.pose.orientation
+        yaw = _quat_to_yaw(q.x, q.y, q.z, q.w)
+
+        sigma_head: float | None = None
+        if self._use_uwb_cov:
+            sigma_head = _cov36_heading_sigma(list(msg.pose.covariance))
+        if sigma_head is None or sigma_head <= 0.0:
+            sigma_head = self._sigma_uwb_head
+
+        # Only update heading if quaternion is non-trivial (some nodes publish q=(0,0,0,0))
+        if abs(q.w) > 0.01 or abs(q.z) > 0.01:
+            self._ekf.update_uwb_heading(yaw, sigma_rad=sigma_head)
+
+    # ── Visual-odometry callback — velocity update ─────────────────────────────
+
+    def _on_vo(self, msg: Odometry) -> None:
+        if not self._ekf.is_initialised:
+            return
+
+        vx_body = msg.twist.twist.linear.x
+        omega   = msg.twist.twist.angular.z
+
+        # Extract per-sensor sigma from twist covariance if present
+        # Row-major 6×6: [0]=vx, [7]=vy, [35]=ωz
+        sigma_vel:   float | None = None
+        sigma_omega: float | None = None
+        cov = list(msg.twist.covariance)
+        if len(cov) == 36:
+            p_vx = cov[0]
+            p_wz = cov[35]
+            if p_vx > 0.0:
+                sigma_vel   = float(math.sqrt(p_vx))
+            if p_wz > 0.0:
+                sigma_omega = float(math.sqrt(p_wz))
+
+        # Warn on VO dropout (but never suppress — VO is secondary)
+        if self._ekf.vo_dropout_s > self._vo_warn:
+            self.get_logger().warn(
+                f'VO dropout {self._ekf.vo_dropout_s:.1f}s',
+                throttle_duration_sec=2.0,
+            )
+
+        self._ekf.update_vo_velocity(
+            vx_body     = vx_body,
+            omega       = omega,
+            sigma_vel   = sigma_vel,
+            sigma_omega = sigma_omega,
+        )
+
+    # ── Status timer (10 Hz) ──────────────────────────────────────────────────
+
+    def _on_status_timer(self) -> None:
+        if not self._ekf.is_initialised:
+            return
+        x, y = self._ekf.position
+        status = {
+            'x':               round(x, 4),
+            'y':               round(y, 4),
+            'heading_deg':     round(math.degrees(self._ekf.heading), 2),
+            'pos_uncertainty_m': round(self._ekf.position_uncertainty_m(), 4),
+            'uwb_dropout_s':   round(self._ekf.uwb_dropout_s, 2),
+            'vo_dropout_s':    round(self._ekf.vo_dropout_s,  2),
+            'healthy':         self._ekf.uwb_dropout_s < self._uwb_max,
+        }
+        msg      = String()
+        msg.data = json.dumps(status)
+        self._status_pub.publish(msg)
+
+    # ── Publish authoritative pose ────────────────────────────────────────────
+
+    def _publish_pose(self, stamp) -> None:
+        x, y    = self._ekf.position
+        heading = self._ekf.heading
+        cov_pos = self._ekf.covariance_position
+        cov6    = self._ekf.covariance_full
+        vx, vy  = self._ekf.velocity
+        omega   = self._ekf.yaw_rate
+
+        qz, qw = _yaw_to_qz_qw(heading)
+
+        msg                          = PoseWithCovarianceStamped()
+        msg.header.stamp             = stamp
+        msg.header.frame_id          = self._map_frame
+        msg.pose.pose.position.x     = x
+        msg.pose.pose.position.y     = y
+        msg.pose.pose.position.z     = 0.0
+        msg.pose.pose.orientation.z  = qz
+        msg.pose.pose.orientation.w  = qw
+
+        # Build 6×6 row-major covariance
+        cov36          = [0.0] * 36
+        cov36[0]       = cov_pos[0, 0]    # x,x
+        cov36[1]       = cov_pos[0, 1]    # x,y
+        cov36[6]       = cov_pos[1, 0]    # y,x
+        cov36[7]       = cov_pos[1, 1]    # y,y
+        cov36[14]      = 1e-4             # z (not estimated)
+        cov36[21]      = 1e-4             # roll
+        cov36[28]      = 1e-4             # pitch
+        cov36[35]      = float(cov6[2, 2])  # yaw
+        msg.pose.covariance = cov36
+
+        self._pose_pub.publish(msg)
+
+        # TF2: map → base_link
+        tf                          = TransformStamped()
+        tf.header.stamp             = stamp
+        tf.header.frame_id          = self._map_frame
+        tf.child_frame_id           = self._base_frame
+        tf.transform.translation.x  = x
+        tf.transform.translation.y  = y
+        tf.transform.translation.z  = 0.0
+        tf.transform.rotation.z     = qz
+        tf.transform.rotation.w     = qw
+        self._tf_br.sendTransform(tf)
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = PoseFusionNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_pose_fusion/setup.cfg

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

jetson/ros2_ws/src/saltybot_pose_fusion/setup.py

@@ -0,0 +1,30 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = 'saltybot_pose_fusion'
+
+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='Multi-sensor EKF pose fusion — UWB + visual odom + IMU (Issue #595)',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'pose_fusion = saltybot_pose_fusion.pose_fusion_node:main',
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_pose_fusion/test/test_pose_fusion_ekf.py

@@ -0,0 +1,134 @@
+"""Unit tests for PoseFusionEKF (no ROS2 required)."""
+
+import math
+import pytest
+from saltybot_pose_fusion.pose_fusion_ekf import PoseFusionEKF
+
+
+def make_ekf(**kwargs):
+    return PoseFusionEKF(**kwargs)
+
+
+# ── Initialisation ─────────────────────────────────────────────────────────────
+
+def test_not_initialised_before_uwb():
+    ekf = make_ekf()
+    assert not ekf.is_initialised
+
+
+def test_initialised_after_uwb_update():
+    ekf = make_ekf()
+    ekf.update_uwb_position(1.0, 2.0)
+    assert ekf.is_initialised
+    x, y = ekf.position
+    assert abs(x - 1.0) < 1e-9
+    assert abs(y - 2.0) < 1e-9
+
+
+# ── IMU predict ───────────────────────────────────────────────────────────────
+
+def test_predict_advances_position():
+    ekf = make_ekf()
+    ekf.update_uwb_position(0.0, 0.0)
+    # Give it a forward velocity by injecting a forward acceleration
+    ekf.predict_imu(ax_body=1.0, ay_body=0.0, omega=0.0, dt=1.0)
+    ekf.predict_imu(ax_body=1.0, ay_body=0.0, omega=0.0, dt=1.0)
+    x, y = ekf.position
+    assert x > 0.0, f"Expected forward motion, got x={x}"
+
+
+def test_predict_no_crash_before_init():
+    ekf = make_ekf()
+    ekf.predict_imu(ax_body=1.0, ay_body=0.0, omega=0.0, dt=0.01)
+    assert not ekf.is_initialised
+
+
+# ── UWB position update ───────────────────────────────────────────────────────
+
+def test_uwb_update_converges():
+    ekf = make_ekf(sigma_uwb_pos_m=0.10)
+    ekf.update_uwb_position(5.0, 3.0)
+    for _ in range(20):
+        ekf.predict_imu(ax_body=0.0, ay_body=0.0, omega=0.0, dt=0.05)
+        ekf.update_uwb_position(5.0, 3.0)
+    x, y = ekf.position
+    assert abs(x - 5.0) < 0.5
+    assert abs(y - 3.0) < 0.5
+
+
+def test_uwb_resets_dropout():
+    ekf = make_ekf()
+    ekf.update_uwb_position(0.0, 0.0)
+    ekf.predict_imu(0.0, 0.0, 0.0, dt=5.0)
+    assert ekf.uwb_dropout_s > 4.0
+    ekf.update_uwb_position(0.0, 0.0)
+    assert ekf.uwb_dropout_s == 0.0
+
+
+# ── UWB heading update ────────────────────────────────────────────────────────
+
+def test_uwb_heading_update():
+    ekf = make_ekf(sigma_uwb_head_rad=0.1)
+    ekf.update_uwb_position(0.0, 0.0)
+    target_yaw = math.radians(45.0)
+    for _ in range(10):
+        ekf.update_uwb_heading(target_yaw)
+    assert abs(ekf.heading - target_yaw) < 0.2
+
+
+# ── VO velocity update ────────────────────────────────────────────────────────
+
+def test_vo_velocity_update():
+    ekf = make_ekf(sigma_vo_vel_m_s=0.05, sigma_vo_omega_r_s=0.03)
+    ekf.update_uwb_position(0.0, 0.0)
+    ekf.update_vo_velocity(vx_body=1.0, omega=0.0)
+    vx, vy = ekf.velocity
+    assert vx > 0.5, f"Expected vx>0.5 after VO update, got {vx}"
+
+
+def test_vo_resets_dropout():
+    ekf = make_ekf()
+    ekf.update_uwb_position(0.0, 0.0)
+    ekf.predict_imu(0.0, 0.0, 0.0, dt=3.0)
+    assert ekf.vo_dropout_s > 2.0
+    ekf.update_vo_velocity(vx_body=0.0, omega=0.0)
+    assert ekf.vo_dropout_s == 0.0
+
+
+# ── Covariance ────────────────────────────────────────────────────────────────
+
+def test_covariance_decreases_with_updates():
+    ekf = make_ekf()
+    ekf.update_uwb_position(0.0, 0.0)
+    p_init = ekf.position_uncertainty_m()
+    for _ in range(30):
+        ekf.predict_imu(0.0, 0.0, 0.0, dt=0.05)
+        ekf.update_uwb_position(0.0, 0.0)
+    assert ekf.position_uncertainty_m() < p_init
+
+
+def test_covariance_matrix_shape():
+    ekf = make_ekf()
+    ekf.update_uwb_position(1.0, 1.0)
+    assert ekf.covariance_position.shape == (2, 2)
+    assert ekf.covariance_full.shape == (6, 6)
+
+
+# ── Sigma override ────────────────────────────────────────────────────────────
+
+def test_custom_sigma_override():
+    """High sigma should produce weaker updates than low sigma."""
+    ekf_weak   = make_ekf(sigma_uwb_pos_m=2.0)
+    ekf_strong = make_ekf(sigma_uwb_pos_m=0.01)
+
+    for ekf in (ekf_weak, ekf_strong):
+        ekf.update_uwb_position(10.0, 0.0)
+        ekf.predict_imu(0.0, 0.0, 0.0, dt=0.1)
+        ekf.update_uwb_position(0.0, 0.0)
+
+    x_weak,   _ = ekf_weak.position
+    x_strong, _ = ekf_strong.position
+
+    # Strong measurement should pull closer to 0
+    assert x_strong < x_weak, \
+        f"Strong sigma should pull x closer to 0: strong={x_strong:.3f} weak={x_weak:.3f}"

jetson/ros2_ws/src/saltybot_uwb_calibration/config/calibration_params.yaml

@@ -0,0 +1,17 @@
+uwb_anchor_calibration:
+  ros__parameters:
+    # Serial ports for each anchor, indexed by anchor ID
+    # udev rules create stable symlinks in /dev/uwb-anchor<N>
+    anchor_serials:
+      - /dev/uwb-anchor0
+      - /dev/uwb-anchor1
+      - /dev/uwb-anchor2
+      - /dev/uwb-anchor3
+
+    baud_rate: 115200
+
+    # Seconds to wait for a +PEER_RANGE response per attempt
+    peer_range_timeout_s: 2.0
+
+    # Range measurements to average per anchor pair (higher = more accurate)
+    default_n_samples: 5

jetson/ros2_ws/src/saltybot_uwb_calibration/launch/uwb_calibration.launch.py

@@ -0,0 +1,32 @@
+"""
+uwb_calibration.launch.py — Launch the UWB anchor auto-calibration service.
+
+Usage:
+    ros2 launch saltybot_uwb_calibration uwb_calibration.launch.py
+
+Then trigger from command line:
+    ros2 service call /saltybot/uwb/calibrate_anchors \\
+        saltybot_uwb_calibration_msgs/srv/CalibrateAnchors \\
+        "{anchor_ids: [], n_samples: 5}"
+"""
+
+from launch import LaunchDescription
+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_calibration")
+    params_file = os.path.join(pkg_dir, "config", "calibration_params.yaml")
+
+    calibration_node = Node(
+        package="saltybot_uwb_calibration",
+        executable="uwb_calibration",
+        name="uwb_anchor_calibration",
+        parameters=[params_file],
+        output="screen",
+        emulate_tty=True,
+    )
+
+    return LaunchDescription([calibration_node])

jetson/ros2_ws/src/saltybot_uwb_calibration/package.xml

@@ -0,0 +1,29 @@
+<?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_calibration</name>
+  <version>0.1.0</version>
+  <description>
+    UWB anchor auto-calibration for SaltyBot (Issue #602).
+    Commands anchors to range against each other via AT+PEER_RANGE=,
+    builds a pairwise distance matrix, and uses classical MDS to recover
+    2-D anchor positions.  Exposes /saltybot/uwb/calibrate_anchors service.
+  </description>
+  <maintainer email="sl-uwb@saltylab.local">sl-uwb</maintainer>
+  <license>Apache-2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>saltybot_uwb_calibration_msgs</depend>
+
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-serial</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_calibration/saltybot_uwb_calibration/calibration_node.py

@@ -0,0 +1,272 @@
+"""
+calibration_node.py — UWB anchor auto-calibration ROS2 service (Issue #602)
+
+Service:  /saltybot/uwb/calibrate_anchors  (CalibrateAnchors)
+
+Workflow
+────────
+  1. For every ordered pair (i, j) with i < j, send AT+PEER_RANGE=<j> over
+     anchor i's serial port, collect `n_samples` measurements, average them.
+  2. Build the symmetric N×N distance matrix D.
+  3. Run classical MDS (mds_math.mds_2d) to recover (x, y) positions.
+  4. Align the frame: anchor-0 at origin, anchor-1 on +X axis.
+  5. Return positions + residual RMS + JSON config string.
+
+Parameters (ROS2, see config/calibration_params.yaml)
+──────────────────────────────────────────────────────
+  anchor_serials   list of serial port paths, index = anchor ID
+                   e.g. ["/dev/uwb-anchor0", "/dev/uwb-anchor1", ...]
+  baud_rate        115200
+  peer_range_timeout_s  2.0   (per AT+PEER_RANGE= attempt)
+  default_n_samples     5
+"""
+
+from __future__ import annotations
+
+import json
+import re
+import time
+from typing import Optional
+
+import numpy as np
+import serial
+
+import rclpy
+from rclpy.node import Node
+
+from saltybot_uwb_calibration_msgs.srv import CalibrateAnchors
+from saltybot_uwb_calibration.mds_math import mds_2d, anchor_frame_align
+
+
+_PEER_RANGE_RE = re.compile(
+    r"\+PEER_RANGE:(\d+),(\d+),(\d+),([-\d.]+)"
+)
+_PEER_ERR_RE = re.compile(
+    r"\+PEER_RANGE:ERR,(\d+),(\w+)"
+)
+
+
+class CalibrationNode(Node):
+
+    def __init__(self) -> None:
+        super().__init__("uwb_anchor_calibration")
+
+        self.declare_parameter("anchor_serials", [
+            "/dev/uwb-anchor0",
+            "/dev/uwb-anchor1",
+            "/dev/uwb-anchor2",
+            "/dev/uwb-anchor3",
+        ])
+        self.declare_parameter("baud_rate", 115200)
+        self.declare_parameter("peer_range_timeout_s", 2.0)
+        self.declare_parameter("default_n_samples", 5)
+
+        self._serials: list[str] = (
+            self.get_parameter("anchor_serials").value
+        )
+        self._baud: int = self.get_parameter("baud_rate").value
+        self._timeout: float = self.get_parameter("peer_range_timeout_s").value
+        self._default_n: int = self.get_parameter("default_n_samples").value
+
+        self._srv = self.create_service(
+            CalibrateAnchors,
+            "/saltybot/uwb/calibrate_anchors",
+            self._handle_calibrate,
+        )
+
+        self.get_logger().info(
+            f"UWB calibration service ready — "
+            f"{len(self._serials)} configured anchors"
+        )
+
+    # ── Service handler ────────────────────────────────────────────────────
+
+    def _handle_calibrate(
+        self,
+        request: CalibrateAnchors.Request,
+        response: CalibrateAnchors.Response,
+    ) -> CalibrateAnchors.Response:
+
+        # Determine anchor IDs to calibrate
+        if request.anchor_ids:
+            ids = list(request.anchor_ids)
+        else:
+            ids = list(range(len(self._serials)))
+
+        n_anchors = len(ids)
+        if n_anchors < 4:
+            response.success = False
+            response.message = (
+                f"Need at least 4 anchors, got {n_anchors}"
+            )
+            return response
+
+        n_samples = int(request.n_samples) if request.n_samples > 0 \
+            else self._default_n
+
+        self.get_logger().info(
+            f"Starting calibration: anchors={ids}, n_samples={n_samples}"
+        )
+
+        # Open serial ports
+        ports: dict[int, serial.Serial] = {}
+        try:
+            for anchor_id in ids:
+                if anchor_id >= len(self._serials):
+                    raise RuntimeError(
+                        f"No serial port configured for anchor {anchor_id}"
+                    )
+                port_path = self._serials[anchor_id]
+                ports[anchor_id] = serial.Serial(
+                    port_path,
+                    baudrate=self._baud,
+                    timeout=self._timeout,
+                )
+                # Flush any stale data
+                ports[anchor_id].reset_input_buffer()
+                time.sleep(0.05)
+                self.get_logger().info(
+                    f"  Opened anchor {anchor_id} → {port_path}"
+                )
+        except Exception as exc:
+            _close_all(ports)
+            response.success = False
+            response.message = f"Serial open failed: {exc}"
+            return response
+
+        # Measure all unique pairs
+        D = np.zeros((n_anchors, n_anchors), dtype=float)
+        id_to_idx = {aid: idx for idx, aid in enumerate(ids)}
+
+        try:
+            for i_idx, i_id in enumerate(ids):
+                for j_idx, j_id in enumerate(ids):
+                    if j_idx <= i_idx:
+                        continue
+
+                    dist_m = self._measure_pair(
+                        ports[i_id], i_id, j_id, n_samples
+                    )
+                    if dist_m is None:
+                        raise RuntimeError(
+                            f"No valid range between anchor {i_id} "
+                            f"and anchor {j_id}"
+                        )
+                    D[i_idx, j_idx] = dist_m
+                    D[j_idx, i_idx] = dist_m
+                    self.get_logger().info(
+                        f"  [{i_id}↔{j_id}] = {dist_m:.3f} m"
+                    )
+
+        except Exception as exc:
+            _close_all(ports)
+            response.success = False
+            response.message = str(exc)
+            return response
+
+        _close_all(ports)
+
+        # MDS
+        try:
+            positions, residual = mds_2d(D)
+            positions = anchor_frame_align(positions)
+        except Exception as exc:
+            response.success = False
+            response.message = f"MDS failed: {exc}"
+            return response
+
+        # Build JSON config
+        pos_dict = {}
+        for idx, aid in enumerate(ids):
+            pos_dict[str(aid)] = [
+                round(float(positions[idx, 0]), 4),
+                round(float(positions[idx, 1]), 4),
+                0.0,
+            ]
+        json_str = json.dumps(pos_dict, indent=2)
+
+        self.get_logger().info(
+            f"Calibration complete — residual RMS {residual:.4f} m\n{json_str}"
+        )
+
+        # Fill response
+        response.success = True
+        response.message = (
+            f"OK — residual RMS {residual*1000:.1f} mm"
+        )
+        response.anchor_ids = [int(a) for a in ids]
+        response.positions_x = [float(positions[i, 0]) for i in range(n_anchors)]
+        response.positions_y = [float(positions[i, 1]) for i in range(n_anchors)]
+        response.positions_z = [0.0] * n_anchors
+        response.residual_rms_m = float(residual)
+        response.anchor_positions_json = json_str
+        return response
+
+    # ── Per-pair measurement ───────────────────────────────────────────────
+
+    def _measure_pair(
+        self,
+        port: serial.Serial,
+        my_id: int,
+        peer_id: int,
+        n_samples: int,
+    ) -> Optional[float]:
+        """
+        Send AT+PEER_RANGE=<peer_id> `n_samples` times over `port`.
+        Returns the mean range in metres, or None if all attempts fail.
+        """
+        measurements: list[float] = []
+
+        for _ in range(n_samples):
+            cmd = f"AT+PEER_RANGE={peer_id}\r\n"
+            port.write(cmd.encode())
+            port.flush()
+
+            deadline = time.monotonic() + self._timeout
+            while time.monotonic() < deadline:
+                line = port.readline().decode(errors="replace").strip()
+                if not line:
+                    continue
+
+                m = _PEER_RANGE_RE.match(line)
+                if m:
+                    # +PEER_RANGE:<my_id>,<peer_id>,<mm>,<rssi>
+                    if int(m.group(1)) == my_id and int(m.group(2)) == peer_id:
+                        mm = float(m.group(3))
+                        measurements.append(mm / 1000.0)  # mm → m
+                        break
+
+                if _PEER_ERR_RE.match(line):
+                    break  # timeout from firmware — try next sample
+
+            time.sleep(0.05)  # inter-command gap
+
+        if not measurements:
+            return None
+        return float(np.mean(measurements))
+
+
+# ── Helpers ────────────────────────────────────────────────────────────────
+
+def _close_all(ports: dict) -> None:
+    for p in ports.values():
+        try:
+            p.close()
+        except Exception:
+            pass
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = CalibrationNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_uwb_calibration/saltybot_uwb_calibration/mds_math.py

@@ -0,0 +1,140 @@
+"""
+mds_math.py — Classical Multi-Dimensional Scaling for UWB anchor calibration
+             (Issue #602)
+
+Given an N×N symmetric distance matrix D (in metres), recover the 2-D
+Cartesian positions of N anchors using classical (metric) MDS.
+
+Algorithm
+─────────
+  1. Compute the double-centred squared-distance matrix B = -½ H D² H
+     where H = I - (1/N) 11ᵀ  (centering matrix).
+  2. Eigen-decompose B = V Λ Vᵀ (symmetric).
+  3. Keep the 2 largest positive eigenvalues (λ₁, λ₂).
+  4. Positions X = V[:, :2] · diag(√λ₁, √λ₂).
+
+The recovered configuration is up to a global rigid transformation
+(rotation, reflection, translation).  The caller is responsible for
+anchoring the coordinate frame (e.g. fix anchor-0 at origin, anchor-1
+on the positive-X axis).
+
+Usage
+─────
+  from saltybot_uwb_calibration.mds_math import mds_2d, anchor_frame_align
+
+  D = np.array([[0, d01, d02, d03],
+                [d01, 0, d12, d13],
+                ...])
+  positions, residual = mds_2d(D)          # (N,2) array, float RMS (m)
+  positions = anchor_frame_align(positions) # align frame: anchor-0 at origin
+"""
+
+from __future__ import annotations
+
+import numpy as np
+
+
+def mds_2d(D: np.ndarray) -> tuple[np.ndarray, float]:
+    """
+    Recover 2-D anchor positions from pairwise distance matrix D.
+
+    Parameters
+    ----------
+    D : (N, N) symmetric ndarray of float
+        Pairwise distances in metres.  Diagonal must be 0.
+        N must be >= 3 (4+ recommended for overdetermined system).
+
+    Returns
+    -------
+    positions : (N, 2) ndarray
+        Recovered x, y coordinates in metres.
+    residual_rms : float
+        RMS of (D_measured - D_recovered) in metres across all unique pairs.
+
+    Raises
+    ------
+    ValueError
+        If D is not square, not symmetric, or N < 3.
+    """
+    D = np.asarray(D, dtype=float)
+    n = D.shape[0]
+    if D.ndim != 2 or D.shape[1] != n:
+        raise ValueError(f"D must be square, got shape {D.shape}")
+    if n < 3:
+        raise ValueError(f"Need at least 3 anchors, got {n}")
+    if not np.allclose(D, D.T, atol=1e-3):
+        raise ValueError("Distance matrix D is not symmetric")
+
+    # ── Step 1: double-centre D² ──────────────────────────────────────────
+    D2 = D ** 2
+    H = np.eye(n) - np.ones((n, n)) / n
+    B = -0.5 * H @ D2 @ H
+
+    # ── Step 2: eigendecomposition ────────────────────────────────────────
+    eigvals, eigvecs = np.linalg.eigh(B)      # eigvals in ascending order
+
+    # ── Step 3: keep top-2 positive eigenvalues ───────────────────────────
+    idx = np.argsort(eigvals)[::-1]           # descending
+    top2_idx = idx[:2]
+    lam = eigvals[top2_idx]
+    lam = np.maximum(lam, 0.0)               # clamp numerical negatives
+
+    # ── Step 4: recover positions ─────────────────────────────────────────
+    V = eigvecs[:, top2_idx]
+    positions = V * np.sqrt(lam)             # (N, 2)
+
+    # ── Residual RMS ──────────────────────────────────────────────────────
+    residual_rms = _residual(D, positions)
+
+    return positions, residual_rms
+
+
+def anchor_frame_align(positions: np.ndarray) -> np.ndarray:
+    """
+    Align recovered positions so that:
+      - anchor-0 is at the origin (0, 0)
+      - anchor-1 lies on the positive-X axis (y = 0)
+
+    This removes the MDS ambiguity (translation + rotation + reflection).
+
+    Parameters
+    ----------
+    positions : (N, 2) ndarray
+
+    Returns
+    -------
+    (N, 2) ndarray  — aligned positions
+    """
+    positions = np.array(positions, dtype=float)
+    if positions.shape[0] < 2:
+        return positions
+
+    # Translate so anchor-0 is at origin
+    positions -= positions[0]
+
+    # Rotate so anchor-1 is on positive-X axis
+    dx, dy = positions[1, 0], positions[1, 1]
+    angle = np.arctan2(dy, dx)
+    cos_a, sin_a = np.cos(-angle), np.sin(-angle)
+    R = np.array([[cos_a, -sin_a],
+                  [sin_a,  cos_a]])
+    positions = (R @ positions.T).T
+
+    # Ensure anchor-1 has positive X (handle reflection)
+    if positions[1, 0] < 0:
+        positions[:, 0] *= -1.0
+
+    return positions
+
+
+def _residual(D_meas: np.ndarray, positions: np.ndarray) -> float:
+    """Compute RMS of (measured - recovered) distances over all unique pairs."""
+    n = D_meas.shape[0]
+    errors = []
+    for i in range(n):
+        for j in range(i + 1, n):
+            d_rec = float(np.linalg.norm(positions[i] - positions[j]))
+            errors.append(D_meas[i, j] - d_rec)
+    if not errors:
+        return 0.0
+    return float(np.sqrt(np.mean(np.array(errors) ** 2)))

jetson/ros2_ws/src/saltybot_uwb_calibration/setup.cfg

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

jetson/ros2_ws/src/saltybot_uwb_calibration/setup.py

@@ -0,0 +1,29 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = "saltybot_uwb_calibration"
+
+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 anchor auto-calibration via MDS (Issue #602)",
+    license="Apache-2.0",
+    entry_points={
+        "console_scripts": [
+            "uwb_calibration = saltybot_uwb_calibration.calibration_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_uwb_calibration/test/test_mds_math.py

@@ -0,0 +1,142 @@
+"""
+Unit tests for saltybot_uwb_calibration.mds_math (Issue #602).
+No ROS2 or hardware required — pure numpy.
+"""
+
+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_calibration.mds_math import mds_2d, anchor_frame_align, _residual
+
+
+def _dist_matrix(positions: np.ndarray) -> np.ndarray:
+    """Build exact pairwise distance matrix from ground-truth positions."""
+    n = len(positions)
+    D = np.zeros((n, n))
+    for i in range(n):
+        for j in range(n):
+            D[i, j] = np.linalg.norm(positions[i] - positions[j])
+    return D
+
+
+# ── Square configuration (4 anchors) ──────────────────────────────────────
+
+GT_SQUARE = np.array([
+    [0.0, 0.0],
+    [4.0, 0.0],
+    [4.0, 3.0],
+    [0.0, 3.0],
+])
+
+
+class TestMdsSquare:
+    def test_residual_near_zero(self):
+        D = _dist_matrix(GT_SQUARE)
+        positions, rms = mds_2d(D)
+        assert rms < 1e-6, f"Residual too large: {rms}"
+
+    def test_correct_distances_preserved(self):
+        D = _dist_matrix(GT_SQUARE)
+        positions, _ = mds_2d(D)
+        positions = anchor_frame_align(positions)
+        D_rec = _dist_matrix(positions)
+        # All pairwise distances should match to < 1 mm
+        assert np.max(np.abs(D - D_rec)) < 1e-3
+
+    def test_anchor_0_at_origin(self):
+        D = _dist_matrix(GT_SQUARE)
+        positions, _ = mds_2d(D)
+        positions = anchor_frame_align(positions)
+        assert abs(positions[0, 0]) < 1e-9
+        assert abs(positions[0, 1]) < 1e-9
+
+    def test_anchor_1_on_positive_x(self):
+        D = _dist_matrix(GT_SQUARE)
+        positions, _ = mds_2d(D)
+        positions = anchor_frame_align(positions)
+        assert positions[1, 0] > 0
+        assert abs(positions[1, 1]) < 1e-9
+
+
+# ── Non-uniform configuration (5 anchors) ─────────────────────────────────
+
+GT_5 = np.array([
+    [0.0, 0.0],
+    [5.0, 0.0],
+    [5.0, 4.0],
+    [2.5, 6.0],
+    [0.0, 4.0],
+])
+
+
+class TestMds5Anchors:
+    def test_residual_near_zero(self):
+        D = _dist_matrix(GT_5)
+        positions, rms = mds_2d(D)
+        assert rms < 1e-6
+
+    def test_shape(self):
+        D = _dist_matrix(GT_5)
+        positions, _ = mds_2d(D)
+        assert positions.shape == (5, 2)
+
+
+# ── Noisy distances (simulate UWB measurement noise) ──────────────────────
+
+class TestMdsNoisyInput:
+    def test_residual_acceptable_with_5cm_noise(self):
+        rng = np.random.default_rng(42)
+        D = _dist_matrix(GT_SQUARE)
+        noise = rng.normal(0, 0.05, D.shape)
+        noise = (noise + noise.T) / 2   # keep symmetric
+        np.fill_diagonal(noise, 0)
+        D_noisy = np.maximum(D + noise, 0)
+        positions, rms = mds_2d(D_noisy)
+        # With 5 cm noise the residual should be sub-10 cm
+        assert rms < 0.10, f"Residual {rms:.4f} m too large"
+
+
+# ── Error handling ─────────────────────────────────────────────────────────
+
+class TestMdsErrors:
+    def test_non_square_raises(self):
+        with pytest.raises(ValueError):
+            mds_2d(np.ones((3, 4)))
+
+    def test_too_few_anchors_raises(self):
+        with pytest.raises(ValueError):
+            mds_2d(np.array([[0.0, 1.0], [1.0, 0.0]]))
+
+    def test_asymmetric_raises(self):
+        D = _dist_matrix(GT_SQUARE)
+        D[0, 1] += 0.5  # break symmetry
+        with pytest.raises(ValueError):
+            mds_2d(D)
+
+
+# ── anchor_frame_align ─────────────────────────────────────────────────────
+
+class TestFrameAlign:
+    def test_translated_positions_aligned(self):
+        pts = GT_SQUARE.copy() + np.array([10.0, -5.0])
+        aligned = anchor_frame_align(pts)
+        assert abs(aligned[0, 0]) < 1e-9
+        assert abs(aligned[0, 1]) < 1e-9
+
+    def test_rotated_positions_aligned(self):
+        angle = math.pi / 3
+        R = np.array([[math.cos(angle), -math.sin(angle)],
+                      [math.sin(angle),  math.cos(angle)]])
+        pts = (R @ GT_SQUARE.T).T
+        aligned = anchor_frame_align(pts)
+        assert aligned[1, 0] > 0
+        assert abs(aligned[1, 1]) < 1e-9
+
+
+if __name__ == "__main__":
+    pytest.main([__file__, "-v"])

jetson/ros2_ws/src/saltybot_uwb_calibration_msgs/CMakeLists.txt

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.8)
+project(saltybot_uwb_calibration_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/CalibrateAnchors.srv"
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

jetson/ros2_ws/src/saltybot_uwb_calibration_msgs/package.xml

@@ -0,0 +1,23 @@
+<?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_calibration_msgs</name>
+  <version>0.1.0</version>
+  <description>
+    Service definitions for UWB anchor auto-calibration (Issue #602).
+    Provides CalibrateAnchors.srv used by saltybot_uwb_calibration node.
+  </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>
+
+  <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_calibration_msgs/srv/CalibrateAnchors.srv

@@ -0,0 +1,29 @@
+# CalibrateAnchors.srv — UWB anchor auto-calibration service (Issue #602)
+#
+# Request: list of anchor IDs to calibrate (minimum 4).
+#          If anchor_ids is empty, the node uses all discovered anchors.
+#
+# Each anchor is commanded via AT+PEER_RANGE=<peer_id> to range against every
+# other anchor.  The resulting N×N distance matrix is passed through classical
+# MDS to recover 2-D positions.  Results are written to anchor_positions_json.
+#
+# Response: success flag, per-anchor [x, y, z] positions, residual RMS error,
+#           and JSON string suitable for dropping into fusion_params.yaml.
+
+# ── Request ────────────────────────────────────────────────────────────────
+uint8[] anchor_ids          # IDs of anchors to calibrate (empty = auto-discover)
+uint8   n_samples           # range measurements to average per pair (default 5)
+
+---
+
+# ── Response ───────────────────────────────────────────────────────────────
+bool    success
+string  message             # human-readable status / error description
+
+uint8[]  anchor_ids         # same order as positions below
+float64[] positions_x       # metres, MDS-recovered X (2-D plane)
+float64[] positions_y       # metres, MDS-recovered Y
+float64[] positions_z       # metres, always 0.0 (flat-floor assumption)
+
+float64 residual_rms_m      # RMS of |D_measured - D_recovered| across all pairs
+string  anchor_positions_json  # JSON: {"0": [x,y,z], "1": [x,y,z], ...}

src/balance.c

@@ -1,4 +1,5 @@
 #include "balance.h"
+#include "slope_estimator.h"
 #include "config.h"
 #include <math.h>
 
@@ -19,6 +20,9 @@ void balance_init(balance_t *b) {
     /* Safety defaults from config */
     b->max_tilt = MAX_TILT_DEG;
     b->max_speed = MAX_SPEED_LIMIT;
+
+    /* Slope estimator */
+    slope_estimator_init(&b->slope);
 }
 
 void balance_update(balance_t *b, const IMUData *imu, float dt) {
@@ -28,12 +32,16 @@ void balance_update(balance_t *b, const IMUData *imu, float dt) {
     b->pitch_deg  = imu->pitch;
     b->pitch_rate = imu->pitch_rate;
 
+    /* Advance slope estimator (no-op when not armed) */
+    slope_estimator_update(&b->slope, b->pitch_deg, dt);
+
     /* Safety: tilt cutoff */
     if (b->state == BALANCE_ARMED) {
         if (fabsf(b->pitch_deg) > b->max_tilt) {
             b->state = BALANCE_TILT_FAULT;
             b->motor_cmd = 0;
             b->integral = 0.0f;
+            slope_estimator_reset(&b->slope);
             return;
         }
     }
@@ -45,8 +53,9 @@ void balance_update(balance_t *b, const IMUData *imu, float dt) {
         return;
     }
 
-    /* PID */
-    float error = b->setpoint - b->pitch_deg;
+    /* PID — subtract slope estimate so controller balances on incline */
+    float tilt_corrected = b->pitch_deg - slope_estimator_get_deg(&b->slope);
+    float error = b->setpoint - tilt_corrected;
 
     /* Proportional */
     float p_term = b->kp * error;
@@ -85,4 +94,5 @@ void balance_disarm(balance_t *b) {
     b->state = BALANCE_DISARMED;
     b->motor_cmd = 0;
     b->integral = 0.0f;
+    slope_estimator_reset(&b->slope);
 }

src/jlink.c

@@ -545,6 +545,30 @@ void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm)
     jlink_tx_locked(frame, (uint16_t)(3u + plen + 3u));
 }
 
+/* ---- jlink_send_slope_tlm() -- Issue #600 ---- */
+void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm)
+{
+    /*
+     * Frame: [STX][LEN][0x88][4 bytes SLOPE][CRC_hi][CRC_lo][ETX]
+     * Total: 1+1+1+4+2+1 = 10 bytes
+     */
+    static uint8_t frame[10];
+    const uint8_t  plen = (uint8_t)sizeof(jlink_tlm_slope_t);  /* 4 */
+    const uint8_t  len  = 1u + plen;  /* CMD byte + payload */
+
+    frame[0] = JLINK_STX;
+    frame[1] = len;
+    frame[2] = JLINK_TLM_SLOPE;
+    memcpy(&frame[3], tlm, plen);
+
+    uint16_t crc = crc16_xmodem(&frame[2], len);
+    frame[3 + plen]     = (uint8_t)(crc >> 8);
+    frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
+    frame[3 + plen + 2] = JLINK_ETX;
+
+    jlink_tx_locked(frame, sizeof(frame));
+}
+
 /* ---- jlink_send_fault_log() -- Issue #565 ---- */
 void jlink_send_fault_log(const jlink_tlm_fault_log_t *fl)
 {

src/slope_estimator.c

@@ -0,0 +1,82 @@
+/*
+ * slope_estimator.c — slow-adapting terrain slope estimator (Issue #600).
+ *
+ * First-order IIR low-pass filter on fused IMU pitch with tau = SLOPE_TAU_S (5 s).
+ * The long time constant means fast balance corrections and transients are
+ * ignored; only sustained pitch bias (i.e., genuine slope) accumulates.
+ *
+ * Estimate is clamped to ±SLOPE_MAX_DEG (15°) per the safety requirement.
+ */
+
+#include "slope_estimator.h"
+#include "jlink.h"
+
+/* ---- slope_estimator_init() ---- */
+void slope_estimator_init(slope_estimator_t *se)
+{
+    se->estimate_deg  = 0.0f;
+    se->enabled       = true;
+    /* Initialize so first send_tlm call fires immediately */
+    se->last_tlm_ms   = (uint32_t)(-(uint32_t)(1000u / (SLOPE_TLM_HZ > 0u ? SLOPE_TLM_HZ : 1u)));
+}
+
+/* ---- slope_estimator_reset() ---- */
+void slope_estimator_reset(slope_estimator_t *se)
+{
+    se->estimate_deg = 0.0f;
+}
+
+/* ---- slope_estimator_update() ---- */
+void slope_estimator_update(slope_estimator_t *se, float pitch_deg, float dt)
+{
+    if (!se->enabled || dt <= 0.0f) return;
+
+    /*
+     * First-order IIR: alpha = dt / (tau + dt)
+     * At 1 kHz (dt=0.001): alpha ≈ 0.0002  → time to ~63% ≈ 5 s
+     * At 100 Hz (dt=0.01):  alpha ≈ 0.002   → time to ~63% ≈ 5 s (same tau)
+     */
+    float alpha = dt / (SLOPE_TAU_S + dt);
+    float raw   = se->estimate_deg * (1.0f - alpha) + pitch_deg * alpha;
+
+    /* Clamp to ±SLOPE_MAX_DEG */
+    if (raw >  SLOPE_MAX_DEG) raw =  SLOPE_MAX_DEG;
+    if (raw < -SLOPE_MAX_DEG) raw = -SLOPE_MAX_DEG;
+
+    se->estimate_deg = raw;
+}
+
+/* ---- slope_estimator_get_deg() ---- */
+float slope_estimator_get_deg(const slope_estimator_t *se)
+{
+    if (!se->enabled) return 0.0f;
+    return se->estimate_deg;
+}
+
+/* ---- slope_estimator_set_enabled() ---- */
+void slope_estimator_set_enabled(slope_estimator_t *se, bool en)
+{
+    se->enabled = en;
+}
+
+/* ---- slope_estimator_send_tlm() ---- */
+void slope_estimator_send_tlm(const slope_estimator_t *se, uint32_t now_ms)
+{
+    if (SLOPE_TLM_HZ == 0u) return;
+
+    uint32_t interval_ms = 1000u / SLOPE_TLM_HZ;
+    if ((now_ms - se->last_tlm_ms) < interval_ms) return;
+    /* Cast away const for timestamp update -- only mutable field */
+    ((slope_estimator_t *)se)->last_tlm_ms = now_ms;
+
+    jlink_tlm_slope_t tlm;
+    /* Scale to ×100 for 0.01° resolution, clamped to int16 range */
+    float scaled = se->estimate_deg * 100.0f;
+    if (scaled >  32767.0f) scaled =  32767.0f;
+    if (scaled < -32768.0f) scaled = -32768.0f;
+    tlm.slope_x100 = (int16_t)scaled;
+    tlm.active     = se->enabled ? 1u : 0u;
+    tlm._pad       = 0u;
+
+    jlink_send_slope_tlm(&tlm);
+}

test/test_slope_estimator.c

@@ -0,0 +1,421 @@
+/*
+ * test_slope_estimator.c — Unit tests for slope estimator (Issue #600).
+ *
+ * Build (host, no hardware):
+ *   gcc -I include -I test/stubs -DTEST_HOST -lm \
+ *       -o /tmp/test_slope src/slope_estimator.c test/test_slope_estimator.c
+ *
+ * All tests are self-contained; no HAL, no ADC, no UART required.
+ * slope_estimator.c calls jlink_send_slope_tlm() which is stubbed below.
+ */
+
+/* ---- Stubs ---- */
+
+/* Prevent jlink.h from pulling in HAL / pid_flash types */
+#define JLINK_H
+#include <stdint.h>
+#include <stdbool.h>
+
+/* Minimal jlink_tlm_slope_t matching the real definition */
+typedef struct __attribute__((packed)) {
+    int16_t  slope_x100;
+    uint8_t  active;
+    uint8_t  _pad;
+} jlink_tlm_slope_t;
+
+/* Capture last transmitted tlm for inspection */
+static jlink_tlm_slope_t g_last_tlm;
+static int g_tlm_count = 0;
+
+void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm)
+{
+    g_last_tlm = *tlm;
+    g_tlm_count++;
+}
+
+/* ---- Include implementation directly ---- */
+#include "../src/slope_estimator.c"
+
+/* ---- Test framework ---- */
+#include <stdio.h>
+#include <math.h>
+#include <string.h>
+
+static int g_pass = 0;
+static int g_fail = 0;
+
+#define ASSERT(cond, msg) do { \
+    if (cond) { g_pass++; } \
+    else { g_fail++; printf("FAIL [%s:%d] %s\n", __FILE__, __LINE__, msg); } \
+} while(0)
+
+#define ASSERT_NEAR(a, b, tol, msg) \
+    ASSERT(fabsf((a)-(b)) <= (tol), msg)
+
+/* ---- Tests ---- */
+
+static void test_init_zeroes_state(void)
+{
+    slope_estimator_t se;
+    /* dirty the memory first */
+    memset(&se, 0xAB, sizeof(se));
+    slope_estimator_init(&se);
+    ASSERT(se.estimate_deg == 0.0f,  "init: estimate_deg == 0");
+    ASSERT(se.enabled == true,        "init: enabled == true");
+}
+
+static void test_get_when_disabled_returns_zero(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    /* Force a non-zero estimate by running a few ticks, then disable */
+    slope_estimator_update(&se, 10.0f, 0.1f);
+    slope_estimator_set_enabled(&se, false);
+    ASSERT(slope_estimator_get_deg(&se) == 0.0f,
+           "get while disabled must return 0");
+}
+
+static void test_update_when_disabled_no_change(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    slope_estimator_set_enabled(&se, false);
+    slope_estimator_update(&se, 10.0f, 0.01f);
+    ASSERT(se.estimate_deg == 0.0f,
+           "update while disabled must not change estimate");
+}
+
+static void test_update_zero_dt_no_change(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    slope_estimator_update(&se, 5.0f, 0.0f);
+    ASSERT(se.estimate_deg == 0.0f,
+           "update with dt=0 must not change estimate");
+}
+
+static void test_update_negative_dt_no_change(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    slope_estimator_update(&se, 5.0f, -0.001f);
+    ASSERT(se.estimate_deg == 0.0f,
+           "update with dt<0 must not change estimate");
+}
+
+static void test_single_step_converges_toward_input(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    /* One update with dt=0.01s, pitch=10 deg */
+    slope_estimator_update(&se, 10.0f, 0.01f);
+    /* alpha = 0.01 / (5.0 + 0.01) ≈ 0.002; new estimate ≈ 0 + 0.002*10 = 0.02 */
+    ASSERT(se.estimate_deg > 0.0f,
+           "estimate should move toward positive pitch");
+    ASSERT(se.estimate_deg < 10.0f,
+           "estimate should not jump to full pitch in one step");
+}
+
+static void test_iir_reaches_63pct_in_one_tau(void)
+{
+    /* Run estimator at 1 kHz for exactly SLOPE_TAU_S seconds */
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    const float pitch   = 10.0f;
+    const float dt      = 0.001f;
+    const int   steps   = (int)(SLOPE_TAU_S / dt);  /* 5000 steps */
+    for (int i = 0; i < steps; i++) {
+        slope_estimator_update(&se, pitch, dt);
+    }
+    /* IIR should be at ≈63.2% of the step */
+    float expected_lo = pitch * 0.60f;
+    float expected_hi = pitch * 0.66f;
+    ASSERT(se.estimate_deg >= expected_lo && se.estimate_deg <= expected_hi,
+           "IIR should reach ~63% of step at t=tau");
+}
+
+static void test_iir_reaches_63pct_at_100hz(void)
+{
+    /* Same tau, but at 100 Hz */
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    const float pitch   = 8.0f;
+    const float dt      = 0.01f;
+    const int   steps   = (int)(SLOPE_TAU_S / dt);  /* 500 steps */
+    for (int i = 0; i < steps; i++) {
+        slope_estimator_update(&se, pitch, dt);
+    }
+    float expected_lo = pitch * 0.60f;
+    float expected_hi = pitch * 0.66f;
+    ASSERT(se.estimate_deg >= expected_lo && se.estimate_deg <= expected_hi,
+           "IIR 100 Hz: should reach ~63% at t=tau");
+}
+
+static void test_positive_clamp(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    /* Drive with a huge pitch for a long time — estimate must clamp at SLOPE_MAX_DEG */
+    for (int i = 0; i < 100000; i++) {
+        slope_estimator_update(&se, 90.0f, 0.001f);
+    }
+    ASSERT_NEAR(se.estimate_deg, SLOPE_MAX_DEG, 0.001f,
+                "estimate must clamp at +SLOPE_MAX_DEG");
+}
+
+static void test_negative_clamp(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    for (int i = 0; i < 100000; i++) {
+        slope_estimator_update(&se, -90.0f, 0.001f);
+    }
+    ASSERT_NEAR(se.estimate_deg, -SLOPE_MAX_DEG, 0.001f,
+                "estimate must clamp at -SLOPE_MAX_DEG");
+}
+
+static void test_reset_zeroes_estimate(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    /* Build up some estimate */
+    for (int i = 0; i < 1000; i++) {
+        slope_estimator_update(&se, 12.0f, 0.001f);
+    }
+    ASSERT(se.estimate_deg > 0.1f, "pre-reset: estimate should be non-zero");
+    slope_estimator_reset(&se);
+    ASSERT(se.estimate_deg == 0.0f, "post-reset: estimate should be zero");
+    /* enabled state must be preserved */
+    ASSERT(se.enabled == true, "reset must not change enabled flag");
+}
+
+static void test_reset_preserves_disabled_state(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    slope_estimator_set_enabled(&se, false);
+    slope_estimator_reset(&se);
+    ASSERT(se.enabled == false, "reset must not re-enable disabled estimator");
+}
+
+static void test_balance_setpoint_compensation(void)
+{
+    /*
+     * Simulate: robot on 5-deg slope, pitch = 5 deg, slope estimate converges.
+     * After convergence, tilt_corrected = pitch - slope ≈ 0.
+     * Verify that the effective PID error (setpoint=0, tilt_corrected≈0) is near 0.
+     */
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    const float slope_deg = 5.0f;
+    const float dt        = 0.001f;
+    const int   steps     = (int)(3.0f * SLOPE_TAU_S / dt);  /* 3 tau = ~95% converged */
+
+    for (int i = 0; i < steps; i++) {
+        /* Robot holds steady on slope — pitch stays constant */
+        slope_estimator_update(&se, slope_deg, dt);
+    }
+
+    float est   = slope_estimator_get_deg(&se);
+    float tilt_corrected = slope_deg - est;
+
+    /* After 3 tau, estimate should be within 5% of slope */
+    ASSERT(est >= slope_deg * 0.90f, "3-tau estimate >= 90% of slope");
+    /* tilt_corrected should be close to zero — minimal PID error */
+    ASSERT(fabsf(tilt_corrected) < slope_deg * 0.10f,
+           "corrected tilt error < 10% of slope after 3 tau");
+}
+
+static void test_fast_tilt_not_tracked(void)
+{
+    /*
+     * Simulate: robot starts balanced (pitch≈0), then tips to 20 deg suddenly
+     * for 100 ms (balance intervention), then returns to 0.
+     * The slope estimate should not move significantly.
+     */
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    const float dt = 0.001f;
+
+    /* 2 s of stable balancing at pitch ≈ 0 */
+    for (int i = 0; i < 2000; i++) {
+        slope_estimator_update(&se, 0.0f, dt);
+    }
+    float before = se.estimate_deg;
+
+    /* 100 ms spike to 20 deg (fast tilt) */
+    for (int i = 0; i < 100; i++) {
+        slope_estimator_update(&se, 20.0f, dt);
+    }
+    /* Return to 0 for another 100 ms */
+    for (int i = 0; i < 100; i++) {
+        slope_estimator_update(&se, 0.0f, dt);
+    }
+    float after = se.estimate_deg;
+
+    /* 200ms / 5000ms = 4% of tau — estimate should move < 1 deg */
+    ASSERT(fabsf(after - before) < 1.0f,
+           "fast tilt transient should not significantly move slope estimate");
+}
+
+static void test_slope_change_tracks_slowly(void)
+{
+    /* Robot transitions from flat (0°) to 10° slope at t=0.
+     * After 1 tau, estimate should be ~63% of 10 = ~6.3 deg. */
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    const float dt     = 0.001f;
+    const int   steps  = (int)(SLOPE_TAU_S / dt);
+
+    for (int i = 0; i < steps; i++) {
+        slope_estimator_update(&se, 10.0f, dt);
+    }
+    ASSERT(se.estimate_deg >= 6.0f && se.estimate_deg <= 6.5f,
+           "slope change: 1-tau estimate should be 6.0-6.5 deg");
+}
+
+static void test_symmetry_negative_slope(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    const float dt    = 0.001f;
+    const int   steps = (int)(SLOPE_TAU_S / dt);
+
+    for (int i = 0; i < steps; i++) {
+        slope_estimator_update(&se, -8.0f, dt);
+    }
+    /* Should be ~63% of -8 */
+    ASSERT(se.estimate_deg <= -4.0f && se.estimate_deg >= -5.5f,
+           "negative slope: estimate should converge symmetrically");
+}
+
+static void test_enable_disable_toggle(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+
+    /* Build estimate */
+    for (int i = 0; i < 2000; i++) {
+        slope_estimator_update(&se, 10.0f, 0.001f);
+    }
+    float val = se.estimate_deg;
+    ASSERT(val > 0.1f, "estimate built before disable");
+
+    /* Disable: estimate frozen */
+    slope_estimator_set_enabled(&se, false);
+    slope_estimator_update(&se, 10.0f, 0.001f);
+    ASSERT(se.estimate_deg == val, "estimate frozen while disabled");
+
+    /* get returns 0 when disabled */
+    ASSERT(slope_estimator_get_deg(&se) == 0.0f, "get returns 0 while disabled");
+
+    /* Re-enable: estimate resumes */
+    slope_estimator_set_enabled(&se, true);
+    slope_estimator_update(&se, 10.0f, 0.001f);
+    ASSERT(se.estimate_deg > val, "estimate resumes after re-enable");
+}
+
+static void test_tlm_rate_limiting(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+
+    g_tlm_count = 0;
+
+    /* Call at 1000 Hz for 3 seconds → should send 3 TLMs (at 1 Hz rate limit) */
+    for (uint32_t ms = 0; ms < 3000; ms++) {
+        slope_estimator_send_tlm(&se, ms);
+    }
+    /* First call at ms=0 sends, then 1000ms, 2000ms → 3 total */
+    ASSERT(g_tlm_count == 3, "TLM rate-limited to SLOPE_TLM_HZ (1 Hz)");
+}
+
+static void test_tlm_payload_encoding(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+
+    /* Force a known estimate */
+    se.estimate_deg = 7.5f;
+    se.enabled = true;
+
+    g_tlm_count = 0;
+    g_last_tlm.slope_x100 = 0;
+
+    /* Trigger a TLM by advancing time enough */
+    slope_estimator_send_tlm(&se, 0u);
+
+    ASSERT(g_tlm_count == 1, "TLM sent");
+    ASSERT(g_last_tlm.slope_x100 == 750, "7.5 deg -> slope_x100 = 750");
+    ASSERT(g_last_tlm.active == 1u, "active flag set when enabled");
+}
+
+static void test_tlm_disabled_active_flag(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    slope_estimator_set_enabled(&se, false);
+    se.estimate_deg = 3.0f;
+
+    g_tlm_count = 0;
+    slope_estimator_send_tlm(&se, 0u);
+
+    ASSERT(g_tlm_count == 1, "TLM sent when disabled");
+    ASSERT(g_last_tlm.active == 0u, "active flag clear when disabled");
+    /* slope_x100 reflects stored estimate but get() returns 0 */
+    ASSERT(g_last_tlm.slope_x100 == 300, "slope_x100 encodes stored estimate");
+}
+
+static void test_tlm_clamped_to_int16(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    /* Manually set beyond int16 range for test */
+    se.estimate_deg = 400.0f;  /* 400 * 100 = 40000, within int16 */
+    se.enabled = true;
+    g_tlm_count = 0;
+    slope_estimator_send_tlm(&se, 0u);
+    ASSERT(g_last_tlm.slope_x100 == 32767, "large estimate clamped to INT16_MAX");
+}
+
+static void test_multiple_resets_idempotent(void)
+{
+    slope_estimator_t se;
+    slope_estimator_init(&se);
+    slope_estimator_reset(&se);
+    slope_estimator_reset(&se);
+    ASSERT(se.estimate_deg == 0.0f, "multiple resets: still zero");
+    ASSERT(se.enabled == true, "multiple resets: still enabled");
+}
+
+/* ---- main ---- */
+int main(void)
+{
+    printf("=== test_slope_estimator ===\n");
+
+    test_init_zeroes_state();
+    test_get_when_disabled_returns_zero();
+    test_update_when_disabled_no_change();
+    test_update_zero_dt_no_change();
+    test_update_negative_dt_no_change();
+    test_single_step_converges_toward_input();
+    test_iir_reaches_63pct_in_one_tau();
+    test_iir_reaches_63pct_at_100hz();
+    test_positive_clamp();
+    test_negative_clamp();
+    test_reset_zeroes_estimate();
+    test_reset_preserves_disabled_state();
+    test_balance_setpoint_compensation();
+    test_fast_tilt_not_tracked();
+    test_slope_change_tracks_slowly();
+    test_symmetry_negative_slope();
+    test_enable_disable_toggle();
+    test_tlm_rate_limiting();
+    test_tlm_payload_encoding();
+    test_tlm_disabled_active_flag();
+    test_tlm_clamped_to_int16();
+    test_multiple_resets_idempotent();
+
+    printf("\nResults: %d passed, %d failed\n", g_pass, g_fail);
+    return g_fail ? 1 : 0;
+}

ui/gamepad_panel.css

@@ -0,0 +1,314 @@
+/* gamepad_panel.css — Saltybot Gamepad Teleop (Issue #598) */
+
+*, *::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;
+}
+
+body {
+  font-family: 'Courier New', Courier, monospace;
+  font-size: 12px;
+  background: var(--bg0);
+  color: var(--base);
+  height: 100dvh;
+  display: flex;
+  flex-direction: column;
+  overflow: hidden;
+}
+
+/* ── Header ── */
+#header {
+  display: flex;
+  align-items: center;
+  padding: 5px 12px;
+  background: var(--bg1);
+  border-bottom: 1px solid var(--bd);
+  flex-shrink: 0;
+  gap: 8px;
+  flex-wrap: wrap;
+}
+.logo { color: #f97316; font-weight: bold; letter-spacing: .15em; font-size: 13px; flex-shrink: 0; }
+
+#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; }
+
+#gp-indicator {
+  display: flex; align-items: center; gap: 5px;
+  font-size: 10px; color: var(--mid);
+}
+#gp-dot {
+  width: 8px; height: 8px; border-radius: 50%;
+  background: var(--dim); flex-shrink: 0; transition: background .3s;
+}
+#gp-dot.active { background: var(--amber); }
+
+@keyframes blink { 0%,100%{opacity:1} 50%{opacity:.3} }
+
+#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: 185px;
+}
+#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; }
+
+.estop-btn {
+  border-color: #7f1d1d; background: #1c0505; color: #fca5a5;
+  padding: 3px 12px; font-size: 11px;
+}
+.estop-btn:hover { background: #3b0606; }
+.estop-btn.active {
+  background: #7f1d1d; border-color: var(--red); color: #fff;
+  animation: blink .8s infinite;
+}
+
+.resume-btn {
+  border-color: #14532d; background: #052010; color: #86efac;
+  padding: 3px 12px; font-size: 11px;
+}
+.resume-btn:hover { background: #0a3a1a; }
+
+/* ── Toolbar ── */
+#toolbar {
+  display: flex;
+  align-items: center;
+  gap: 8px;
+  padding: 5px 12px;
+  background: var(--bg1);
+  border-bottom: 1px solid var(--bd);
+  flex-shrink: 0;
+  flex-wrap: wrap;
+  font-size: 10px;
+}
+.tsep { width: 1px; height: 16px; background: var(--bd2); }
+.tlbl { color: var(--mid); letter-spacing: .08em; }
+.tval { color: #67e8f9; min-width: 70px; font-family: monospace; }
+
+.tslider {
+  -webkit-appearance: none;
+  width: 100px; height: 4px; border-radius: 2px;
+  background: var(--bd2); outline: none; cursor: pointer;
+}
+.tslider::-webkit-slider-thumb {
+  -webkit-appearance: none;
+  width: 12px; height: 12px; border-radius: 50%;
+  background: var(--cyan); cursor: pointer;
+}
+.tslider::-moz-range-thumb {
+  width: 12px; height: 12px; border-radius: 50%;
+  background: var(--cyan); cursor: pointer; border: none;
+}
+
+/* ── Main ── */
+#main {
+  flex: 1;
+  display: grid;
+  grid-template-columns: 1fr 220px;
+  min-height: 0;
+  overflow: hidden;
+}
+
+/* ── Joystick area ── */
+#joystick-area {
+  display: flex;
+  align-items: center;
+  justify-content: space-evenly;
+  gap: 12px;
+  padding: 16px;
+  overflow: hidden;
+}
+
+.stick-wrap {
+  display: flex;
+  flex-direction: column;
+  align-items: center;
+  gap: 8px;
+  flex-shrink: 0;
+}
+.stick-label {
+  font-size: 9px; letter-spacing: .12em; color: var(--mid); text-transform: uppercase;
+}
+.stick-vals {
+  font-size: 10px; color: #67e8f9; font-family: monospace; min-width: 100px; text-align: center;
+}
+
+canvas {
+  display: block;
+  border-radius: 50%;
+  border: 1px solid var(--bd2);
+  touch-action: none;
+  cursor: grab;
+}
+canvas.active { cursor: grabbing; }
+
+/* ── Center panel ── */
+#center-panel {
+  flex: 1;
+  display: flex;
+  flex-direction: column;
+  gap: 8px;
+  max-width: 260px;
+  position: relative;
+}
+
+.cp-block {
+  background: var(--bg2);
+  border: 1px solid var(--bd2);
+  border-radius: 6px;
+  padding: 8px;
+}
+.cp-title {
+  font-size: 9px; font-weight: bold; letter-spacing: .12em;
+  color: #0891b2; text-transform: uppercase; margin-bottom: 5px;
+}
+.cp-row {
+  display: flex; justify-content: space-between;
+  padding: 2px 0; border-bottom: 1px solid var(--bd);
+  font-size: 10px;
+}
+.cp-row:last-child { border-bottom: none; }
+.cp-lbl { color: var(--mid); }
+.cp-val { color: var(--hi); font-family: monospace; }
+
+/* Keyboard diagram */
+.key-grid {
+  display: grid;
+  grid-template-columns: repeat(3, 28px);
+  grid-template-rows: repeat(3, 22px);
+  gap: 3px;
+  justify-content: center;
+  margin: 4px 0;
+}
+.key {
+  background: var(--bg1);
+  border: 1px solid var(--bd2);
+  border-radius: 3px;
+  display: flex; align-items: center; justify-content: center;
+  font-size: 9px; font-weight: bold; color: var(--mid);
+  transition: background .1s, color .1s, border-color .1s;
+}
+.key.pressed {
+  background: #0e4f69; border-color: var(--cyan); color: #fff;
+}
+.key-wide {
+  grid-column: 1 / 4;
+  width: 100%; height: 22px;
+}
+
+/* E-stop overlay */
+#estop-panel {
+  position: absolute; inset: 0;
+  background: rgba(127,0,0,0.9);
+  border: 2px solid var(--red);
+  border-radius: 8px;
+  display: flex; flex-direction: column;
+  align-items: center; justify-content: center;
+  gap: 4px;
+  animation: blink .8s infinite;
+}
+.estop-text { font-size: 28px; font-weight: bold; color: #fca5a5; letter-spacing: .1em; }
+.estop-sub  { font-size: 11px; color: #fca5a5; letter-spacing: .2em; }
+
+/* ── Sidebar ── */
+#sidebar {
+  background: var(--bg1);
+  border-left: 1px solid var(--bd);
+  display: flex;
+  flex-direction: column;
+  overflow-y: auto;
+  padding: 8px;
+  gap: 8px;
+  font-size: 11px;
+}
+.sb-card {
+  background: var(--bg2);
+  border: 1px solid var(--bd2);
+  border-radius: 6px;
+  padding: 8px;
+}
+.sb-title {
+  font-size: 9px; font-weight: bold; letter-spacing: .12em;
+  color: #0891b2; text-transform: uppercase; margin-bottom: 6px;
+}
+.sb-row {
+  display: flex; justify-content: space-between;
+  padding: 2px 0; border-bottom: 1px solid var(--bd);
+  font-size: 10px;
+}
+.sb-row:last-child { border-bottom: none; }
+.sb-lbl { color: var(--mid); }
+.sb-val { color: var(--hi); font-family: monospace; }
+
+/* Gamepad axis bars */
+.gp-axis-row {
+  display: flex; align-items: center; gap: 4px;
+  font-size: 9px; color: var(--mid); margin-bottom: 3px;
+}
+.gp-axis-label { width: 30px; flex-shrink: 0; }
+.gp-bar-track {
+  flex: 1; height: 6px; background: var(--bg0);
+  border: 1px solid var(--bd); border-radius: 3px; overflow: hidden;
+  position: relative;
+}
+.gp-bar-center {
+  position: absolute; top: 0; bottom: 0;
+  left: 50%; width: 1px; background: var(--dim);
+}
+.gp-bar-fill {
+  position: absolute; top: 0; bottom: 0;
+  background: var(--cyan); transition: none;
+}
+.gp-axis-val { width: 32px; text-align: right; color: var(--hi); flex-shrink: 0; }
+
+.gp-btn-row {
+  display: flex; flex-wrap: wrap; gap: 3px;
+  margin-top: 6px;
+}
+.gp-btn-chip {
+  font-size: 8px; padding: 1px 5px; border-radius: 2px;
+  border: 1px solid var(--bd); background: var(--bg0); color: var(--dim);
+  transition: background .1s, color .1s;
+}
+.gp-btn-chip.pressed { background: var(--bd2); color: var(--hi); border-color: var(--cyan); }
+
+/* ── Footer ── */
+#footer {
+  background: var(--bg1); border-top: 1px solid var(--bd);
+  padding: 3px 12px;
+  display: flex; align-items: center; justify-content: space-between;
+  flex-shrink: 0; font-size: 10px; color: var(--dim);
+}
+
+/* ── Responsive ── */
+@media (max-width: 800px) {
+  #main { grid-template-columns: 1fr; }
+  #sidebar { display: none; }
+}
+@media (max-width: 560px) {
+  #right-wrap { display: none; }
+  #center-panel { max-width: 100%; }
+  .tslider { width: 70px; }
+}

ui/gamepad_panel.html

@@ -0,0 +1,197 @@
+<!DOCTYPE html>
+<html lang="en">
+<head>
+<meta charset="UTF-8">
+<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
+<title>Saltybot — Gamepad Teleop</title>
+<link rel="stylesheet" href="gamepad_panel.css">
+<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script>
+</head>
+<body>
+
+<!-- ── Header ── -->
+<div id="header">
+  <div class="logo">⚡ SALTYBOT — GAMEPAD TELEOP</div>
+  <div id="conn-dot"></div>
+  <input id="ws-input" type="text" value="ws://localhost:9090" placeholder="ws://robot-ip:9090" />
+  <button id="btn-connect" class="hbtn">CONNECT</button>
+  <span id="conn-label" style="color:#4b5563;font-size:10px">Not connected</span>
+  <div style="flex:1"></div>
+  <div id="gp-indicator">
+    <div id="gp-dot"></div>
+    <span id="gp-label">No gamepad</span>
+  </div>
+</div>
+
+<!-- ── Toolbar ── -->
+<div id="toolbar">
+  <!-- Speed limiter -->
+  <span class="tlbl">LINEAR</span>
+  <input id="slider-linear" type="range" min="10" max="100" value="50" class="tslider">
+  <span id="val-linear" class="tval">0.50 m/s</span>
+
+  <div class="tsep"></div>
+
+  <span class="tlbl">ANGULAR</span>
+  <input id="slider-angular" type="range" min="10" max="100" value="50" class="tslider">
+  <span id="val-angular" class="tval">1.00 rad/s</span>
+
+  <div class="tsep"></div>
+
+  <span class="tlbl">DEADZONE</span>
+  <input id="slider-dz" type="range" min="0" max="40" value="10" class="tslider" style="width:70px">
+  <span id="val-dz" class="tval">10%</span>
+
+  <div class="tsep"></div>
+
+  <button id="btn-estop" class="hbtn estop-btn">⛔ E-STOP</button>
+  <button id="btn-resume" class="hbtn resume-btn" style="display:none">▶ RESUME</button>
+</div>
+
+<!-- ── Main ── -->
+<div id="main">
+
+  <!-- Virtual joystick area -->
+  <div id="joystick-area">
+    <div class="stick-wrap" id="left-wrap">
+      <div class="stick-label">LEFT — DRIVE</div>
+      <canvas id="left-stick" width="200" height="200"></canvas>
+      <div class="stick-vals" id="left-vals">↕ 0.00 m/s</div>
+    </div>
+
+    <!-- Center info -->
+    <div id="center-panel">
+      <div class="cp-block">
+        <div class="cp-title">COMMAND</div>
+        <div class="cp-row">
+          <span class="cp-lbl">Linear</span>
+          <span class="cp-val" id="disp-linear">0.00 m/s</span>
+        </div>
+        <div class="cp-row">
+          <span class="cp-lbl">Angular</span>
+          <span class="cp-val" id="disp-angular">0.00 rad/s</span>
+        </div>
+      </div>
+
+      <div class="cp-block">
+        <div class="cp-title">LIMITS</div>
+        <div class="cp-row">
+          <span class="cp-lbl">Max lin</span>
+          <span class="cp-val" id="lim-linear">0.50 m/s</span>
+        </div>
+        <div class="cp-row">
+          <span class="cp-lbl">Max ang</span>
+          <span class="cp-val" id="lim-angular">1.00 rad/s</span>
+        </div>
+        <div class="cp-row">
+          <span class="cp-lbl">Deadzone</span>
+          <span class="cp-val" id="lim-dz">10%</span>
+        </div>
+      </div>
+
+      <div class="cp-block">
+        <div class="cp-title">KEYBOARD</div>
+        <div class="key-grid">
+          <div></div>
+          <div class="key" id="key-w">W</div>
+          <div></div>
+          <div class="key" id="key-a">A</div>
+          <div class="key" id="key-s">S</div>
+          <div class="key" id="key-d">D</div>
+          <div></div>
+          <div class="key key-wide" id="key-space">SPC</div>
+          <div></div>
+        </div>
+        <div style="font-size:9px;color:#4b5563;text-align:center;margin-top:4px">
+          W/S=fwd/rev · A/D=turn · SPC=stop
+        </div>
+      </div>
+
+      <!-- Gamepad layout diagram -->
+      <div class="cp-block">
+        <div class="cp-title">GAMEPAD MAPPING</div>
+        <div style="font-size:9px;color:#6b7280;line-height:1.8">
+          <div>L-stick ↕ → linear vel</div>
+          <div>R-stick ↔ → angular vel</div>
+          <div>LT/RT → fine speed ctrl</div>
+          <div>B/Circle → E-stop toggle</div>
+          <div>Start → Resume</div>
+        </div>
+      </div>
+
+      <div id="estop-panel" style="display:none">
+        <div class="estop-text">⛔ E-STOP</div>
+        <div class="estop-sub">ACTIVE</div>
+      </div>
+    </div>
+
+    <div class="stick-wrap" id="right-wrap">
+      <div class="stick-label">RIGHT — STEER</div>
+      <canvas id="right-stick" width="200" height="200"></canvas>
+      <div class="stick-vals" id="right-vals">↔ 0.00 rad/s</div>
+    </div>
+  </div>
+
+  <!-- Sidebar -->
+  <aside id="sidebar">
+
+    <!-- Status -->
+    <div class="sb-card">
+      <div class="sb-title">Status</div>
+      <div class="sb-row">
+        <span class="sb-lbl">E-stop</span>
+        <span class="sb-val" id="sb-estop" style="color:#6b7280">OFF</span>
+      </div>
+      <div class="sb-row">
+        <span class="sb-lbl">Input</span>
+        <span class="sb-val" id="sb-input">—</span>
+      </div>
+      <div class="sb-row">
+        <span class="sb-lbl">Pub rate</span>
+        <span class="sb-val" id="sb-rate">—</span>
+      </div>
+      <div class="sb-row">
+        <span class="sb-lbl">Topic</span>
+        <span class="sb-val" style="font-size:9px">/cmd_vel</span>
+      </div>
+    </div>
+
+    <!-- Gamepad raw -->
+    <div class="sb-card">
+      <div class="sb-title">Gamepad Raw</div>
+      <div id="gp-raw">
+        <div style="color:#374151;font-size:10px;text-align:center;padding:12px 0">
+          No gamepad connected.<br>Press any button to activate.
+        </div>
+      </div>
+    </div>
+
+    <!-- Topics -->
+    <div class="sb-card">
+      <div class="sb-title">Topics</div>
+      <div style="font-size:9px;color:#374151;line-height:1.9">
+        <div>PUB <code style="color:#4b5563">/cmd_vel</code></div>
+        <div style="color:#1e3a5f;padding-left:8px">geometry_msgs/Twist</div>
+        <div style="margin-top:4px;color:#6b7280">20 Hz publish rate</div>
+        <div style="margin-top:4px;color:#6b7280">Sends zero on E-stop</div>
+      </div>
+    </div>
+
+  </aside>
+</div>
+
+<!-- ── Footer ── -->
+<div id="footer">
+  <span>virtual sticks · WASD keyboard · Web Gamepad API</span>
+  <span>rosbridge: <code id="footer-ws">ws://localhost:9090</code></span>
+  <span>gamepad teleop — issue #598</span>
+</div>
+
+<script src="gamepad_panel.js"></script>
+<script>
+  document.getElementById('ws-input').addEventListener('input', (e) => {
+    document.getElementById('footer-ws').textContent = e.target.value;
+  });
+</script>
+</body>
+</html>

ui/gamepad_panel.js

@@ -0,0 +1,548 @@
+/* gamepad_panel.js — Saltybot Gamepad Teleop (Issue #598) */
+'use strict';
+
+// ── Constants ──────────────────────────────────────────────────────────────
+const MAX_LINEAR  = 1.0;   // absolute max m/s
+const MAX_ANGULAR = 2.0;   // absolute max rad/s
+const PUBLISH_HZ  = 20;
+const PUBLISH_MS  = 1000 / PUBLISH_HZ;
+
+// ── State ──────────────────────────────────────────────────────────────────
+const state = {
+  connected: false,
+  estop: false,
+  // Speed limits (0..1 fraction of max)
+  linLimit:  0.5,
+  angLimit:  0.5,
+  deadzone:  0.10,
+  // Current command
+  linVel:    0,
+  angVel:    0,
+  // Input source: 'none' | 'keyboard' | 'virtual' | 'gamepad'
+  inputSrc:  'none',
+  // Keyboard keys held
+  keys: { w: false, a: false, s: false, d: false, space: false },
+  // Virtual stick drags
+  sticks: {
+    left:  { active: false, dx: 0, dy: 0 },
+    right: { active: false, dx: 0, dy: 0 },
+  },
+  // Gamepad
+  gpIndex:   null,
+  gpPrevBtns: [],
+  // Stats
+  pubCount:  0,
+  pubTs:     0,
+  pubRate:   0,
+};
+
+// ── ROS ────────────────────────────────────────────────────────────────────
+let ros = null;
+let cmdVelTopic = null;
+
+function rosCmdVelSetup() {
+  cmdVelTopic = new ROSLIB.Topic({
+    ros,
+    name:        '/cmd_vel',
+    messageType: 'geometry_msgs/Twist',
+  });
+}
+
+function publishTwist(lin, ang) {
+  if (!ros || !cmdVelTopic || !state.connected) return;
+  cmdVelTopic.publish(new ROSLIB.Message({
+    linear:  { x: lin, y: 0, z: 0 },
+    angular: { x: 0,   y: 0, z: ang },
+  }));
+  state.pubCount++;
+  const now = performance.now();
+  if (state.pubTs) {
+    const dt = (now - state.pubTs) / 1000;
+    state.pubRate = 1 / dt;
+  }
+  state.pubTs = now;
+}
+
+function sendStop() { publishTwist(0, 0); }
+
+// ── Connection ─────────────────────────────────────────────────────────────
+const $connDot   = document.getElementById('conn-dot');
+const $connLabel = document.getElementById('conn-label');
+const $btnConn   = document.getElementById('btn-connect');
+const $wsInput   = document.getElementById('ws-input');
+
+function connect() {
+  const url = $wsInput.value.trim() || 'ws://localhost:9090';
+  if (ros) { try { ros.close(); } catch(_){} }
+
+  $connLabel.textContent = 'Connecting…';
+  $connLabel.style.color = '#d97706';
+  $connDot.className = '';
+
+  ros = new ROSLIB.Ros({ url });
+
+  ros.on('connection', () => {
+    state.connected = true;
+    $connDot.className = 'connected';
+    $connLabel.style.color = '#22c55e';
+    $connLabel.textContent = 'Connected';
+    rosCmdVelSetup();
+    localStorage.setItem('gp_ws_url', url);
+  });
+
+  ros.on('error', () => {
+    state.connected = false;
+    $connDot.className = 'error';
+    $connLabel.style.color = '#ef4444';
+    $connLabel.textContent = 'Error';
+  });
+
+  ros.on('close', () => {
+    state.connected = false;
+    $connDot.className = '';
+    $connLabel.style.color = '#6b7280';
+    $connLabel.textContent = 'Disconnected';
+  });
+}
+
+$btnConn.addEventListener('click', connect);
+
+// Restore saved URL
+const savedUrl = localStorage.getItem('gp_ws_url');
+if (savedUrl) {
+  $wsInput.value = savedUrl;
+  document.getElementById('footer-ws').textContent = savedUrl;
+}
+
+// ── E-stop ─────────────────────────────────────────────────────────────────
+const $btnEstop  = document.getElementById('btn-estop');
+const $btnResume = document.getElementById('btn-resume');
+const $estopPanel = document.getElementById('estop-panel');
+const $sbEstop   = document.getElementById('sb-estop');
+
+function activateEstop() {
+  state.estop = true;
+  sendStop();
+  $btnEstop.style.display  = 'none';
+  $btnResume.style.display = '';
+  $btnEstop.classList.add('active');
+  $estopPanel.style.display = 'flex';
+  $sbEstop.textContent  = 'ACTIVE';
+  $sbEstop.style.color  = '#ef4444';
+}
+
+function resumeFromEstop() {
+  state.estop = false;
+  $btnEstop.style.display  = '';
+  $btnResume.style.display = 'none';
+  $btnEstop.classList.remove('active');
+  $estopPanel.style.display = 'none';
+  $sbEstop.textContent = 'OFF';
+  $sbEstop.style.color = '#6b7280';
+}
+
+$btnEstop.addEventListener('click',  activateEstop);
+$btnResume.addEventListener('click', resumeFromEstop);
+
+// ── Sliders ────────────────────────────────────────────────────────────────
+function setupSlider(id, valId, minVal, maxVal, onUpdate) {
+  const slider = document.getElementById(id);
+  const valEl  = document.getElementById(valId);
+  slider.addEventListener('input', () => {
+    const frac = parseInt(slider.value) / 100;
+    onUpdate(frac, valEl);
+  });
+  // Init
+  const frac = parseInt(slider.value) / 100;
+  onUpdate(frac, valEl);
+}
+
+setupSlider('slider-linear', 'val-linear', 0, MAX_LINEAR, (frac, el) => {
+  state.linLimit = frac;
+  const ms = (frac * MAX_LINEAR).toFixed(2);
+  el.textContent = ms + ' m/s';
+  document.getElementById('lim-linear').textContent = ms + ' m/s';
+});
+
+setupSlider('slider-angular', 'val-angular', 0, MAX_ANGULAR, (frac, el) => {
+  state.angLimit = frac;
+  const rs = (frac * MAX_ANGULAR).toFixed(2);
+  el.textContent = rs + ' rad/s';
+  document.getElementById('lim-angular').textContent = rs + ' rad/s';
+});
+
+// Deadzone slider (0–40%)
+const $dzSlider = document.getElementById('slider-dz');
+const $dzVal    = document.getElementById('val-dz');
+const $limDz    = document.getElementById('lim-dz');
+$dzSlider.addEventListener('input', () => {
+  state.deadzone = parseInt($dzSlider.value) / 100;
+  $dzVal.textContent  = parseInt($dzSlider.value) + '%';
+  $limDz.textContent  = parseInt($dzSlider.value) + '%';
+});
+
+// ── Keyboard input ─────────────────────────────────────────────────────────
+const KEY_EL = {
+  w: document.getElementById('key-w'),
+  a: document.getElementById('key-a'),
+  s: document.getElementById('key-s'),
+  d: document.getElementById('key-d'),
+  space: document.getElementById('key-space'),
+};
+
+function setKeyState(key, down) {
+  if (!(key in state.keys)) return;
+  state.keys[key] = down;
+  if (KEY_EL[key]) KEY_EL[key].classList.toggle('pressed', down);
+}
+
+document.addEventListener('keydown', (e) => {
+  if (e.target.tagName === 'INPUT') return;
+  switch (e.code) {
+    case 'KeyW': case 'ArrowUp':    setKeyState('w', true);     break;
+    case 'KeyS': case 'ArrowDown':  setKeyState('s', true);     break;
+    case 'KeyA': case 'ArrowLeft':  setKeyState('a', true);     break;
+    case 'KeyD': case 'ArrowRight': setKeyState('d', true);     break;
+    case 'Space':
+      e.preventDefault();
+      if (state.estop) { resumeFromEstop(); } else { activateEstop(); }
+      break;
+  }
+});
+
+document.addEventListener('keyup', (e) => {
+  switch (e.code) {
+    case 'KeyW': case 'ArrowUp':    setKeyState('w', false); break;
+    case 'KeyS': case 'ArrowDown':  setKeyState('s', false); break;
+    case 'KeyA': case 'ArrowLeft':  setKeyState('a', false); break;
+    case 'KeyD': case 'ArrowRight': setKeyState('d', false); break;
+  }
+});
+
+function getKeyboardCommand() {
+  let lin = 0, ang = 0;
+  if (state.keys.w) lin += 1;
+  if (state.keys.s) lin -= 1;
+  if (state.keys.a) ang += 1;
+  if (state.keys.d) ang -= 1;
+  return { lin, ang };
+}
+
+// ── Virtual Joysticks ──────────────────────────────────────────────────────
+function setupVirtualStick(canvasId, stickKey, onValue) {
+  const canvas = document.getElementById(canvasId);
+  const R = canvas.width / 2;
+  const ctx = canvas.getContext('2d');
+  const stick = state.sticks[stickKey];
+  let pointerId = null;
+
+  function draw() {
+    const W = canvas.width, H = canvas.height;
+    ctx.clearRect(0, 0, W, H);
+
+    // Base circle
+    ctx.beginPath();
+    ctx.arc(R, R, R - 2, 0, Math.PI * 2);
+    ctx.fillStyle = 'rgba(10,10,26,0.9)';
+    ctx.fill();
+    ctx.strokeStyle = '#1e3a5f';
+    ctx.lineWidth = 1.5;
+    ctx.stroke();
+
+    // Crosshair
+    ctx.strokeStyle = '#374151';
+    ctx.lineWidth = 0.5;
+    ctx.beginPath(); ctx.moveTo(R, 4); ctx.lineTo(R, H - 4); ctx.stroke();
+    ctx.beginPath(); ctx.moveTo(4, R); ctx.lineTo(W - 4, R); ctx.stroke();
+
+    // Deadzone ring
+    const dzR = state.deadzone * (R - 10);
+    ctx.beginPath();
+    ctx.arc(R, R, dzR, 0, Math.PI * 2);
+    ctx.strokeStyle = '#374151';
+    ctx.lineWidth = 1;
+    ctx.setLineDash([3, 3]);
+    ctx.stroke();
+    ctx.setLineDash([]);
+
+    // Knob
+    const kx = R + stick.dx * (R - 16);
+    const ky = R + stick.dy * (R - 16);
+    const kColor = stick.active ? '#06b6d4' : '#1e3a5f';
+    ctx.beginPath();
+    ctx.arc(kx, ky, 16, 0, Math.PI * 2);
+    ctx.fillStyle = kColor + '44';
+    ctx.fill();
+    ctx.strokeStyle = kColor;
+    ctx.lineWidth = 2;
+    ctx.stroke();
+  }
+
+  function getOffset(e) {
+    const rect = canvas.getBoundingClientRect();
+    return {
+      x: (e.clientX - rect.left) / rect.width  * canvas.width,
+      y: (e.clientY - rect.top)  / rect.height * canvas.height,
+    };
+  }
+
+  function pointerdown(e) {
+    canvas.setPointerCapture(e.pointerId);
+    pointerId = e.pointerId;
+    stick.active = true;
+    canvas.classList.add('active');
+    update(e);
+  }
+
+  function pointermove(e) {
+    if (!stick.active || e.pointerId !== pointerId) return;
+    update(e);
+  }
+
+  function pointerup(e) {
+    if (e.pointerId !== pointerId) return;
+    stick.active = false;
+    stick.dx = 0;
+    stick.dy = 0;
+    pointerId = null;
+    canvas.classList.remove('active');
+    onValue(0, 0);
+    draw();
+  }
+
+  function update(e) {
+    const { x, y } = getOffset(e);
+    let dx = (x - R) / (R - 16);
+    let dy = (y - R) / (R - 16);
+    const dist = Math.sqrt(dx * dx + dy * dy);
+    if (dist > 1) { dx /= dist; dy /= dist; }
+    stick.dx = dx;
+    stick.dy = dy;
+    // Apply deadzone
+    const norm = Math.sqrt(dx * dx + dy * dy);
+    if (norm < state.deadzone) { onValue(0, 0); }
+    else {
+      const scale = (norm - state.deadzone) / (1 - state.deadzone);
+      onValue((-dy / norm) * scale, (-dx / norm) * scale); // up=+lin, left=+ang
+    }
+    draw();
+  }
+
+  canvas.addEventListener('pointerdown', pointerdown);
+  canvas.addEventListener('pointermove', pointermove);
+  canvas.addEventListener('pointerup',   pointerup);
+  canvas.addEventListener('pointercancel', pointerup);
+
+  draw();
+  return { draw };
+}
+
+let virtLeftLin = 0, virtLeftAng = 0;
+let virtRightLin = 0, virtRightAng = 0;
+
+const leftStickDraw  = setupVirtualStick('left-stick',  'left',  (fwd, _)   => { virtLeftLin  = fwd; updateSidebarInput(); }).draw;
+const rightStickDraw = setupVirtualStick('right-stick', 'right', (_, turn)  => { virtRightAng = turn; updateSidebarInput(); }).draw;
+
+// Redraw sticks when deadzone changes (for dz ring)
+$dzSlider.addEventListener('input', () => {
+  if (leftStickDraw)  leftStickDraw();
+  if (rightStickDraw) rightStickDraw();
+});
+
+// ── Gamepad API ────────────────────────────────────────────────────────────
+const $gpDot   = document.getElementById('gp-dot');
+const $gpLabel = document.getElementById('gp-label');
+const $gpRaw   = document.getElementById('gp-raw');
+
+window.addEventListener('gamepadconnected', (e) => {
+  state.gpIndex = e.gamepad.index;
+  $gpDot.classList.add('active');
+  $gpLabel.textContent = e.gamepad.id.substring(0, 28);
+  updateGpRaw();
+});
+
+window.addEventListener('gamepaddisconnected', (e) => {
+  if (e.gamepad.index === state.gpIndex) {
+    state.gpIndex = null;
+    state.gpPrevBtns = [];
+    $gpDot.classList.remove('active');
+    $gpLabel.textContent = 'No gamepad';
+    $gpRaw.innerHTML = '<div style="color:#374151;font-size:10px;text-align:center;padding:12px 0">No gamepad connected.<br>Press any button to activate.</div>';
+  }
+});
+
+function applyDeadzone(v, dz) {
+  const sign = v >= 0 ? 1 : -1;
+  const abs  = Math.abs(v);
+  if (abs < dz) return 0;
+  return sign * (abs - dz) / (1 - dz);
+}
+
+function getGamepadCommand() {
+  if (state.gpIndex === null) return null;
+  const gp = navigator.getGamepads()[state.gpIndex];
+  if (!gp) return null;
+
+  // Standard mapping:
+  // Axes: 0=LX, 1=LY, 2=RX, 3=RY
+  // Buttons: 0=A/X, 1=B/Circle, 6=LT, 7=RT, 9=Start
+  const dz = state.deadzone;
+  const ly  = applyDeadzone(gp.axes[1] || 0, dz);
+  const rx  = applyDeadzone(gp.axes[2] || 0, dz);
+
+  // LT/RT fine speed override (reduce max by up to 50%)
+  const lt = gp.buttons[6] ? (gp.buttons[6].value || 0) : 0;
+  const rt = gp.buttons[7] ? (gp.buttons[7].value || 0) : 0;
+  const triggerScale = 1 - (lt * 0.5) - (rt * 0.5) + (rt * 0.5); // RT = full, LT = half
+  const speedScale = lt > 0.1 ? (1 - lt * 0.5) : 1.0;
+
+  // B/Circle = e-stop
+  const bBtn = gp.buttons[1] && gp.buttons[1].pressed;
+  const prevBBtn = state.gpPrevBtns[1] || false;
+  if (bBtn && !prevBBtn) {
+    if (!state.estop) activateEstop(); else resumeFromEstop();
+  }
+
+  // Start = resume
+  const startBtn = gp.buttons[9] && gp.buttons[9].pressed;
+  const prevStart = state.gpPrevBtns[9] || false;
+  if (startBtn && !prevStart && state.estop) resumeFromEstop();
+
+  state.gpPrevBtns = gp.buttons.map(b => b.pressed);
+
+  return { lin: -ly * speedScale, ang: -rx };
+}
+
+function updateGpRaw() {
+  if (state.gpIndex === null) return;
+  const gp = navigator.getGamepads()[state.gpIndex];
+  if (!gp) return;
+
+  const AXIS_NAMES = ['LX', 'LY', 'RX', 'RY'];
+  const BTN_NAMES  = ['A','B','X','Y','LB','RB','LT','RT','Back','Start',
+                      'LS','RS','↑','↓','←','→','Home'];
+
+  let html = '';
+  // Axes
+  for (let i = 0; i < Math.min(gp.axes.length, 4); i++) {
+    const v  = gp.axes[i];
+    const pct = ((v + 1) / 2 * 100).toFixed(0);
+    const fill = v >= 0
+      ? `left:50%;width:${(v/2*100).toFixed(0)}%`
+      : `left:${((v+1)/2*100).toFixed(0)}%;width:${(Math.abs(v)/2*100).toFixed(0)}%`;
+    html += `<div class="gp-axis-row">
+      <span class="gp-axis-label">${AXIS_NAMES[i]||'A'+i}</span>
+      <div class="gp-bar-track">
+        <div class="gp-bar-center"></div>
+        <div class="gp-bar-fill" style="${fill}"></div>
+      </div>
+      <span class="gp-axis-val">${v.toFixed(2)}</span>
+    </div>`;
+  }
+  // Buttons
+  html += '<div class="gp-btn-row">';
+  for (let i = 0; i < gp.buttons.length; i++) {
+    const pressed = gp.buttons[i].pressed;
+    const name = BTN_NAMES[i] || 'B'+i;
+    html += `<span class="gp-btn-chip${pressed?' pressed':''}">${name}</span>`;
+  }
+  html += '</div>';
+  $gpRaw.innerHTML = html;
+}
+
+// ── Velocity display helpers ───────────────────────────────────────────────
+const $dispLinear  = document.getElementById('disp-linear');
+const $dispAngular = document.getElementById('disp-angular');
+const $leftVals    = document.getElementById('left-vals');
+const $rightVals   = document.getElementById('right-vals');
+const $sbInput     = document.getElementById('sb-input');
+const $sbRate      = document.getElementById('sb-rate');
+
+function updateSidebarInput() {
+  const src = state.inputSrc;
+  $sbInput.textContent = src === 'gamepad' ? 'Gamepad' :
+                          src === 'keyboard' ? 'Keyboard' :
+                          src === 'virtual'  ? 'Virtual sticks' : '—';
+}
+
+// ── Main publish loop ──────────────────────────────────────────────────────
+let lastPublish = 0;
+
+function loop(ts) {
+  requestAnimationFrame(loop);
+
+  // Gamepad raw display (60fps)
+  if (state.gpIndex !== null) updateGpRaw();
+
+  // Publish at limited rate
+  if (ts - lastPublish < PUBLISH_MS) return;
+  lastPublish = ts;
+
+  // Determine command from highest-priority active input
+  let lin = 0, ang = 0;
+  let src = 'none';
+
+  // 1. Gamepad (highest priority if connected)
+  const gpCmd = getGamepadCommand();
+  if (gpCmd && (Math.abs(gpCmd.lin) > 0.001 || Math.abs(gpCmd.ang) > 0.001)) {
+    lin = gpCmd.lin;
+    ang = gpCmd.ang;
+    src = 'gamepad';
+  }
+  // 2. Keyboard
+  else {
+    const kbCmd = getKeyboardCommand();
+    if (kbCmd.lin !== 0 || kbCmd.ang !== 0) {
+      lin = kbCmd.lin;
+      ang = kbCmd.ang;
+      src = 'keyboard';
+    }
+    // 3. Virtual sticks
+    else if (state.sticks.left.active || state.sticks.right.active) {
+      lin = virtLeftLin;
+      ang = virtRightAng;
+      src = 'virtual';
+    }
+  }
+
+  // Apply speed limits
+  lin = lin * state.linLimit * MAX_LINEAR;
+  ang = ang * state.angLimit * MAX_ANGULAR;
+
+  // Clamp
+  lin = Math.max(-MAX_LINEAR,  Math.min(MAX_LINEAR,  lin));
+  ang = Math.max(-MAX_ANGULAR, Math.min(MAX_ANGULAR, ang));
+
+  state.linVel = lin;
+  state.angVel = ang;
+  state.inputSrc = src;
+
+  if (state.estop) { lin = 0; ang = 0; }
+
+  // Publish
+  publishTwist(lin, ang);
+
+  // Update displays
+  $dispLinear.textContent  = lin.toFixed(2) + ' m/s';
+  $dispAngular.textContent = ang.toFixed(2) + ' rad/s';
+  $leftVals.textContent    = '↕ ' + lin.toFixed(2) + ' m/s';
+  $rightVals.textContent   = '↔ ' + ang.toFixed(2) + ' rad/s';
+
+  updateSidebarInput();
+  $sbRate.textContent = state.pubRate > 0 ? state.pubRate.toFixed(1) + ' Hz' : '—';
+
+  // Color command display
+  const linColor  = Math.abs(lin)  > 0.01 ? '#22c55e' : '#6b7280';
+  const angColor  = Math.abs(ang)  > 0.01 ? '#06b6d4' : '#6b7280';
+  $dispLinear.style.color  = linColor;
+  $dispAngular.style.color = angColor;
+  $leftVals.style.color    = linColor;
+  $rightVals.style.color   = angColor;
+}
+
+requestAnimationFrame(loop);
+
+// ── Init auto-connect if URL saved ────────────────────────────────────────
+if (savedUrl) {
+  connect();
+}