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,343 +1,502 @@
 // ============================================================
-// RPLIDAR A1 Mount Bracket — Issue #596
-// Agent   : sl-mechanical
-// Date    : 2026-03-14
+// 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
+//
 // Part catalogue:
-//   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
+//   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()
 //
-// 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)
+// 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
 //
-// 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
+// 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)
 //
 // Export commands:
-//   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
+//   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
 // ============================================================
 
-// ── Render selector ─────────────────────────────────────────
-RENDER = "assembly"; // tnut_base | column | scan_platform | vibe_ring | cable_guide | assembly
-
-// ── Global constants ────────────────────────────────────────
 $fn = 64;
-EPS = 0.01;
+e   = 0.01;
 
-// 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
+// ── Parametric elevation ──────────────────────────────────────────────────────
+ELEV_H     = 120.0;   // scan plane elevation above rail face (mm)
+                      // increase for taller chassis; min ~60 mm recommended
 
-// 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)
+// ── 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
 
-// Elevation column
-COL_OD       = 25.0;   // column outer diameter
-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
+// ── 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;
 
-// 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
+// ── 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;
 
-// 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
+// ── 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)
 
-// 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;
+// ── Column geometry ───────────────────────────────────────────────────────────
+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
 
-// ── 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]);
-    }
+// ── 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
+
+// ── 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)
+
+// ── 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
+
+// ── 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();
 }
 
-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 ───────────────────────────────────────
+// ============================================================
+// 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 tnut_base() {
     difference() {
-        // Body
         union() {
-            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);
+            // ── 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);
         }
 
-        // 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);
-            }
+        // ── 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);
 
-        // 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]);
+        // ── 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);
 
-        // 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 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);
 
-        // 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]);
+        // ── 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);
 
-        // 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]);
+        // ── 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);
     }
 }
 
-// ── Part 2: column ──────────────────────────────────────────
+// ============================================================
+// 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).
 module column() {
-    // Actual column height: ELEV_H minus base boss engagement (8 mm) and platform seating (6 mm)
-    col_h = ELEV_H - 8.0 - PLAT_T;
+    rib_w = 3.0;
+    rib_h = 2.0;   // rib protrusion from column OD
 
     difference() {
         union() {
-            // Hollow tube
-            cylinder(d=COL_OD, h=col_h);
+            // ── Hollow cylinder ───────────────────────────────────────────
+            cylinder(d = COL_OD, h = 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);
+            // ── 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]);
         }
 
-        // Inner cable bore
-        translate([0, 0, -6.0 - EPS])
-            cylinder(d=COL_ID, h=col_h + 12.0 + 2*EPS);
+        // ── Central cable bore (full length) ─────────────────────────────
+        translate([0, 0, -e])
+            cylinder(d = COL_ID, h = ELEV_H + COL_SOCKET_L + 2*e);
 
-        // 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 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 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]);
+        // ── 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 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);
-            }
+        // ── 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]);
+        }
     }
 }
 
-// ── Part 3: scan_platform ───────────────────────────────────
+// ============================================================
+// 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.
 module scan_platform() {
     difference() {
         union() {
-            // Main disc
-            cylinder(d=PLAT_D, h=PLAT_T);
+            // ── Platform disc ─────────────────────────────────────────────
+            cylinder(d = PLAT_OD, h = PLAT_T);
 
-            // 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 boss (underside, -Z) ────────────────────────
+            translate([0, 0, -PLAT_SOCKET_L])
+                cylinder(d = COL_SOCKET_D, h = PLAT_SOCKET_L + e);
         }
 
-        // Central cable pass-through
-        translate([0, 0, -EPS])
-            cylinder(d=RPL_BORE_D, h=PLAT_T + 4.0);
+        // ── 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);
 
-        // Column spigot socket (bottom recess)
-        translate([0, 0, -EPS])
-            cylinder(d=COL_OD - 0.4 + 0.4, h=6.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);
 
-        // RPLIDAR M3 mounting holes — 4× on Ø40 BC at 45°/135°/225°/315°
+        // ── 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°) ────────
         for (a = [45, 135, 225, 315])
-            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);
-                }
+            translate([RPL_BC_D/2 * cos(a),
+                       RPL_BC_D/2 * sin(a), -e])
+                cylinder(d = RPL_BOLT_D, h = PLAT_T + 2*e);
 
-        // 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]);
+        // ── 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);
 
-        // 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]);
+        // ── 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);
     }
 }
 
-// ── Part 4: vibe_ring ───────────────────────────────────────
-// Printed silicone-grommet retainer ring — press-fits over M3 bolt with grommet seated
+// ============================================================
+// 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.
 module vibe_ring() {
     difference() {
-        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);
+        // ── 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);
     }
 }
 
-// ── Part 5: cable_guide ─────────────────────────────────────
-// Snap-on cable clip for column mid-section
+// ============================================================
+// 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.
 module cable_guide() {
-    arm_t = SNAP_T;
-    gap   = CLIP_GAP;
+    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;
 
     difference() {
         union() {
-            // Saddle body (U-shape wrapping column)
-            difference() {
-                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]);
-            }
+            // ── Clip body (flat plate on column face) ─────────────────────
+            translate([-GUIDE_BODY_W/2, 0, 0])
+                cube([GUIDE_BODY_W, 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]);
+            // ── Cable channel (C-shape, opens toward +Y) ──────────────────
+            translate([0, GUIDE_T + snap_oc/2, body_h/2])
+                rotate([0, 90, 0])
+                    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);
+                    }
 
-            // 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]);
+            // ── 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 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 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]);
+        // ── 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);
     }
 }
-
-// ── 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,8 +27,6 @@
  *   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
  * ──────────────────────────────────────
@@ -190,116 +188,6 @@ 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];
@@ -324,23 +212,6 @@ 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,7 +3,6 @@
 
 #include <stdint.h>
 #include "mpu6000.h"
-#include "slope_estimator.h"
 
 /*
  * SaltyLab Balance Controller
@@ -37,9 +36,6 @@ 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,14 +170,6 @@ 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)) {
@@ -192,6 +184,14 @@ 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

@@ -1,101 +0,0 @@
-#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

@@ -1,315 +0,0 @@
-# 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

@@ -1,168 +0,0 @@
-"""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

@@ -1,42 +0,0 @@
-<?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

@@ -1,218 +0,0 @@
-"""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

@@ -1,349 +0,0 @@
-"""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

@@ -1,176 +0,0 @@
-"""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

@@ -1,4 +0,0 @@
-[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

@@ -1,35 +0,0 @@
-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

@@ -1,459 +0,0 @@
-"""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

@@ -1,69 +0,0 @@
-#!/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,7 +15,6 @@
   <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

@@ -1,485 +0,0 @@
-#!/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,7 +29,6 @@ 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

@@ -1,30 +0,0 @@
-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

@@ -1,29 +0,0 @@
-"""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

@@ -1,35 +0,0 @@
-<?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

@@ -1,377 +0,0 @@
-"""
-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

@@ -1,396 +0,0 @@
-"""
-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

@@ -1,5 +0,0 @@
-[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

@@ -1,30 +0,0 @@
-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

@@ -1,134 +0,0 @@
-"""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

@@ -1,17 +0,0 @@
-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

@@ -1,32 +0,0 @@
-"""
-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

@@ -1,29 +0,0 @@
-<?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

@@ -1,272 +0,0 @@
-"""
-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

@@ -1,140 +0,0 @@
-"""
-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

@@ -1,4 +0,0 @@
-[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

@@ -1,29 +0,0 @@
-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

@@ -1,142 +0,0 @@
-"""
-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

@@ -1,12 +0,0 @@
-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

@@ -1,23 +0,0 @@
-<?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

@@ -1,29 +0,0 @@
-# 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,5 +1,4 @@
 #include "balance.h"
-#include "slope_estimator.h"
 #include "config.h"
 #include <math.h>
 
@@ -20,9 +19,6 @@ 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) {
@@ -32,16 +28,12 @@ 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;
         }
     }
@@ -53,9 +45,8 @@ void balance_update(balance_t *b, const IMUData *imu, float dt) {
         return;
     }
 
-    /* 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;
+    /* PID */
+    float error = b->setpoint - b->pitch_deg;
 
     /* Proportional */
     float p_term = b->kp * error;
@@ -94,5 +85,4 @@ 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,30 +545,6 @@ 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

@@ -1,82 +0,0 @@
-/*
- * 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

@@ -1,421 +0,0 @@
-/*
- * 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

@@ -1,314 +0,0 @@
-/* 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

@@ -1,197 +0,0 @@
-<!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

@@ -1,548 +0,0 @@
-/* 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();
-}