diff --git a/chassis/uwb_anchor_mount.scad b/chassis/uwb_anchor_mount.scad index 6c77443..8213f04 100644 --- a/chassis/uwb_anchor_mount.scad +++ b/chassis/uwb_anchor_mount.scad @@ -1,275 +1,463 @@ // ============================================================ -// uwb_anchor_mount.scad — Stem-Mounted UWB Anchor Rev A -// Agent: sl-mechanical 2026-03-01 -// Closes issues #57, #62 +// uwb_anchor_mount.scad — Wall/Ceiling UWB Anchor Mount Bracket +// Issue: #564 Agent: sl-mechanical Date: 2026-03-14 +// (supersedes Rev A stem-collar mount — see git history) // ============================================================ -// Clamp-on bracket for 2× MaUWB ESP32-S3 anchor modules on -// SaltyBot 25 mm OD vertical stem. -// Anchors spaced ANCHOR_SPACING = 250 mm apart. // -// Features: -// • Split D-collar with M4 clamping bolts + M4 set screw -// • Anti-rotation flat tab that keys against a small pin -// OR printed key tab that registers on the stem flat (if stem -// has a ground flat) — see ANTI_ROT_MODE parameter -// • Module bracket: faces outward, tilted 10° from vertical -// so antenna clears stem and faces horizon -// • USB cable channel (power from Orin via USB-A) on collar -// • Tool-free capture: M4 thumbscrews (slot-head, hand-tighten) -// • UWB antenna area: NO material within 10 mm of PCB top face +// Parametric wall or ceiling mount bracket for ESP32 UWB Pro anchor. +// Designed for fixed-infrastructure deployment: anchors screw into +// wall or ceiling drywall/timber with standard M4 or #6 wood screws, +// at a user-defined tilt angle so the UWB antenna faces the desired +// coverage zone. // -// Components per mount: -// 2× collar_half print in PLA/PETG, flat-face-down -// 1× module_bracket print in PLA/PETG, flat-face-down +// Architecture: +// Wall base → flat backplate with 2× screw holes (wall or ceiling) +// Tilt knuckle → single-axis articulating joint; 15° detent steps +// locked with M3 nyloc bolt; range 0–90° +// Anchor cradle→ U-cradle holding ESP32 UWB Pro PCB on M2.5 standoffs +// USB-C channel→ routed groove on tilt arm + exit slot in cradle back wall +// Label slot → rear window slot for printed anchor-ID card strip +// +// Part catalogue: +// Part 1 — wall_base() Backplate + 2-ear pivot block + detent arc +// Part 2 — tilt_arm() Pivoting arm with knuckle + cradle stub +// Part 3 — anchor_cradle() PCB cradle, standoffs, USB-C slot, label window +// Part 4 — cable_clip() Snap-on USB-C cable guide for tilt arm +// Part 5 — assembly_preview() +// +// Hardware BOM: +// 2× M4 × 30 mm wood screws (or #6 drywall screws) wall fasteners +// 1× M3 × 20 mm SHCS + M3 nyloc nut tilt pivot bolt +// 4× M2.5 × 8 mm SHCS PCB-to-cradle +// 4× M2.5 hex nuts captured in standoffs +// 1× USB-C cable anchor power +// +// ESP32 UWB Pro interface (verify with calipers): +// PCB size : UWB_L × UWB_W × UWB_H (55 × 28 × 10 mm default) +// Mounting holes : M2.5, 4× corners on UWB_HOLE_X × UWB_HOLE_Y pattern +// USB-C port : centred on short edge, UWB_USBC_W × UWB_USBC_H +// Antenna area : top face rear half — 10 mm keep-out of bracket material +// +// Tilt angles (15° detent steps, set TILT_DEG before export): +// 0° → horizontal face-up (ceiling, antenna faces down) +// 30° → 30° downward (wall near ceiling) [default] +// 45° → diagonal (wall mid-height) +// 90° → vertical face-out (wall, antenna faces forward) // // RENDER options: -// "assembly" single mount assembled (default) -// "collar_front" front collar half for slicing (×2 per mount × 2 mounts = 4) -// "collar_rear" rear collar half -// "bracket" module bracket (×2 mounts) -// "pair" both mounts on 350 mm stem section +// "assembly" full assembly at TILT_DEG (default) +// "wall_base_stl" Part 1 +// "tilt_arm_stl" Part 2 +// "anchor_cradle_stl" Part 3 +// "cable_clip_stl" Part 4 +// +// Export commands: +// openscad uwb_anchor_mount.scad -D 'RENDER="wall_base_stl"' -o uwb_wall_base.stl +// openscad uwb_anchor_mount.scad -D 'RENDER="tilt_arm_stl"' -o uwb_tilt_arm.stl +// openscad uwb_anchor_mount.scad -D 'RENDER="anchor_cradle_stl"' -o uwb_anchor_cradle.stl +// openscad uwb_anchor_mount.scad -D 'RENDER="cable_clip_stl"' -o uwb_cable_clip.stl // ============================================================ +$fn = 64; +e = 0.01; + +// ── Tilt angle (override per anchor, 0–90°, 15° steps) ─────────────────────── +TILT_DEG = 30; + +// ── ESP32 UWB Pro PCB dimensions (verify with calipers) ────────────────────── +UWB_L = 55.0; // PCB length +UWB_W = 28.0; // PCB width +UWB_H = 10.0; // PCB + components height +UWB_HOLE_X = 47.5; // M2.5 hole X span +UWB_HOLE_Y = 21.0; // M2.5 hole Y span +UWB_USBC_W = 9.5; // USB-C receptacle width +UWB_USBC_H = 4.0; // USB-C receptacle height +UWB_ANTENNA_L = 20.0; // antenna area at PCB rear (keep-out) + +// ── Wall base geometry ──────────────────────────────────────────────────────── +BASE_W = 60.0; +BASE_H = 50.0; +BASE_T = 5.0; +BASE_SCREW_D = 4.5; // M4 clearance +BASE_SCREW_HD = 8.5; // countersink OD +BASE_SCREW_HH = 3.5; // countersink depth +BASE_SCREW_SPC = 35.0; // Z span between screw holes +KNUCKLE_T = BASE_T + 4.0; // pivot ear depth (Y) + +// ── Tilt arm geometry ───────────────────────────────────────────────────────── +ARM_W = 12.0; +ARM_T = 5.0; +ARM_L = 35.0; +PIVOT_D = 3.3; // M3 clearance +PIVOT_NUT_AF = 5.5; +PIVOT_NUT_H = 2.4; +DETENT_D = 3.2; // detent notch diameter +DETENT_R = 8.0; // detent notch radius from pivot + +// ── Anchor cradle geometry ──────────────────────────────────────────────────── +CRADLE_WALL_T = 3.5; +CRADLE_BACK_T = 4.0; +CRADLE_FLOOR_T = 3.0; +CRADLE_LIP_H = 4.0; +CRADLE_LIP_T = 2.5; +STANDOFF_H = 3.0; +STANDOFF_OD = 5.5; +LABEL_W = UWB_L - 4.0; +LABEL_H = UWB_W * 0.55; +LABEL_T = 1.2; // label card thickness + +// ── USB-C cable routing ─────────────────────────────────────────────────────── +USBC_CHAN_W = 11.0; +USBC_CHAN_H = 7.0; + +// ── Cable clip ──────────────────────────────────────────────────────────────── +CLIP_CABLE_D = 4.5; +CLIP_T = 2.0; +CLIP_BODY_W = 16.0; +CLIP_BODY_H = 10.0; + +// ── Fasteners ───────────────────────────────────────────────────────────────── +M2P5_D = 2.7; +M3_D = 3.3; +M3_NUT_AF = 5.5; +M3_NUT_H = 2.4; + +// ============================================================ +// RENDER DISPATCH +// ============================================================ RENDER = "assembly"; -// ── ⚠ Verify with calipers ─────────────────────────────────── -MAWB_L = 50.0; // PCB length -MAWB_W = 25.0; // PCB width -MAWB_H = 10.0; // PCB + components -MAWB_HOLE_X = 43.0; // M2 mounting hole X span -MAWB_HOLE_Y = 20.0; // M2 mounting hole Y span -M2_D = 2.2; // M2 clearance +if (RENDER == "assembly") assembly_preview(); +else if (RENDER == "wall_base_stl") wall_base(); +else if (RENDER == "tilt_arm_stl") tilt_arm(); +else if (RENDER == "anchor_cradle_stl") anchor_cradle(); +else if (RENDER == "cable_clip_stl") cable_clip(); -// ── Stem ───────────────────────────────────────────────────── -STEM_OD = 25.0; -STEM_BORE = 25.4; // +0.4 clearance -WALL = 2.0; // wall thickness (used in thumbscrew recess) +// ============================================================ +// ASSEMBLY PREVIEW +// ============================================================ +module assembly_preview() { + // Ghost wall surface + %color("Wheat", 0.22) + translate([-BASE_W/2, -10, -BASE_H/2]) + cube([BASE_W, 10, BASE_H + 40]); -// ── Collar ─────────────────────────────────────────────────── -COL_OD = 52.0; -COL_H = 30.0; // taller than sensor-head collar for rigidity -COL_BOLT_X = 19.0; // M4 bolt CL from stem axis -COL_BOLT_D = 4.5; // M4 clearance -THUMB_HEAD_D= 8.0; // M4 thumbscrew head OD (slot for access) -COL_NUT_W = 7.0; // M4 hex nut A/F -COL_NUT_H = 3.4; + // Wall base + color("OliveDrab", 0.85) + wall_base(); -// Anti-rotation flat tab: a 3 mm wall tab that protrudes radially -// and bears against the bracket arm, preventing axial rotation -// without needing a stem flat. -ANTI_ROT_T = 3.0; // tab thickness (radial) -ANTI_ROT_W = 8.0; // tab width (tangential) -ANTI_ROT_Z = 4.0; // distance from collar base + // Tilt arm at TILT_DEG, pivoting at knuckle + color("SteelBlue", 0.85) + translate([0, KNUCKLE_T, 0]) + rotate([TILT_DEG, 0, 0]) + tilt_arm(); -// USB cable channel: groove on collar outer surface, runs Z direction -// Cable routes from anchor module down to base -USB_CHAN_W = 9.0; // channel width (fits USB-A cable Ø6 mm) -USB_CHAN_D = 5.0; // channel depth + // Anchor cradle at arm end + color("DarkSlateGray", 0.85) + translate([0, KNUCKLE_T, 0]) + rotate([TILT_DEG, 0, 0]) + translate([0, ARM_T, ARM_L]) + anchor_cradle(); -// ── Module bracket ─────────────────────────────────────────── -ARM_L = 20.0; // arm length from collar OD to bracket face -ARM_W = MAWB_W + 6.0; // bracket width (Y, includes side walls) -ARM_H = 6.0; // arm thickness (Z) -BRKT_TILT = 10.0; // tilt outward from vertical (antenna faces horizon) + // PCB ghost + %color("ForestGreen", 0.38) + translate([0, KNUCKLE_T, 0]) + rotate([TILT_DEG, 0, 0]) + translate([-UWB_L/2, + ARM_T + CRADLE_BACK_T, + ARM_L + CRADLE_FLOOR_T + STANDOFF_H]) + cube([UWB_L, UWB_W, UWB_H]); -BRKT_BACK_T = 3.0; // bracket back wall (module sits against this) -BRKT_SIDE_T = 2.0; // bracket side walls + // Cable clip at arm mid-point + color("DimGray", 0.70) + translate([ARM_W/2, KNUCKLE_T, 0]) + rotate([TILT_DEG, 0, 0]) + translate([0, ARM_T + e, ARM_L/2]) + rotate([0, -90, 90]) + cable_clip(); +} -M2_STNDFF = 3.0; // M2 standoff height -M2_STNDFF_OD= 4.5; - -// USB port access notch in bracket side wall (8×5 mm) -USB_NOTCH_W = 10.0; -USB_NOTCH_H = 7.0; - -// ── Spacing ─────────────────────────────────────────────────── -ANCHOR_SPACING = 250.0; // centre-to-centre Z separation - -$fn = 64; -e = 0.01; - -// ───────────────────────────────────────────────────────────── -// collar_half(side) -// split at Y=0 plane. Bracket arm on front (+Y) half. -// Print flat-face-down. -// ───────────────────────────────────────────────────────────── -module collar_half(side = "front") { - y_front = (side == "front"); +// ============================================================ +// PART 1 — WALL BASE +// ============================================================ +// Flat backplate screws to wall or ceiling with 2× countersunk +// M4/#6 wood screws on BASE_SCREW_SPC (35 mm) centres. +// Two pivot ears straddle the tilt arm; M3 pivot bolt passes through +// both ears and arm knuckle. +// Detent arc on inner face of +X ear: 7 notches at 15° steps (0–90°) +// so tilt can be set without a protractor. +// Shallow rear recess accepts a printed installation-zone label. +// +// Dual-use: flat face to wall (vertical screw axis) or flat face +// to ceiling (horizontal screw axis) — same part either way. +// +// Print: backplate flat on bed, PETG, 5 perims, 40 % gyroid. +module wall_base() { + ear_h = ARM_W + 3.0; + ear_t = 6.0; + ear_sep = ARM_W + 1.0; difference() { union() { - // D-shaped body - intersection() { - cylinder(d=COL_OD, h=COL_H); - translate([-COL_OD/2, y_front ? 0 : -COL_OD/2, 0]) - cube([COL_OD, COL_OD/2, COL_H]); - } + // ── Backplate ──────────────────────────────────────────────── + translate([-BASE_W/2, -BASE_T, -BASE_H/2]) + cube([BASE_W, BASE_T, BASE_H]); - // Anti-rotation tab (front half only, at +X side) - if (y_front) { - translate([COL_OD/2, -ANTI_ROT_W/2, ANTI_ROT_Z]) - cube([ANTI_ROT_T, ANTI_ROT_W, - COL_H - ANTI_ROT_Z - 4]); - } + // ── Two pivot ears ──────────────────────────────────────────── + for (ex = [-(ear_sep/2 + ear_t), ear_sep/2]) + translate([ex, -BASE_T + e, -ear_h/2]) + cube([ear_t, KNUCKLE_T + e, ear_h]); - // Bracket arm attachment boss (front half only, top centre) - if (y_front) { - translate([-ARM_W/2, COL_OD/2, COL_H * 0.3]) - cube([ARM_W, ARM_L, COL_H * 0.4]); - } + // ── Stiffening gussets ──────────────────────────────────────── + for (ex = [-(ear_sep/2 + ear_t), ear_sep/2]) + hull() { + translate([ex, -BASE_T, -ear_h/4]) + cube([ear_t, BASE_T - 1, ear_h/2]); + translate([ex + (ex < 0 ? ear_t*0.6 : 0), + -BASE_T, -ear_h/6]) + cube([ear_t * 0.4, 1, ear_h/3]); + } } - // ── Stem bore ───────────────────────────────────────── - translate([0,0,-e]) - cylinder(d=STEM_BORE, h=COL_H + 2*e); - - // ── M4 clamping bolt holes (Y direction) ────────────── - for (bx=[-COL_BOLT_X, COL_BOLT_X]) { - translate([bx, y_front ? COL_OD/2 : 0, COL_H/2]) - rotate([90,0,0]) - cylinder(d=COL_BOLT_D, h=COL_OD/2 + e); - // Thumbscrew head recess on outer face (front only — access side) - if (y_front) { - translate([bx, COL_OD/2 - WALL, COL_H/2]) - rotate([90,0,0]) - cylinder(d=THUMB_HEAD_D, h=8 + e); - } + // ── 2× countersunk wall screws ──────────────────────────────────── + for (sz = [-BASE_SCREW_SPC/2, BASE_SCREW_SPC/2]) { + translate([0, -BASE_T - e, sz]) + rotate([-90, 0, 0]) + cylinder(d = BASE_SCREW_D, h = BASE_T + 2*e); + translate([0, -BASE_T - e, sz]) + rotate([-90, 0, 0]) + cylinder(d1 = BASE_SCREW_HD, d2 = BASE_SCREW_D, + h = BASE_SCREW_HH + e); } - // ── M4 hex nut pockets (rear half) ──────────────────── - if (!y_front) { - for (bx=[-COL_BOLT_X, COL_BOLT_X]) { - translate([bx, -(COL_OD/4 + e), COL_H/2]) - rotate([90,0,0]) - cylinder(d=COL_NUT_W/cos(30), h=COL_NUT_H + e, - $fn=6); - } - } + // ── Pivot bolt bore (M3, through both ears) ─────────────────────── + translate([-(ear_sep/2 + ear_t + e), KNUCKLE_T * 0.55, 0]) + rotate([0, 90, 0]) + cylinder(d = PIVOT_D, h = ear_sep + 2*ear_t + 2*e); - // ── Set screw (height lock, front half) ─────────────── - if (y_front) { - translate([0, COL_OD/2, COL_H * 0.8]) - rotate([90,0,0]) - cylinder(d=COL_BOLT_D, - h=COL_OD/2 - STEM_BORE/2 + e); - } + // ── M3 nyloc nut pocket (outer face of one ear) ─────────────────── + translate([ear_sep/2 + ear_t - PIVOT_NUT_H - 0.4, + KNUCKLE_T * 0.55, 0]) + rotate([0, 90, 0]) + cylinder(d = PIVOT_NUT_AF / cos(30), + h = PIVOT_NUT_H + 0.5, $fn = 6); - // ── USB cable routing channel (rear half, −X side) ──── - if (!y_front) { - translate([-COL_OD/2, -USB_CHAN_W/2, -e]) - cube([USB_CHAN_D, USB_CHAN_W, COL_H + 2*e]); - } + // ── Detent arc — 7 notches at 15° steps on +X ear inner face ───── + for (da = [0 : 15 : 90]) + translate([ear_sep/2 - e, + KNUCKLE_T * 0.55 + DETENT_R * sin(da), + DETENT_R * cos(da)]) + rotate([0, 90, 0]) + cylinder(d = DETENT_D, h = ear_t * 0.45 + e); - // ── M4 hole through arm boss (Z direction, for bracket bolt) ─ - if (y_front) { - for (dx=[-ARM_W/4, ARM_W/4]) - translate([dx, COL_OD/2 + ARM_L/2, COL_H * 0.35]) - cylinder(d=COL_BOLT_D, h=COL_H * 0.35 + e); - } + // ── Installation label recess (rear face of backplate) ──────────── + translate([0, -BASE_T - e, 0]) + rotate([-90, 0, 0]) + cube([BASE_W - 12, BASE_H - 16, 1.6], center = true); + + // ── Lightening pocket ───────────────────────────────────────────── + translate([0, -BASE_T + 1.5, 0]) + cube([BASE_W - 14, BASE_T - 3, BASE_H - 20], center = true); } } -// ───────────────────────────────────────────────────────────── -// module_bracket() -// Bolts to collar arm boss. Holds MaUWB PCB facing outward. -// Tilted BRKT_TILT° from vertical — antenna clears stem. -// Print flat-face-down (back wall on bed). -// ───────────────────────────────────────────────────────────── -module module_bracket() { - bk = BRKT_BACK_T; - sd = BRKT_SIDE_T; +// ============================================================ +// PART 2 — TILT ARM +// ============================================================ +// Pivoting arm linking wall_base pivot ears to anchor_cradle. +// Knuckle end (Z=0): M3 pivot bore + spring-plunger detent pocket +// that indexes into the base ear detent arc notches. +// Cradle end (Z=ARM_L): 2× M3 bolt attachment to cradle back wall. +// USB-C cable channel runs along outer (+Y) face, full arm length. +// +// Print: knuckle face flat on bed, PETG, 5 perims, 40 % gyroid. +module tilt_arm() { + total_h = ARM_L + 10; difference() { union() { - // ── Back wall (mounts to collar arm boss) ───────── - cube([ARM_W, bk, MAWB_H + M2_STNDFF + 6]); + // ── Arm body ───────────────────────────────────────────────── + translate([-ARM_W/2, 0, 0]) + cube([ARM_W, ARM_T, total_h]); - // ── Side walls ──────────────────────────────────── - for (sx=[0, ARM_W - sd]) - translate([sx, bk, 0]) - cube([sd, MAWB_L + 2, MAWB_H + M2_STNDFF + 6]); + // ── Knuckle boss (rounded pivot end) ───────────────────────── + translate([0, ARM_T/2, 0]) + rotate([90, 0, 0]) + cylinder(d = ARM_W, h = ARM_T, center = true); - // ── M2 standoff posts (PCB mounts to these) ─────── - for (hx=[0, MAWB_HOLE_X], hy=[0, MAWB_HOLE_Y]) - translate([(ARM_W - MAWB_HOLE_X)/2 + hx, - bk + (MAWB_L - MAWB_HOLE_Y)/2 + hy, - 0]) - cylinder(d=M2_STNDFF_OD, h=M2_STNDFF); + // ── Cradle attach stub (Z = ARM_L) ──────────────────────────── + translate([-ARM_W/2, 0, ARM_L]) + cube([ARM_W, ARM_T + CRADLE_BACK_T, ARM_T]); } - // ── M2 bores through standoffs ──────────────────────── - for (hx=[0, MAWB_HOLE_X], hy=[0, MAWB_HOLE_Y]) - translate([(ARM_W - MAWB_HOLE_X)/2 + hx, - bk + (MAWB_L - MAWB_HOLE_Y)/2 + hy, - -e]) - cylinder(d=M2_D, h=M2_STNDFF + e); + // ── M3 pivot bore ───────────────────────────────────────────────── + translate([-ARM_W/2 - e, ARM_T/2, 0]) + rotate([0, 90, 0]) + cylinder(d = PIVOT_D, h = ARM_W + 2*e); - // ── Antenna clearance cutout in back wall ───────────── - // Open slot near top of back wall so antenna is unobstructed - translate([sd, -e, M2_STNDFF + 2]) - cube([ARM_W - 2*sd, bk + 2*e, MAWB_H]); + // ── Detent plunger pocket (3 mm spring-ball, outer +Y face) ────── + translate([0, ARM_T + e, 0]) + rotate([90, 0, 0]) + cylinder(d = 3.2, h = 4 + e); - // ── USB port access notch on one side wall ──────────── - translate([-e, bk + 2, M2_STNDFF - 1]) - cube([sd + 2*e, USB_NOTCH_W, USB_NOTCH_H]); + // ── USB-C cable channel (outer +Y face, mid-arm length) ─────────── + translate([-USBC_CHAN_W/2, ARM_T - e, ARM_T + 4]) + cube([USBC_CHAN_W, USBC_CHAN_H, ARM_L - ARM_T - 8]); - // ── Mounting holes to collar arm boss (×2) ──────────── - for (dx=[-ARM_W/4, ARM_W/4]) - translate([ARM_W/2 + dx, bk + ARM_L/2, -e]) - cylinder(d=COL_BOLT_D, h=6 + e); + // ── Cradle attach bolt holes (2× M3 at cradle stub) ─────────────── + for (bx = [-ARM_W/4, ARM_W/4]) + translate([bx, ARM_T/2, ARM_L + ARM_T/2]) + rotate([90, 0, 0]) + cylinder(d = M3_D, h = ARM_T + CRADLE_BACK_T + 2*e); + + // ── M3 nut pockets (front of cradle stub) ───────────────────────── + for (bx = [-ARM_W/4, ARM_W/4]) + translate([bx, ARM_T/2, ARM_L + ARM_T/2]) + rotate([-90, 0, 0]) + cylinder(d = M3_NUT_AF / cos(30), + h = M3_NUT_H + 0.5, $fn = 6); + + // ── Lightening pocket ───────────────────────────────────────────── + translate([0, ARM_T/2, ARM_L/2]) + cube([ARM_W - 4, ARM_T - 2, ARM_L - 18], center = true); } } -// ───────────────────────────────────────────────────────────── -// single_anchor_assembly() -// ───────────────────────────────────────────────────────────── -module single_anchor_assembly(show_phantom=false) { - // Collar - color("SteelBlue", 0.9) collar_half("front"); - color("CornflowerBlue", 0.9) mirror([0,1,0]) collar_half("rear"); +// ============================================================ +// PART 3 — ANCHOR CRADLE +// ============================================================ +// Open-front U-cradle for ESP32 UWB Pro PCB. +// PCB retained on 4× M2.5 standoffs matching UWB_HOLE_X × UWB_HOLE_Y. +// Back wall features: +// • USB-C exit slot — aligns with PCB USB-C port +// • USB-C groove — cable routes from slot toward arm channel +// • Label card slot — insert printed strip for anchor ID +// • Antenna keep-out — back wall material removed above antenna area +// Front lip prevents PCB from sliding forward. +// Two attachment tabs bolt to tilt_arm cradle stub. +// +// Print: back wall flat on bed, PETG, 5 perims, 40 % gyroid. +module anchor_cradle() { + outer_l = UWB_L + 2*CRADLE_WALL_T; + outer_w = UWB_W + CRADLE_FLOOR_T; + pcb_z = CRADLE_FLOOR_T + STANDOFF_H; + total_z = pcb_z + UWB_H + 2; - // Bracket tilted BRKT_TILT° outward from top of arm boss - color("LightSteelBlue", 0.85) - translate([0, COL_OD/2 + ARM_L, COL_H * 0.3]) - rotate([BRKT_TILT, 0, 0]) - translate([-ARM_W/2, 0, 0]) - module_bracket(); + difference() { + union() { + // ── Cradle body ─────────────────────────────────────────────── + translate([-outer_l/2, 0, 0]) + cube([outer_l, outer_w, total_z]); - // Phantom UWB PCB - if (show_phantom) - color("ForestGreen", 0.4) - translate([-MAWB_L/2, - COL_OD/2 + ARM_L + BRKT_BACK_T, - COL_H * 0.3 + M2_STNDFF]) - cube([MAWB_L, MAWB_W, MAWB_H]); + // ── Front retaining lip ─────────────────────────────────────── + translate([-outer_l/2, outer_w - CRADLE_LIP_T, 0]) + cube([outer_l, CRADLE_LIP_T, CRADLE_LIP_H]); + + // ── Arm attachment tabs (behind back wall) ───────────────────── + for (tx = [-ARM_W/4, ARM_W/4]) + translate([tx - 4, -CRADLE_BACK_T, 0]) + cube([8, CRADLE_BACK_T + 1, total_z]); + } + + // ── PCB pocket ──────────────────────────────────────────────────── + translate([-UWB_L/2, 0, pcb_z]) + cube([UWB_L, UWB_W + 1, UWB_H + 4]); + + // ── USB-C exit slot (through back wall, aligned to PCB port) ───── + translate([0, -CRADLE_BACK_T - e, + pcb_z + UWB_H/2 - UWB_USBC_H/2]) + cube([UWB_USBC_W + 2, CRADLE_BACK_T + 2*e, UWB_USBC_H + 2], + center = [true, false, false]); + + // ── USB-C cable routing groove (outer back wall face) ───────────── + translate([0, -CRADLE_BACK_T - e, -e]) + cube([USBC_CHAN_W, USBC_CHAN_H, pcb_z + UWB_H/2 + USBC_CHAN_H], + center = [true, false, false]); + + // ── Label card slot (insert from below, rear face upper half) ───── + // Paper/laminate card strip identifying this anchor instance + translate([0, -CRADLE_BACK_T - e, pcb_z + UWB_H/2]) + cube([LABEL_W, LABEL_T + 0.3, LABEL_H], + center = [true, false, false]); + + // ── Antenna keep-out: remove back wall above antenna area ───────── + translate([0, -e, pcb_z + UWB_H - UWB_ANTENNA_L]) + cube([UWB_L - 4, CRADLE_BACK_T + 2*e, UWB_ANTENNA_L + 4], + center = [true, false, false]); + + // ── Arm bolt holes through attachment tabs ──────────────────────── + for (tx = [-ARM_W/4, ARM_W/4]) + translate([tx, ARM_T/2 - CRADLE_BACK_T, total_z/2]) + rotate([-90, 0, 0]) + cylinder(d = M3_D, h = ARM_T + CRADLE_BACK_T + 2*e); + + // ── Lightening slots in side walls ──────────────────────────────── + for (side_x = [-outer_l/2 - e, outer_l/2 - CRADLE_WALL_T - e]) + translate([side_x, 2, pcb_z + 2]) + cube([CRADLE_WALL_T + 2*e, UWB_W - 4, UWB_H - 4]); + } + + // ── M2.5 standoff bosses (positive, inside cradle floor) ────────────── + for (hx = [-UWB_HOLE_X/2, UWB_HOLE_X/2]) + for (hy = [(outer_w - UWB_W)/2 + (UWB_W - UWB_HOLE_Y)/2, + (outer_w - UWB_W)/2 + (UWB_W - UWB_HOLE_Y)/2 + UWB_HOLE_Y]) + difference() { + translate([hx, hy, CRADLE_FLOOR_T - e]) + cylinder(d = STANDOFF_OD, h = STANDOFF_H + e); + translate([hx, hy, CRADLE_FLOOR_T - 2*e]) + cylinder(d = M2P5_D, h = STANDOFF_H + 4); + } } -// ───────────────────────────────────────────────────────────── -// Render selector -// ───────────────────────────────────────────────────────────── -if (RENDER == "assembly") { - single_anchor_assembly(show_phantom=true); +// ============================================================ +// PART 4 — CABLE CLIP +// ============================================================ +// Snap-on C-clip retaining USB-C cable along tilt arm outer face. +// Presses onto ARM_T-wide arm with PETG snap tongues. +// Open-front cable channel for push-in cable insertion. +// Print ×2–3 per anchor, spaced 25 mm along arm. +// +// Print: clip-opening face down, PETG, 3 perims, 20 % infill. +module cable_clip() { + ch_r = CLIP_CABLE_D/2 + CLIP_T; + snap_t = 1.6; -} else if (RENDER == "collar_front") { - collar_half("front"); + difference() { + union() { + // ── Body plate ──────────────────────────────────────────────── + translate([-CLIP_BODY_W/2, 0, 0]) + cube([CLIP_BODY_W, CLIP_T, CLIP_BODY_H]); -} else if (RENDER == "collar_rear") { - collar_half("rear"); + // ── Cable channel (C-shape, opens toward +Y) ───────────────── + translate([0, CLIP_T + ch_r, CLIP_BODY_H/2]) + rotate([0, 90, 0]) + difference() { + cylinder(r = ch_r, h = CLIP_BODY_W, center = true); + cylinder(r = CLIP_CABLE_D/2, + h = CLIP_BODY_W + 2*e, center = true); + // open insertion slot + translate([0, ch_r + e, 0]) + cube([CLIP_CABLE_D * 0.85, + ch_r * 2 + 2*e, + CLIP_BODY_W + 2*e], center = true); + } -} else if (RENDER == "bracket") { - module_bracket(); + // ── Snap tongues (straddle arm, -Y side of body) ───────────── + for (tx = [-CLIP_BODY_W/2 + 1.5, + CLIP_BODY_W/2 - 1.5 - snap_t]) + translate([tx, -ARM_T - 1, 0]) + cube([snap_t, ARM_T + 1 + CLIP_T, CLIP_BODY_H]); -} else if (RENDER == "pair") { - // Both anchors at 250 mm spacing on a stem stub - color("Silver", 0.2) - translate([0, 0, -50]) - cylinder(d=STEM_OD, h=ANCHOR_SPACING + COL_H + 100); + // ── Snap barbs ──────────────────────────────────────────────── + for (tx = [-CLIP_BODY_W/2 + 1.5, + CLIP_BODY_W/2 - 1.5 - snap_t]) + translate([tx + snap_t/2, -ARM_T - 1, CLIP_BODY_H/2]) + rotate([0, 90, 0]) + cylinder(d = 2, h = snap_t, center = true); + } - // Lower anchor (Z = 0) - single_anchor_assembly(show_phantom=true); - - // Upper anchor (Z = ANCHOR_SPACING) - translate([0, 0, ANCHOR_SPACING]) - single_anchor_assembly(show_phantom=true); + // ── Arm slot (arm body passes between tongues) ───────────────────── + translate([0, -ARM_T - 1 - e, CLIP_BODY_H/2]) + cube([CLIP_BODY_W - 6, ARM_T + 2, CLIP_BODY_H - 4], center = true); + } } - diff --git a/esp32/uwb_anchor/platformio.ini b/esp32/uwb_anchor/platformio.ini new file mode 100644 index 0000000..9d88d5f --- /dev/null +++ b/esp32/uwb_anchor/platformio.ini @@ -0,0 +1,30 @@ +; SaltyBot UWB Anchor Firmware — Issue #544 +; Target: Makerfabs ESP32 UWB Pro (DW3000 chip) +; +; Library: Makerfabs MaUWB_DW3000 +; https://github.com/Makerfabs/MaUWB_DW3000 +; +; Flash: +; pio run -e anchor0 --target upload (port-side anchor) +; pio run -e anchor1 --target upload (starboard anchor) +; Monitor: +; pio device monitor -e anchor0 -b 115200 + +[common] +platform = espressif32 +board = esp32dev +framework = arduino +monitor_speed = 115200 +upload_speed = 921600 +lib_deps = + https://github.com/Makerfabs/MaUWB_DW3000.git +build_flags = + -DCORE_DEBUG_LEVEL=0 + +[env:anchor0] +extends = common +build_flags = ${common.build_flags} -DANCHOR_ID=0 + +[env:anchor1] +extends = common +build_flags = ${common.build_flags} -DANCHOR_ID=1 diff --git a/esp32/uwb_anchor/src/main.cpp b/esp32/uwb_anchor/src/main.cpp new file mode 100644 index 0000000..e022ee2 --- /dev/null +++ b/esp32/uwb_anchor/src/main.cpp @@ -0,0 +1,413 @@ +/* + * uwb_anchor — SaltyBot ESP32 UWB Pro anchor firmware (TWR responder) + * Issue #544 + * + * Hardware: Makerfabs ESP32 UWB Pro (DW3000 chip) + * + * Role + * ──── + * Anchor sits on SaltyBot body, USB-connected to Jetson Orin. + * Two anchors per robot (anchor-0 port side, anchor-1 starboard). + * Person-worn tags initiate ranging; anchors respond. + * + * Protocol: Double-Sided TWR (DS-TWR) + * ──────────────────────────────────── + * Tag → Anchor POLL (msg_type 0x01) + * Anchor → Tag RESP (msg_type 0x02, payload: T_poll_rx, T_resp_tx) + * Tag → Anchor FINAL (msg_type 0x03, payload: Ra, Da, Db timestamps) + * Anchor computes range via DS-TWR formula, emits +RANGE on Serial. + * + * Serial output (115200 8N1, USB-CDC to Jetson) + * ────────────────────────────────────────────── + * +RANGE:,,\r\n (on each successful range) + * + * AT commands (host → anchor) + * ─────────────────────────── + * AT+RANGE? → returns last buffered +RANGE line + * AT+RANGE_ADDR= → pair with specific tag (filter others) + * AT+RANGE_ADDR= → clear pairing (accept all tags) + * AT+ID? → returns +ID: + * + * Pin mapping — Makerfabs ESP32 UWB Pro + * ────────────────────────────────────── + * SPI SCK 18 SPI MISO 19 SPI MOSI 23 + * DW CS 21 DW RST 27 DW IRQ 34 + * + * Build + * ────── + * pio run -e anchor0 --target upload (port side) + * pio run -e anchor1 --target upload (starboard) + */ + +#include +#include +#include +#include "dw3000.h" // Makerfabs MaUWB_DW3000 library + +/* ── Configurable ───────────────────────────────────────────────── */ + +#ifndef ANCHOR_ID +# define ANCHOR_ID 0 /* 0 = port, 1 = starboard */ +#endif + +#define SERIAL_BAUD 115200 + +/* ── Pin map (Makerfabs ESP32 UWB Pro) ─────────────────────────── */ + +#define PIN_SCK 18 +#define PIN_MISO 19 +#define PIN_MOSI 23 +#define PIN_CS 21 +#define PIN_RST 27 +#define PIN_IRQ 34 + +/* ── DW3000 channel / PHY config ───────────────────────────────── */ + +static dwt_config_t dw_cfg = { + 5, /* channel 5 (6.5 GHz, best penetration) */ + DWT_PLEN_128, /* preamble length */ + DWT_PAC8, /* PAC size */ + 9, /* TX preamble code */ + 9, /* RX preamble code */ + 1, /* SFD type (IEEE 802.15.4z) */ + DWT_BR_6M8, /* data rate 6.8 Mbps */ + DWT_PHR_MODE_STD, /* standard PHR */ + DWT_PHR_RATE_DATA, + (129 + 8 - 8), /* SFD timeout */ + DWT_STS_MODE_OFF, /* STS off — standard TWR */ + DWT_STS_LEN_64, + DWT_PDOA_M0, /* no PDoA */ +}; + +/* ── Frame format ──────────────────────────────────────────────── */ + +/* Byte layout for all frames: + * [0] frame_type (FTYPE_*) + * [1] src_id (tag 8-bit addr, or ANCHOR_ID) + * [2] dst_id + * [3..] payload + * (FCS appended automatically by DW3000 — 2 bytes) + */ + +#define FTYPE_POLL 0x01 +#define FTYPE_RESP 0x02 +#define FTYPE_FINAL 0x03 + +#define FRAME_HDR 3 +#define FCS_LEN 2 + +/* RESP payload: T_poll_rx(5 B) + T_resp_tx(5 B) */ +#define RESP_PAYLOAD 10 +#define RESP_FRAME_LEN (FRAME_HDR + RESP_PAYLOAD + FCS_LEN) + +/* FINAL payload: Ra(5 B) + Da(5 B) + Db(5 B) */ +#define FINAL_PAYLOAD 15 +#define FINAL_FRAME_LEN (FRAME_HDR + FINAL_PAYLOAD + FCS_LEN) + +/* ── Timing ────────────────────────────────────────────────────── */ + +/* Turnaround delay: anchor waits 500 µs after poll_rx before tx_resp. + * DW3000 tick = 1/(128×499.2e6) ≈ 15.65 ps → 500 µs = ~31.95M ticks. + * Stored as uint32 shifted right 8 bits for dwt_setdelayedtrxtime. */ +#define RESP_TX_DLY_US 500UL +#define DWT_TICKS_PER_US 63898UL /* 1µs in DW3000 ticks (×8 prescaler) */ +#define RESP_TX_DLY_TICKS (RESP_TX_DLY_US * DWT_TICKS_PER_US) + +/* How long anchor listens for FINAL after sending RESP */ +#define FINAL_RX_TIMEOUT_US 3000 + +/* Speed of light (m/s) */ +#define SPEED_OF_LIGHT 299702547.0 + +/* DW3000 40-bit timestamp mask */ +#define DWT_TS_MASK 0xFFFFFFFFFFULL + +/* Antenna delay (factory default; calibrate per unit for best accuracy) */ +#define ANT_DELAY 16385 + +/* ── Interrupt flags (set in ISR, polled in main) ──────────────── */ + +static volatile bool g_rx_ok = false; +static volatile bool g_tx_done = false; +static volatile bool g_rx_err = false; +static volatile bool g_rx_to = false; + +static uint8_t g_rx_buf[128]; +static uint32_t g_rx_len = 0; + +/* ── State ──────────────────────────────────────────────────────── */ + +/* Last successful range (serves AT+RANGE? queries) */ +static int32_t g_last_range_mm = -1; +static char g_last_range_line[72] = {}; + +/* Optional tag pairing: 0 = accept all tags */ +static uint8_t g_paired_tag_id = 0; + +/* ── DW3000 ISR callbacks ───────────────────────────────────────── */ + +static void cb_tx_done(const dwt_cb_data_t *) { g_tx_done = true; } + +static void cb_rx_ok(const dwt_cb_data_t *d) { + g_rx_len = d->datalength; + if (g_rx_len > sizeof(g_rx_buf)) g_rx_len = sizeof(g_rx_buf); + dwt_readrxdata(g_rx_buf, g_rx_len, 0); + g_rx_ok = true; +} + +static void cb_rx_err(const dwt_cb_data_t *) { g_rx_err = true; } +static void cb_rx_to(const dwt_cb_data_t *) { g_rx_to = true; } + +/* ── Timestamp helpers ──────────────────────────────────────────── */ + +static uint64_t ts_read(const uint8_t *p) { + uint64_t v = 0; + for (int i = 4; i >= 0; i--) v = (v << 8) | p[i]; + return v; +} + +static void ts_write(uint8_t *p, uint64_t v) { + for (int i = 0; i < 5; i++, v >>= 8) p[i] = (uint8_t)(v & 0xFF); +} + +static inline uint64_t ts_diff(uint64_t later, uint64_t earlier) { + return (later - earlier) & DWT_TS_MASK; +} + +static inline double ticks_to_s(uint64_t t) { + return (double)t / (128.0 * 499200000.0); +} + +/* Estimate receive power from CIR diagnostics (dBm) */ +static float rx_power_dbm(void) { + dwt_rxdiag_t d; + dwt_readdiagnostics(&d); + if (d.maxGrowthCIR == 0 || d.rxPreamCount == 0) return 0.0f; + float f = (float)d.maxGrowthCIR; + float n = (float)d.rxPreamCount; + return 10.0f * log10f((f * f) / (n * n)) - 121.74f; +} + +/* ── AT command handler ─────────────────────────────────────────── */ + +static char g_at_buf[64]; +static int g_at_idx = 0; + +static void at_dispatch(const char *cmd) { + if (strcmp(cmd, "AT+RANGE?") == 0) { + if (g_last_range_mm >= 0) + Serial.println(g_last_range_line); + else + Serial.println("+RANGE:NO_DATA"); + + } else if (strcmp(cmd, "AT+ID?") == 0) { + Serial.printf("+ID:%d\r\n", ANCHOR_ID); + + } else if (strncmp(cmd, "AT+RANGE_ADDR=", 14) == 0) { + const char *v = cmd + 14; + if (*v == '\0') { + g_paired_tag_id = 0; + Serial.println("+OK:UNPAIRED"); + } else { + g_paired_tag_id = (uint8_t)strtoul(v, nullptr, 0); + Serial.printf("+OK:PAIRED=0x%02X\r\n", g_paired_tag_id); + } + } else { + Serial.println("+ERR:UNKNOWN_CMD"); + } +} + +static void serial_poll(void) { + while (Serial.available()) { + char c = (char)Serial.read(); + if (c == '\r') continue; + if (c == '\n') { + g_at_buf[g_at_idx] = '\0'; + if (g_at_idx > 0) at_dispatch(g_at_buf); + g_at_idx = 0; + } else if (g_at_idx < (int)(sizeof(g_at_buf) - 1)) { + g_at_buf[g_at_idx++] = c; + } + } +} + +/* ── DS-TWR anchor state machine ────────────────────────────────── */ + +/* + * DS-TWR responder (one shot): + * 1. Wait for POLL from tag + * 2. Delayed-TX RESP (carry T_poll_rx + scheduled T_resp_tx) + * 3. Wait for FINAL from tag (tag embeds Ra, Da, Db) + * 4. Compute: Rb = T_final_rx − T_resp_tx + * tof = (Ra·Rb − Da·Db) / (Ra+Rb+Da+Db) + * range_m = tof × c + * 5. Print +RANGE line + */ +static void twr_cycle(void) { + + /* --- 1. Listen for POLL --- */ + dwt_setrxtimeout(0); + dwt_rxenable(DWT_START_RX_IMMEDIATE); + + g_rx_ok = g_rx_err = false; + uint32_t deadline = millis() + 2000; + while (!g_rx_ok && !g_rx_err) { + serial_poll(); + if (millis() > deadline) { + /* restart RX if we've been stuck */ + dwt_rxenable(DWT_START_RX_IMMEDIATE); + deadline = millis() + 2000; + } + yield(); + } + if (!g_rx_ok || g_rx_len < FRAME_HDR) return; + + /* validate POLL */ + if (g_rx_buf[0] != FTYPE_POLL) return; + uint8_t tag_id = g_rx_buf[1]; + if (g_paired_tag_id != 0 && tag_id != g_paired_tag_id) return; + + /* --- 2. Record T_poll_rx --- */ + uint8_t poll_rx_raw[5]; + dwt_readrxtimestamp(poll_rx_raw); + uint64_t T_poll_rx = ts_read(poll_rx_raw); + + /* Compute delayed TX time: poll_rx + turnaround, aligned to 512-tick grid */ + uint64_t resp_tx_sched = (T_poll_rx + RESP_TX_DLY_TICKS) & ~0x1FFULL; + + /* Build RESP frame */ + uint8_t resp[RESP_FRAME_LEN]; + resp[0] = FTYPE_RESP; + resp[1] = ANCHOR_ID; + resp[2] = tag_id; + ts_write(&resp[3], T_poll_rx); /* T_poll_rx (tag uses this) */ + ts_write(&resp[8], resp_tx_sched); /* scheduled T_resp_tx */ + + dwt_writetxdata(RESP_FRAME_LEN - FCS_LEN, resp, 0); + dwt_writetxfctrl(RESP_FRAME_LEN, 0, 1 /*ranging*/); + dwt_setdelayedtrxtime((uint32_t)(resp_tx_sched >> 8)); + + /* Enable RX after TX to receive FINAL */ + dwt_setrxaftertxdelay(300); + dwt_setrxtimeout(FINAL_RX_TIMEOUT_US); + + /* Fire delayed TX */ + g_tx_done = g_rx_ok = g_rx_err = g_rx_to = false; + if (dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED) != DWT_SUCCESS) { + dwt_forcetrxoff(); + return; /* TX window missed — try next cycle */ + } + + /* Wait for TX done (short wait, ISR fires fast) */ + uint32_t t0 = millis(); + while (!g_tx_done && millis() - t0 < 15) { yield(); } + + /* Read actual T_resp_tx */ + uint8_t resp_tx_raw[5]; + dwt_readtxtimestamp(resp_tx_raw); + uint64_t T_resp_tx = ts_read(resp_tx_raw); + + /* --- 3. Wait for FINAL --- */ + t0 = millis(); + while (!g_rx_ok && !g_rx_err && !g_rx_to && millis() - t0 < 60) { + serial_poll(); + yield(); + } + if (!g_rx_ok || g_rx_len < FRAME_HDR + FINAL_PAYLOAD) return; + if (g_rx_buf[0] != FTYPE_FINAL) return; + if (g_rx_buf[1] != tag_id) return; + + /* Extract DS-TWR timestamps from FINAL payload */ + uint64_t Ra = ts_read(&g_rx_buf[3]); /* tag: T_resp_rx − T_poll_tx */ + uint64_t Da = ts_read(&g_rx_buf[8]); /* tag: T_final_tx − T_resp_rx */ + /* g_rx_buf[13..17] = Db from tag (cross-check, unused here) */ + + /* T_final_rx */ + uint8_t final_rx_raw[5]; + dwt_readrxtimestamp(final_rx_raw); + uint64_t T_final_rx = ts_read(final_rx_raw); + + /* --- 4. DS-TWR formula --- */ + uint64_t Rb = ts_diff(T_final_rx, T_resp_tx); /* anchor round-trip */ + uint64_t Db = ts_diff(T_resp_tx, T_poll_rx); /* anchor turnaround */ + + double ra = ticks_to_s(Ra), rb = ticks_to_s(Rb); + double da = ticks_to_s(Da), db = ticks_to_s(Db); + + double denom = ra + rb + da + db; + if (denom < 1e-15) return; + + double tof = (ra * rb - da * db) / denom; + double range_m = tof * SPEED_OF_LIGHT; + + /* Validity window: 0.1 m – 130 m */ + if (range_m < 0.1 || range_m > 130.0) return; + + int32_t range_mm = (int32_t)(range_m * 1000.0 + 0.5); + float rssi = rx_power_dbm(); + + /* --- 5. Emit +RANGE --- */ + snprintf(g_last_range_line, sizeof(g_last_range_line), + "+RANGE:%d,%ld,%.1f", ANCHOR_ID, (long)range_mm, rssi); + g_last_range_mm = range_mm; + Serial.println(g_last_range_line); +} + +/* ── Arduino setup ──────────────────────────────────────────────── */ + +void setup(void) { + Serial.begin(SERIAL_BAUD); + delay(300); + + Serial.printf("\r\n[uwb_anchor] anchor_id=%d starting\r\n", ANCHOR_ID); + + SPI.begin(PIN_SCK, PIN_MISO, PIN_MOSI, PIN_CS); + + /* Hardware reset */ + pinMode(PIN_RST, OUTPUT); + digitalWrite(PIN_RST, LOW); + delay(2); + pinMode(PIN_RST, INPUT_PULLUP); + delay(5); + + /* DW3000 probe + init (Makerfabs MaUWB_DW3000 library) */ + if (dwt_probe((struct dwt_probe_s *)&dw3000_probe_interf)) { + Serial.println("[uwb_anchor] FATAL: DW3000 probe failed — check SPI wiring"); + for (;;) delay(1000); + } + + if (dwt_initialise(DWT_DW_INIT) != DWT_SUCCESS) { + Serial.println("[uwb_anchor] FATAL: dwt_initialise failed"); + for (;;) delay(1000); + } + + if (dwt_configure(&dw_cfg) != DWT_SUCCESS) { + Serial.println("[uwb_anchor] FATAL: dwt_configure failed"); + for (;;) delay(1000); + } + + dwt_setrxantennadelay(ANT_DELAY); + dwt_settxantennadelay(ANT_DELAY); + dwt_settxpower(0x0E080222UL); /* max TX power for 120 m range */ + + dwt_setcallbacks(cb_tx_done, cb_rx_ok, cb_rx_to, cb_rx_err, + nullptr, nullptr, nullptr); + dwt_setinterrupt( + DWT_INT_TXFRS | DWT_INT_RFCG | DWT_INT_RFTO | + DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_ARFE | DWT_INT_CPERR, + 0, DWT_ENABLE_INT_ONLY); + + attachInterrupt(digitalPinToInterrupt(PIN_IRQ), + []() { dwt_isr(); }, RISING); + + Serial.printf("[uwb_anchor] DW3000 ready ch=%d 6.8Mbps id=%d\r\n", + dw_cfg.chan, ANCHOR_ID); + Serial.println("[uwb_anchor] Listening for tags..."); +} + +/* ── Arduino loop ───────────────────────────────────────────────── */ + +void loop(void) { + serial_poll(); + twr_cycle(); +} diff --git a/esp32/uwb_anchor/udev/99-uwb-anchors.rules b/esp32/uwb_anchor/udev/99-uwb-anchors.rules new file mode 100644 index 0000000..c850400 --- /dev/null +++ b/esp32/uwb_anchor/udev/99-uwb-anchors.rules @@ -0,0 +1,19 @@ +# SaltyBot UWB anchor USB-serial persistent symlinks +# Install: +# sudo cp 99-uwb-anchors.rules /etc/udev/rules.d/ +# sudo udevadm control --reload && sudo udevadm trigger +# +# Find serial numbers: +# udevadm info -a /dev/ttyUSB0 | grep ATTRS{serial} +# +# Fill ANCHOR0_SERIAL and ANCHOR1_SERIAL with the values found above. +# Anchor 0 = port side → /dev/uwb-anchor0 +# Anchor 1 = starboard → /dev/uwb-anchor1 + +SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \ + ATTRS{serial}=="ANCHOR0_SERIAL", \ + SYMLINK+="uwb-anchor0", MODE="0666" + +SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", \ + ATTRS{serial}=="ANCHOR1_SERIAL", \ + SYMLINK+="uwb-anchor1", MODE="0666" diff --git a/include/jlink.h b/include/jlink.h index c008754..7e2289e 100644 --- a/include/jlink.h +++ b/include/jlink.h @@ -3,9 +3,10 @@ #include #include +#include "pid_flash.h" /* pid_sched_entry_t, PID_SCHED_MAX_BANDS */ /* - * JLink — Jetson serial binary protocol over USART1 (PB6=TX, PB7=RX). + * JLink -- Jetson serial binary protocol over USART1 (PB6=TX, PB7=RX). * * Issue #120: replaces jetson_cmd ASCII-over-USB-CDC with a dedicated * hardware UART at 921600 baud using DMA circular RX and IDLE interrupt. @@ -28,14 +29,21 @@ * 0x05 PID_SET - float kp, float ki, float kd (12 bytes, IEEE-754 LE) * 0x06 DFU_ENTER - no payload; request OTA DFU reboot (denied while armed) * 0x07 ESTOP - no payload; engage emergency stop + * 0x08 AUDIO - int16 PCM samples (up to 126 samples) + * 0x09 SLEEP - no payload; request STOP-mode sleep * 0x0A PID_SAVE - no payload; save current Kp/Ki/Kd to flash (Issue #531) * 0x0B GIMBAL_POS - int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547) + * 0x0C SCHED_GET - no payload; reply with TLM_SCHED (Issue #550) + * 0x0D SCHED_SET - uint8 num_bands + N*16-byte pid_sched_entry_t (Issue #550) + * 0x0E SCHED_SAVE - float kp, ki, kd (12 bytes); save sched+single to flash (Issue #550) * * STM32 to Jetson telemetry: - * 0x80 STATUS - jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ - * 0x81 POWER - jlink_tlm_power_t (11 bytes), sent at PM_TLM_HZ + * 0x80 STATUS - jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ + * 0x81 POWER - jlink_tlm_power_t (11 bytes), sent at PM_TLM_HZ + * 0x82 BATTERY - jlink_tlm_battery_t (10 bytes, Issue #533) * 0x83 PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531) * 0x84 GIMBAL_STATE - jlink_tlm_gimbal_state_t (10 bytes, Issue #547) + * 0x85 SCHED - jlink_tlm_sched_t (1+N*16 bytes), sent on SCHED_GET (Issue #550) * * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied * when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and @@ -62,13 +70,17 @@ #define JLINK_CMD_SLEEP 0x09u /* no payload; request STOP-mode sleep */ #define JLINK_CMD_PID_SAVE 0x0Au /* no payload; save Kp/Ki/Kd to flash (Issue #531) */ #define JLINK_CMD_GIMBAL_POS 0x0Bu /* int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547) */ +#define JLINK_CMD_SCHED_GET 0x0Cu /* no payload; reply TLM_SCHED (Issue #550) */ +#define JLINK_CMD_SCHED_SET 0x0Du /* uint8 num_bands + N*16-byte entries (Issue #550) */ +#define JLINK_CMD_SCHED_SAVE 0x0Eu /* float kp,ki,kd; save sched+single to flash (Issue #550) */ /* ---- Telemetry IDs (STM32 to Jetson) ---- */ #define JLINK_TLM_STATUS 0x80u #define JLINK_TLM_POWER 0x81u /* jlink_tlm_power_t (11 bytes) */ #define JLINK_TLM_BATTERY 0x82u /* jlink_tlm_battery_t (10 bytes, Issue #533) */ -#define JLINK_TLM_PID_RESULT 0x83u /* jlink_tlm_pid_result_t (13 bytes) Issue #531 */ +#define JLINK_TLM_PID_RESULT 0x83u /* jlink_tlm_pid_result_t (13 bytes, Issue #531) */ #define JLINK_TLM_GIMBAL_STATE 0x84u /* jlink_tlm_gimbal_state_t (10 bytes, Issue #547) */ +#define JLINK_TLM_SCHED 0x85u /* jlink_tlm_sched_t (1+N*16 bytes, Issue #550) */ /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */ typedef struct __attribute__((packed)) { @@ -98,15 +110,15 @@ typedef struct __attribute__((packed)) { uint32_t idle_ms; /* ms since last cmd_vel activity */ } jlink_tlm_power_t; /* 11 bytes */ -/* ---- Telemetry BATTERY payload (10 bytes, packed) — Issue #533 ---- */ +/* ---- Telemetry BATTERY payload (10 bytes, packed) Issue #533 ---- */ typedef struct __attribute__((packed)) { - uint16_t vbat_mv; /* DMA-sampled LPF-filtered Vbat (mV) */ + uint16_t vbat_mv; /* DMA-sampled LPF-filtered Vbat (mV) */ int16_t ibat_ma; /* DMA-sampled LPF-filtered Ibat (mA, + = discharge) */ - uint16_t vbat_raw_mv; /* unfiltered last-tick average (mV) */ - uint8_t flags; /* bit0=low, bit1=critical, bit2=4S, bit3=adc_ready */ - int8_t cal_offset; /* vbat_offset_mv / 4 (±127 → ±508 mV) */ - uint8_t lpf_shift; /* IIR shift factor (α = 1/2^lpf_shift) */ - uint8_t soc_pct; /* voltage-based SoC 0–100, 255 = unknown */ + uint16_t vbat_raw_mv; /* unfiltered last-tick average (mV) */ + uint8_t flags; /* bit0=low, bit1=critical, bit2=4S, bit3=adc_ready */ + int8_t cal_offset; /* vbat_offset_mv / 4 (+-127 -> +-508 mV) */ + uint8_t lpf_shift; /* IIR shift factor (alpha = 1/2^lpf_shift) */ + uint8_t soc_pct; /* voltage-based SoC 0-100, 255 = unknown */ } jlink_tlm_battery_t; /* 10 bytes */ /* ---- Telemetry PID_RESULT payload (13 bytes, packed) Issue #531 ---- */ @@ -129,6 +141,13 @@ typedef struct __attribute__((packed)) { uint8_t rx_err_pct; /* bus error rate 0-100% (rx_err*100/(rx_ok+rx_err)) */ } jlink_tlm_gimbal_state_t; /* 10 bytes */ +/* ---- Telemetry SCHED payload (1 + N*16 bytes, packed) Issue #550 ---- */ +/* Sent in response to JLINK_CMD_SCHED_GET; N = num_bands (1..PID_SCHED_MAX_BANDS). */ +typedef struct __attribute__((packed)) { + uint8_t num_bands; /* number of valid entries */ + pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* up to 6 x 16 = 96 bytes */ +} jlink_tlm_sched_t; /* 1 + 96 = 97 bytes max */ + /* ---- Volatile state (read from main loop) ---- */ typedef struct { /* Drive command - updated on JLINK_CMD_DRIVE */ @@ -161,55 +180,37 @@ typedef struct { volatile int16_t gimbal_pan_x10; /* pan angle deg x10 */ volatile int16_t gimbal_tilt_x10; /* tilt angle deg x10 */ volatile uint16_t gimbal_speed; /* servo speed 0-4095 (0=max) */ + + /* PID schedule commands (Issue #550) - set by parser, cleared by main loop */ + volatile uint8_t sched_get_req; /* SCHED_GET: main loop calls jlink_send_sched_telemetry() */ + volatile uint8_t sched_save_req; /* SCHED_SAVE: main loop calls pid_schedule_flash_save() */ + volatile float sched_save_kp; /* kp for single-PID record in SCHED_SAVE */ + volatile float sched_save_ki; + volatile float sched_save_kd; } JLinkState; extern volatile JLinkState jlink_state; +/* ---- SCHED_SET receive buffer -- Issue #550 ---- */ +/* + * Populated by the parser on JLINK_CMD_SCHED_SET. Main loop reads via + * jlink_get_sched_set() and calls pid_schedule_set_table() before clearing. + */ +typedef struct { + volatile uint8_t ready; /* set by parser, cleared by main loop */ + volatile uint8_t num_bands; + pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* copied from frame */ +} JLinkSchedSetBuf; + /* ---- API ---- */ -/* - * jlink_init() - configure USART1 (PB6=TX, PB7=RX) at 921600 baud with - * DMA2_Stream2_Channel4 circular RX (128-byte buffer) and IDLE interrupt. - * Call once before safety_init(). - */ void jlink_init(void); - -/* - * jlink_is_active(now_ms) - returns true if a valid frame arrived within - * JLINK_HB_TIMEOUT_MS. Returns false if no frame ever received. - */ bool jlink_is_active(uint32_t now_ms); - -/* - * jlink_send_telemetry(status) - build and transmit a JLINK_TLM_STATUS frame - * over USART1 TX (blocking, ~0.2ms at 921600). Call at JLINK_TLM_HZ. - */ -void jlink_send_telemetry(const jlink_tlm_status_t *status); - -/* - * jlink_process() - drain DMA circular buffer and parse frames. - * Call from main loop every iteration (not ISR). Lightweight: O(bytes_pending). - */ void jlink_process(void); - -/* - * jlink_send_power_telemetry(power) - build and transmit a JLINK_TLM_POWER - * frame (17 bytes) at PM_TLM_HZ. Call from main loop when not in STOP mode. - */ +void jlink_send_telemetry(const jlink_tlm_status_t *status); void jlink_send_power_telemetry(const jlink_tlm_power_t *power); - -/* - * jlink_send_pid_result(result) - build and transmit a JLINK_TLM_PID_RESULT - * frame (19 bytes) to confirm PID flash save outcome (Issue #531). - */ -void jlink_send_pid_result(const jlink_tlm_pid_result_t *result); - -/* - * jlink_send_battery_telemetry(batt) - build and transmit JLINK_TLM_BATTERY - * (0x82) frame (16 bytes) at BATTERY_ADC_PUBLISH_HZ (1 Hz). - * Called by battery_adc_publish(); not normally called directly. - */ void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt); +void jlink_send_pid_result(const jlink_tlm_pid_result_t *result); /* * jlink_send_gimbal_state(state) - transmit JLINK_TLM_GIMBAL_STATE (0x84) @@ -217,4 +218,18 @@ void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt); */ void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state); +/* + * jlink_send_sched_telemetry(tlm) - transmit JLINK_TLM_SCHED (0x85) in + * response to SCHED_GET. tlm->num_bands determines actual frame size. + * Issue #550. + */ +void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm); + +/* + * jlink_get_sched_set() - return pointer to the most-recently received + * SCHED_SET payload buffer (static storage in jlink.c). Main loop calls + * pid_schedule_set_table() from this buffer, then clears ready. Issue #550. + */ +JLinkSchedSetBuf *jlink_get_sched_set(void); + #endif /* JLINK_H */ diff --git a/jetson/ros2_ws/src/saltybot_health_monitor/config/sensor_health_params.yaml b/jetson/ros2_ws/src/saltybot_health_monitor/config/sensor_health_params.yaml new file mode 100644 index 0000000..f203362 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_health_monitor/config/sensor_health_params.yaml @@ -0,0 +1,66 @@ +sensor_health_node: + ros__parameters: + # Publish rate for DiagnosticArray and JSON summary + check_hz: 1.0 + + # MQTT broker for saltybot/health topic + mqtt_broker: "localhost" + mqtt_port: 1883 + mqtt_topic: "saltybot/health" + mqtt_enabled: true + +# Per-sensor thresholds and configuration +# Each entry: topic, name, warn_s (WARN threshold), error_s (ERROR threshold), critical +# +# critical=true: system cannot operate without this sensor +# warn_s: topic age (s) that triggers WARN level +# error_s: topic age (s) that triggers ERROR level + +sensors: + - topic: "/camera/color/image_raw" + name: "camera_color" + warn_s: 1.0 + error_s: 3.0 + critical: true + + - topic: "/camera/depth/image_rect_raw" + name: "camera_depth" + warn_s: 1.0 + error_s: 3.0 + critical: true + + - topic: "/camera/color/camera_info" + name: "camera_info" + warn_s: 2.0 + error_s: 5.0 + critical: false + + - topic: "/scan" + name: "lidar" + warn_s: 1.0 + error_s: 3.0 + critical: true + + - topic: "/imu/data" + name: "imu" + warn_s: 0.5 + error_s: 2.0 + critical: true + + - topic: "/saltybot/uwb/range" + name: "uwb" + warn_s: 2.0 + error_s: 5.0 + critical: false + + - topic: "/saltybot/battery" + name: "battery" + warn_s: 3.0 + error_s: 8.0 + critical: false + + - topic: "/saltybot/motor_daemon/status" + name: "motor_daemon" + warn_s: 2.0 + error_s: 5.0 + critical: true diff --git a/jetson/ros2_ws/src/saltybot_health_monitor/launch/sensor_health.launch.py b/jetson/ros2_ws/src/saltybot_health_monitor/launch/sensor_health.launch.py new file mode 100644 index 0000000..c333907 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_health_monitor/launch/sensor_health.launch.py @@ -0,0 +1,50 @@ +"""sensor_health.launch.py — Launch sensor health monitor node (Issue #566).""" + +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() -> LaunchDescription: + pkg = get_package_share_directory("saltybot_health_monitor") + + check_hz_arg = DeclareLaunchArgument( + "check_hz", + default_value="1.0", + description="Health check publish rate (Hz)", + ) + mqtt_broker_arg = DeclareLaunchArgument( + "mqtt_broker", + default_value="localhost", + description="MQTT broker hostname", + ) + mqtt_enabled_arg = DeclareLaunchArgument( + "mqtt_enabled", + default_value="true", + description="Enable MQTT publishing to saltybot/health", + ) + + sensor_health_node = Node( + package="saltybot_health_monitor", + executable="sensor_health_node", + name="sensor_health_node", + output="screen", + parameters=[ + os.path.join(pkg, "config", "sensor_health_params.yaml"), + { + "check_hz": LaunchConfiguration("check_hz"), + "mqtt_broker": LaunchConfiguration("mqtt_broker"), + "mqtt_enabled": LaunchConfiguration("mqtt_enabled"), + }, + ], + ) + + return LaunchDescription([ + check_hz_arg, + mqtt_broker_arg, + mqtt_enabled_arg, + sensor_health_node, + ]) diff --git a/jetson/ros2_ws/src/saltybot_health_monitor/package.xml b/jetson/ros2_ws/src/saltybot_health_monitor/package.xml index b49a494..95dab9b 100644 --- a/jetson/ros2_ws/src/saltybot_health_monitor/package.xml +++ b/jetson/ros2_ws/src/saltybot_health_monitor/package.xml @@ -1,27 +1,24 @@ - saltybot_health_monitor - 0.1.0 + 0.2.0 - ROS2 system health monitor for SaltyBot. Central node that monitors heartbeats - from all critical nodes, detects when nodes go down (>5s silent), triggers - auto-restart, publishes /saltybot/system_health JSON, and alerts face display - on critical failures. + ROS2 health monitor for SaltyBot. Tracks node heartbeats and sensor topic + staleness. Publishes DiagnosticArray on /saltybot/diagnostics and MQTT on + saltybot/health. Issue #566. - sl-controls - MIT + sl-jetson + Apache-2.0 rclpy std_msgs geometry_msgs + sensor_msgs + diagnostic_msgs ament_python - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest + pytest ament_python diff --git a/jetson/ros2_ws/src/saltybot_health_monitor/saltybot_health_monitor/sensor_health_node.py b/jetson/ros2_ws/src/saltybot_health_monitor/saltybot_health_monitor/sensor_health_node.py new file mode 100644 index 0000000..960132d --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_health_monitor/saltybot_health_monitor/sensor_health_node.py @@ -0,0 +1,385 @@ +#!/usr/bin/env python3 +"""sensor_health_node.py — ROS2 sensor topic health monitor (Issue #566). + +Monitors all sensor topics for staleness. Each sensor is checked against +configurable WARN and ERROR timeouts. Results are published as a ROS2 +DiagnosticArray and as an MQTT JSON summary. + +Monitored topics (configurable via sensor_health_params.yaml): + /camera/color/image_raw — RealSense colour stream + /camera/depth/image_rect_raw — RealSense depth stream + /camera/color/camera_info — RealSense camera info + /scan — LiDAR 2-D scan + /imu/data — IMU (BNO055 via JLink) + /saltybot/uwb/range — UWB ranging + /saltybot/battery — Battery telemetry (JSON string) + /saltybot/motor_daemon/status — Motor daemon status + +Published topics: + /saltybot/diagnostics (diagnostic_msgs/DiagnosticArray) — full per-sensor status + /saltybot/sensor_health (std_msgs/String) — JSON summary + +MQTT: + Topic: saltybot/health + Payload: JSON {timestamp, overall, sensors:{name: {status, age_s, hz}}} + +Parameters (config/sensor_health_params.yaml): + check_hz 1.0 Health check publish rate + mqtt_broker localhost + mqtt_port 1883 + mqtt_topic saltybot/health + mqtt_enabled true + sensors list of {topic, name, warn_s, error_s, critical} +""" + +from __future__ import annotations + +import json +import time +import threading +from dataclasses import dataclass, field +from typing import Dict, List, Optional + +import rclpy +from rclpy.node import Node +from rclpy.qos import ( + DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +) + +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue +from sensor_msgs.msg import CameraInfo, Image, Imu, LaserScan +from std_msgs.msg import String + + +# ── Diagnostic level aliases ─────────────────────────────────────────────────── + +OK = DiagnosticStatus.OK # 0 +WARN = DiagnosticStatus.WARN # 1 +ERROR = DiagnosticStatus.ERROR # 2 + +_LEVEL_NAMES = {OK: "OK", WARN: "WARN", ERROR: "ERROR"} + +# ── Default sensor configuration ────────────────────────────────────────────── + +DEFAULT_SENSORS: List[dict] = [ + {"topic": "/camera/color/image_raw", "name": "camera_color", "warn_s": 1.0, "error_s": 3.0, "critical": True}, + {"topic": "/camera/depth/image_rect_raw", "name": "camera_depth", "warn_s": 1.0, "error_s": 3.0, "critical": True}, + {"topic": "/camera/color/camera_info", "name": "camera_info", "warn_s": 2.0, "error_s": 5.0, "critical": False}, + {"topic": "/scan", "name": "lidar", "warn_s": 1.0, "error_s": 3.0, "critical": True}, + {"topic": "/imu/data", "name": "imu", "warn_s": 0.5, "error_s": 2.0, "critical": True}, + {"topic": "/saltybot/uwb/range", "name": "uwb", "warn_s": 2.0, "error_s": 5.0, "critical": False}, + {"topic": "/saltybot/battery", "name": "battery", "warn_s": 3.0, "error_s": 8.0, "critical": False}, + {"topic": "/saltybot/motor_daemon/status", "name": "motor_daemon", "warn_s": 2.0, "error_s": 5.0, "critical": True}, +] + + +# ── SensorWatcher ───────────────────────────────────────────────────────────── + +class SensorWatcher: + """Tracks message receipt timestamps and rate for a single topic. + + Thread-safe: callback may fire from any executor thread. + """ + + def __init__(self, topic: str, name: str, + warn_s: float, error_s: float, critical: bool) -> None: + self.topic = topic + self.name = name + self.warn_s = warn_s + self.error_s = error_s + self.critical = critical + + self._last_rx: float = 0.0 # monotonic; 0 = never received + self._count: int = 0 + self._rate_hz: float = 0.0 + self._rate_ts: float = time.monotonic() + self._rate_cnt: int = 0 + self._lock = threading.Lock() + + # ── Callback (called from subscription) ──────────────────────────────── + + def on_message(self, _msg) -> None: + """Record receipt of any message on this topic.""" + now = time.monotonic() + with self._lock: + self._last_rx = now + self._count += 1 + self._rate_cnt += 1 + # Update rate estimate every ~2 s + elapsed = now - self._rate_ts + if elapsed >= 2.0: + self._rate_hz = self._rate_cnt / elapsed + self._rate_cnt = 0 + self._rate_ts = now + + # ── Status query ─────────────────────────────────────────────────────── + + def age_s(self, now: Optional[float] = None) -> float: + """Seconds since last message (∞ if never received).""" + if now is None: + now = time.monotonic() + with self._lock: + if self._last_rx == 0.0: + return float("inf") + return now - self._last_rx + + def rate_hz(self) -> float: + with self._lock: + return self._rate_hz + + def msg_count(self) -> int: + with self._lock: + return self._count + + def level(self, now: Optional[float] = None) -> int: + age = self.age_s(now) + if age >= self.error_s: + return ERROR + if age >= self.warn_s: + return WARN + return OK + + def diagnostic_status(self, now: Optional[float] = None) -> DiagnosticStatus: + if now is None: + now = time.monotonic() + age = self.age_s(now) + lvl = self.level(now) + hz = self.rate_hz() + cnt = self.msg_count() + + if age == float("inf"): + msg = f"ERROR: no data received on {self.topic}" + elif lvl == ERROR: + msg = f"ERROR: stale {age:.1f}s (threshold {self.error_s:.1f}s)" + elif lvl == WARN: + msg = f"WARN: stale {age:.1f}s (threshold {self.warn_s:.1f}s)" + else: + msg = f"OK ({hz:.1f} Hz)" + + age_str = "inf" if age == float("inf") else f"{age:.2f}" + status = DiagnosticStatus() + status.level = lvl + status.name = self.name + status.message = msg + status.hardware_id = self.topic + status.values = [ + KeyValue(key="topic", value=self.topic), + KeyValue(key="age_s", value=age_str), + KeyValue(key="rate_hz", value=f"{hz:.2f}"), + KeyValue(key="count", value=str(cnt)), + KeyValue(key="warn_s", value=str(self.warn_s)), + KeyValue(key="error_s", value=str(self.error_s)), + KeyValue(key="critical", value=str(self.critical)), + ] + return status + + def summary_dict(self, now: Optional[float] = None) -> dict: + if now is None: + now = time.monotonic() + age = self.age_s(now) + return { + "status": _LEVEL_NAMES[self.level(now)], + "age_s": round(age, 2) if age != float("inf") else None, + "rate_hz": round(self.rate_hz(), 2), + "count": self.msg_count(), + "critical": self.critical, + } + + +# ── SensorHealthNode ─────────────────────────────────────────────────────────── + +class SensorHealthNode(Node): + """Monitor all sensor topics; publish DiagnosticArray + MQTT JSON.""" + + def __init__(self) -> None: + super().__init__("sensor_health_node") + + # ── Parameters ───────────────────────────────────────────────────── + self.declare_parameter("check_hz", 1.0) + self.declare_parameter("mqtt_broker", "localhost") + self.declare_parameter("mqtt_port", 1883) + self.declare_parameter("mqtt_topic", "saltybot/health") + self.declare_parameter("mqtt_enabled", True) + + check_hz = self.get_parameter("check_hz").value + self._mqtt_broker = self.get_parameter("mqtt_broker").value + self._mqtt_port = int(self.get_parameter("mqtt_port").value) + self._mqtt_topic = self.get_parameter("mqtt_topic").value + mqtt_enabled = self.get_parameter("mqtt_enabled").value + + # ── Build sensor watchers ─────────────────────────────────────────── + self._watchers: Dict[str, SensorWatcher] = {} + for cfg in DEFAULT_SENSORS: + w = SensorWatcher( + topic = cfg["topic"], + name = cfg["name"], + warn_s = float(cfg.get("warn_s", 1.0)), + error_s = float(cfg.get("error_s", 3.0)), + critical = bool(cfg.get("critical", False)), + ) + self._watchers[cfg["name"]] = w + + # ── Subscriptions — one per sensor type ──────────────────────────── + # Best-effort QoS for sensor data (sensors may use BEST_EFFORT publishers) + be_qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, depth=1, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + ) + rel_qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, depth=5, + reliability=ReliabilityPolicy.RELIABLE, + ) + + self._subscribe(Image, "/camera/color/image_raw", "camera_color", be_qos) + self._subscribe(Image, "/camera/depth/image_rect_raw", "camera_depth", be_qos) + self._subscribe(CameraInfo, "/camera/color/camera_info", "camera_info", be_qos) + self._subscribe(LaserScan, "/scan", "lidar", be_qos) + self._subscribe(Imu, "/imu/data", "imu", be_qos) + self._subscribe(String, "/saltybot/uwb/range", "uwb", rel_qos) + self._subscribe(String, "/saltybot/battery", "battery", rel_qos) + self._subscribe(String, "/saltybot/motor_daemon/status", "motor_daemon", rel_qos) + + # ── Publishers ───────────────────────────────────────────────────── + self._pub_diag = self.create_publisher( + DiagnosticArray, "/saltybot/diagnostics", 10) + self._pub_health = self.create_publisher( + String, "/saltybot/sensor_health", 10) + + # ── MQTT ─────────────────────────────────────────────────────────── + self._mqtt_client = None + if mqtt_enabled: + self._connect_mqtt() + + # ── Timer ────────────────────────────────────────────────────────── + self.create_timer(1.0 / max(0.1, check_hz), self._publish_diagnostics) + + sensor_list = ", ".join(self._watchers.keys()) + self.get_logger().info( + f"[sensor_health] monitoring {len(self._watchers)} sensors: {sensor_list}" + ) + + # ── Subscription helper ──────────────────────────────────────────────── + + def _subscribe(self, msg_type, topic: str, name: str, qos) -> None: + if name not in self._watchers: + return + watcher = self._watchers[name] + self.create_subscription(msg_type, topic, watcher.on_message, qos) + + # ── MQTT ─────────────────────────────────────────────────────────────── + + def _connect_mqtt(self) -> None: + try: + import paho.mqtt.client as mqtt # type: ignore + self._mqtt_client = mqtt.Client(client_id="saltybot_sensor_health") + self._mqtt_client.on_connect = self._on_mqtt_connect + self._mqtt_client.connect_async(self._mqtt_broker, self._mqtt_port, 60) + self._mqtt_client.loop_start() + except ImportError: + self.get_logger().warn( + "[sensor_health] paho-mqtt not installed — MQTT publishing disabled" + ) + except Exception as e: + self.get_logger().warn(f"[sensor_health] MQTT connect failed: {e}") + + def _on_mqtt_connect(self, client, userdata, flags, rc) -> None: + if rc == 0: + self.get_logger().info( + f"[sensor_health] MQTT connected to {self._mqtt_broker}:{self._mqtt_port}" + ) + else: + self.get_logger().warn(f"[sensor_health] MQTT connect rc={rc}") + + def _publish_mqtt(self, payload: str) -> None: + if self._mqtt_client is None: + return + try: + self._mqtt_client.publish(self._mqtt_topic, payload, qos=0, retain=True) + except Exception as e: + self.get_logger().warn(f"[sensor_health] MQTT publish error: {e}") + + # ── Diagnostic publisher ─────────────────────────────────────────────── + + def _publish_diagnostics(self) -> None: + now = time.monotonic() + wall = time.time() + + # Build DiagnosticArray + diag_array = DiagnosticArray() + diag_array.header.stamp = self.get_clock().now().to_msg() + + sensor_summaries: dict = {} + worst_level = OK + + for name, watcher in self._watchers.items(): + ds = watcher.diagnostic_status(now) + diag_array.status.append(ds) + + if ds.level > worst_level: + worst_level = ds.level + + sensor_summaries[name] = watcher.summary_dict(now) + + # Overall system status + n_error = sum(1 for w in self._watchers.values() if w.level(now) == ERROR) + n_warn = sum(1 for w in self._watchers.values() if w.level(now) == WARN) + crit_err = [n for n, w in self._watchers.items() + if w.critical and w.level(now) == ERROR] + + overall = "OK" + if crit_err: + overall = "ERROR" + elif n_error > 0: + overall = "ERROR" + elif n_warn > 0: + overall = "WARN" + + # Publish DiagnosticArray + self._pub_diag.publish(diag_array) + + # JSON summary + summary = { + "timestamp": wall, + "overall": overall, + "n_ok": len(self._watchers) - n_error - n_warn, + "n_warn": n_warn, + "n_error": n_error, + "critical_err": crit_err, + "sensors": sensor_summaries, + } + payload = json.dumps(summary) + self._pub_health.publish(String(data=payload)) + self._publish_mqtt(payload) + + # Log on transitions or errors + if worst_level >= ERROR: + self.get_logger().warn( + f"[sensor_health] {overall}: {n_error} error(s), {n_warn} warn(s)" + + (f" — critical: {crit_err}" if crit_err else "") + ) + + def destroy_node(self) -> None: + if self._mqtt_client is not None: + try: + self._mqtt_client.loop_stop() + self._mqtt_client.disconnect() + except Exception: + pass + super().destroy_node() + + +def main(args=None) -> None: + rclpy.init(args=args) + node = SensorHealthNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_health_monitor/setup.py b/jetson/ros2_ws/src/saltybot_health_monitor/setup.py index 1b08bbb..5179230 100644 --- a/jetson/ros2_ws/src/saltybot_health_monitor/setup.py +++ b/jetson/ros2_ws/src/saltybot_health_monitor/setup.py @@ -4,27 +4,34 @@ package_name = "saltybot_health_monitor" setup( name=package_name, - version="0.1.0", + version="0.2.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/health_monitor.launch.py"]), - (f"share/{package_name}/config", ["config/health_config.yaml"]), + (f"share/{package_name}/launch", [ + "launch/health_monitor.launch.py", + "launch/sensor_health.launch.py", + ]), + (f"share/{package_name}/config", [ + "config/health_config.yaml", + "config/sensor_health_params.yaml", + ]), ], - install_requires=["setuptools", "pyyaml"], + install_requires=["setuptools", "pyyaml", "paho-mqtt"], zip_safe=True, - maintainer="sl-controls", - maintainer_email="sl-controls@saltylab.local", + maintainer="sl-jetson", + maintainer_email="sl-jetson@saltylab.local", description=( - "System health monitor: tracks node heartbeats, detects down nodes, " - "triggers auto-restart, publishes system health status" + "System health monitor: node heartbeats + sensor topic staleness " + "detection with DiagnosticArray and MQTT (Issue #566)" ), - license="MIT", + license="Apache-2.0", tests_require=["pytest"], entry_points={ "console_scripts": [ "health_monitor_node = saltybot_health_monitor.health_monitor_node:main", + "sensor_health_node = saltybot_health_monitor.sensor_health_node:main", ], }, ) diff --git a/jetson/ros2_ws/src/saltybot_health_monitor/test/test_sensor_health.py b/jetson/ros2_ws/src/saltybot_health_monitor/test/test_sensor_health.py new file mode 100644 index 0000000..0ea909a --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_health_monitor/test/test_sensor_health.py @@ -0,0 +1,344 @@ +"""test_sensor_health.py — Unit tests for sensor_health_node (Issue #566). + +Runs entirely offline: no ROS2 runtime, no hardware, no MQTT broker required. +""" + +import json +import sys +import time +import types +import unittest +from unittest.mock import MagicMock, patch + +# ── Stub ROS2 and sensor_msgs so tests run offline ──────────────────────────── + +def _install_stubs(): + # rclpy + rclpy = types.ModuleType("rclpy") + rclpy.init = lambda **_: None + rclpy.spin = lambda _: None + rclpy.shutdown = lambda: None + + node_mod = types.ModuleType("rclpy.node") + class _Node: + def __init__(self, *a, **kw): pass + def declare_parameter(self, *a, **kw): pass + def get_parameter(self, name): + defaults = { + "check_hz": 1.0, "mqtt_broker": "localhost", + "mqtt_port": 1883, "mqtt_topic": "saltybot/health", + "mqtt_enabled": False, + } + m = MagicMock(); m.value = defaults.get(name, False) + return m + def get_logger(self): return MagicMock() + def get_clock(self): return MagicMock() + def create_subscription(self, *a, **kw): pass + def create_publisher(self, *a, **kw): return MagicMock() + def create_timer(self, *a, **kw): pass + def destroy_node(self): pass + node_mod.Node = _Node + rclpy.node = node_mod + + qos_mod = types.ModuleType("rclpy.qos") + for attr in ("QoSProfile", "HistoryPolicy", "ReliabilityPolicy", "DurabilityPolicy"): + setattr(qos_mod, attr, MagicMock()) + rclpy.qos = qos_mod + + sys.modules.setdefault("rclpy", rclpy) + sys.modules.setdefault("rclpy.node", rclpy.node) + sys.modules.setdefault("rclpy.qos", rclpy.qos) + + # diagnostic_msgs + diag = types.ModuleType("diagnostic_msgs") + diag_msg = types.ModuleType("diagnostic_msgs.msg") + + class _DiagStatus: + OK = 0 + WARN = 1 + ERROR = 2 + def __init__(self): + self.level = 0; self.name = ""; self.message = "" + self.hardware_id = ""; self.values = [] + + class _DiagArray: + def __init__(self): + self.header = MagicMock(); self.status = [] + + class _KeyValue: + def __init__(self, key="", value=""): + self.key = key; self.value = value + + diag_msg.DiagnosticStatus = _DiagStatus + diag_msg.DiagnosticArray = _DiagArray + diag_msg.KeyValue = _KeyValue + diag.msg = diag_msg + sys.modules.setdefault("diagnostic_msgs", diag) + sys.modules.setdefault("diagnostic_msgs.msg", diag_msg) + + # sensor_msgs + sens = types.ModuleType("sensor_msgs") + sens_msg = types.ModuleType("sensor_msgs.msg") + for cls_name in ("Image", "CameraInfo", "Imu", "LaserScan"): + setattr(sens_msg, cls_name, MagicMock) + sens.msg = sens_msg + sys.modules.setdefault("sensor_msgs", sens) + sys.modules.setdefault("sensor_msgs.msg", sens_msg) + + # std_msgs + std = types.ModuleType("std_msgs") + std_msg = types.ModuleType("std_msgs.msg") + class _String: + def __init__(self, data=""): self.data = data + std_msg.String = _String + std.msg = std_msg + sys.modules.setdefault("std_msgs", std) + sys.modules.setdefault("std_msgs.msg", std_msg) + + +_install_stubs() + +from saltybot_health_monitor.sensor_health_node import ( # noqa: E402 + SensorWatcher, OK, WARN, ERROR, _LEVEL_NAMES, +) + + +# ── SensorWatcher: initial state ────────────────────────────────────────────── + +class TestSensorWatcherInitial(unittest.TestCase): + + def setUp(self): + self.w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=True) + + def test_initial_age_is_inf(self): + self.assertEqual(self.w.age_s(), float("inf")) + + def test_initial_level_is_error(self): + # Never received → age=inf ≥ error_s → ERROR + self.assertEqual(self.w.level(), ERROR) + + def test_initial_count_zero(self): + self.assertEqual(self.w.msg_count(), 0) + + def test_initial_rate_zero(self): + self.assertAlmostEqual(self.w.rate_hz(), 0.0) + + def test_name_stored(self): + self.assertEqual(self.w.name, "lidar") + + def test_topic_stored(self): + self.assertEqual(self.w.topic, "/scan") + + def test_critical_stored(self): + self.assertTrue(self.w.critical) + + +# ── SensorWatcher: after receiving messages ─────────────────────────────────── + +class TestSensorWatcherAfterMessage(unittest.TestCase): + + def setUp(self): + self.w = SensorWatcher("/imu/data", "imu", warn_s=0.5, error_s=2.0, critical=True) + self.w.on_message(None) # simulate message receipt + + def test_age_is_small_after_message(self): + self.assertLess(self.w.age_s(), 0.1) + + def test_level_ok_immediately_after(self): + self.assertEqual(self.w.level(), OK) + + def test_count_increments(self): + self.w.on_message(None) + self.assertEqual(self.w.msg_count(), 2) + + def test_multiple_messages(self): + for _ in range(10): + self.w.on_message(None) + self.assertEqual(self.w.msg_count(), 11) + + +# ── SensorWatcher: staleness thresholds ────────────────────────────────────── + +class TestSensorWatcherStaleness(unittest.TestCase): + + def _make_stale(self, seconds_ago: float) -> SensorWatcher: + """Return a watcher whose last_rx was `seconds_ago` seconds in the past.""" + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + # Backdate the last_rx directly + with w._lock: + w._last_rx -= seconds_ago + return w + + def test_ok_when_fresh(self): + w = self._make_stale(0.5) + self.assertEqual(w.level(), OK) + + def test_warn_at_warn_threshold(self): + w = self._make_stale(1.1) + self.assertEqual(w.level(), WARN) + + def test_error_at_error_threshold(self): + w = self._make_stale(3.1) + self.assertEqual(w.level(), ERROR) + + def test_age_matches_backdated_time(self): + w = self._make_stale(2.0) + self.assertAlmostEqual(w.age_s(), 2.0, delta=0.1) + + def test_warn_level_between_thresholds(self): + w = self._make_stale(2.0) # 1.0 < 2.0 < 3.0 + self.assertEqual(w.level(), WARN) + + +# ── SensorWatcher: diagnostic_status output ────────────────────────────────── + +class TestSensorWatcherDiagStatus(unittest.TestCase): + + def test_never_received_is_error(self): + w = SensorWatcher("/camera/color/image_raw", "camera_color", + warn_s=1.0, error_s=3.0, critical=True) + ds = w.diagnostic_status() + self.assertEqual(ds.level, ERROR) + self.assertIn("no data", ds.message) + + def test_ok_status_message(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + ds = w.diagnostic_status() + self.assertEqual(ds.level, OK) + self.assertIn("OK", ds.message) + + def test_warn_status_message(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + with w._lock: + w._last_rx -= 1.5 + ds = w.diagnostic_status() + self.assertEqual(ds.level, WARN) + self.assertIn("WARN", ds.message) + + def test_hardware_id_is_topic(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + ds = w.diagnostic_status() + self.assertEqual(ds.hardware_id, "/scan") + + def test_kv_keys_present(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + ds = w.diagnostic_status() + kv_keys = {kv.key for kv in ds.values} + for expected in ("topic", "age_s", "rate_hz", "count", "warn_s", "error_s"): + self.assertIn(expected, kv_keys) + + def test_age_inf_displayed_as_inf(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + ds = w.diagnostic_status() + kv = {kv.key: kv.value for kv in ds.values} + self.assertEqual(kv["age_s"], "inf") + + +# ── SensorWatcher: summary_dict output ─────────────────────────────────────── + +class TestSensorWatcherSummaryDict(unittest.TestCase): + + def test_never_received_age_is_none(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + d = w.summary_dict() + self.assertIsNone(d["age_s"]) + self.assertEqual(d["status"], "ERROR") + + def test_ok_status_string(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + d = w.summary_dict() + self.assertEqual(d["status"], "OK") + + def test_warn_status_string(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + with w._lock: + w._last_rx -= 1.5 + d = w.summary_dict() + self.assertEqual(d["status"], "WARN") + + def test_error_status_string(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + with w._lock: + w._last_rx -= 5.0 + d = w.summary_dict() + self.assertEqual(d["status"], "ERROR") + + def test_age_rounded(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + d = w.summary_dict() + self.assertIsInstance(d["age_s"], float) + + def test_critical_flag(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=True) + self.assertTrue(w.summary_dict()["critical"]) + + def test_non_critical_flag(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + self.assertFalse(w.summary_dict()["critical"]) + + def test_count_in_summary(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + for _ in range(5): + w.on_message(None) + self.assertEqual(w.summary_dict()["count"], 5) + + +# ── Level name mapping ───────────────────────────────────────────────────────── + +class TestLevelNames(unittest.TestCase): + + def test_ok_name(self): + self.assertEqual(_LEVEL_NAMES[OK], "OK") + + def test_warn_name(self): + self.assertEqual(_LEVEL_NAMES[WARN], "WARN") + + def test_error_name(self): + self.assertEqual(_LEVEL_NAMES[ERROR], "ERROR") + + +# ── Thread safety ───────────────────────────────────────────────────────────── + +class TestSensorWatcherThreadSafety(unittest.TestCase): + + def test_concurrent_messages(self): + import threading + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + N = 500 + threads = [threading.Thread(target=w.on_message, args=(None,)) for _ in range(N)] + for t in threads: + t.start() + for t in threads: + t.join() + self.assertEqual(w.msg_count(), N) + + def test_concurrent_reads(self): + import threading + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + errors = [] + def read_loop(): + for _ in range(100): + try: + w.level() + w.age_s() + w.rate_hz() + except Exception as e: + errors.append(e) + threads = [threading.Thread(target=read_loop) for _ in range(5)] + for t in threads: t.start() + for t in threads: t.join() + self.assertEqual(errors, []) + + +if __name__ == "__main__": + unittest.main() diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/config/uwb_position_params.yaml b/jetson/ros2_ws/src/saltybot_uwb_position/config/uwb_position_params.yaml new file mode 100644 index 0000000..e728726 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_position/config/uwb_position_params.yaml @@ -0,0 +1,58 @@ +# uwb_position_params.yaml — UWB position node configuration (Issue #546) +# +# ROS2 Python node: saltybot_uwb_position +# Serial: ESP32 UWB tag → Jetson via USB-CDC (JSON per line) +# +# Usage: +# ros2 launch saltybot_uwb_position uwb_position.launch.py +# +# JSON protocol (ESP32 → Jetson): +# Full frame (preferred): +# {"ts": 123456, "ranges": [{"id": 0, "d_mm": 1500, "rssi": -65.0}, ...]} +# Per-anchor: +# {"id": 0, "d_mm": 1500, "rssi": -65.0} +# Both "d_mm" and "range_mm" accepted for the range field. + +uwb_position: + ros__parameters: + + # ── Serial (USB-CDC from ESP32 DW3000 tag) ────────────────────────────── + serial_port: /dev/ttyUSB0 # udev rule: /dev/ttyUWB_TAG recommended + baudrate: 115200 + + # ── Anchor definitions ────────────────────────────────────────────────── + # anchor_ids: integer list of anchor IDs + # anchor_positions: flat float list, 3 values per anchor [x, y, z] in + # the map frame (metres). Length must be 3 × len(anchor_ids). + # + # Example: 4-anchor rectangular room layout, anchors at 2 m height + # Corner NW (0,0), NE (5,0), SE (5,5), SW (0,5) + anchor_ids: [0, 1, 2, 3] + anchor_positions: [ + 0.0, 0.0, 2.0, # anchor 0 — NW corner + 5.0, 0.0, 2.0, # anchor 1 — NE corner + 5.0, 5.0, 2.0, # anchor 2 — SE corner + 0.0, 5.0, 2.0, # anchor 3 — SW corner + ] + + # ── Trilateration mode ────────────────────────────────────────────────── + solve_z: false # false = 2D (robot_z fixed), true = full 3D (needs 4+ anchors) + robot_z: 0.0 # m — robot floor height in map frame (used when solve_z=false) + + # ── Validity & timing ─────────────────────────────────────────────────── + publish_rate: 10.0 # Hz — /saltybot/uwb/pose + /saltybot/uwb/status + range_timeout_s: 1.5 # s — discard anchor range after this age + min_range_m: 0.05 # m — minimum valid range + max_range_m: 30.0 # m — maximum valid range (DW3000: up to 120 m, use conservative) + + # ── Outlier rejection ─────────────────────────────────────────────────── + outlier_threshold_m: 0.40 # m — residual above this → outlier + outlier_strikes_max: 5 # consecutive outlier frames before anchor is flagged + + # ── Kalman filter ─────────────────────────────────────────────────────── + kf_process_noise: 0.05 # Q — lower = smoother but slower to respond + kf_meas_noise: 0.10 # R — higher = trust KF prediction more than measurement + + # ── TF2 frames ────────────────────────────────────────────────────────── + map_frame: map # parent frame + uwb_frame: uwb_link # child frame (robot UWB tag position) diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/launch/uwb_position.launch.py b/jetson/ros2_ws/src/saltybot_uwb_position/launch/uwb_position.launch.py new file mode 100644 index 0000000..348b410 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_position/launch/uwb_position.launch.py @@ -0,0 +1,35 @@ +"""Launch file for saltybot_uwb_position (Issue #546).""" + +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() -> LaunchDescription: + config_path = os.path.join( + get_package_share_directory("saltybot_uwb_position"), + "config", + "uwb_position_params.yaml", + ) + + port_arg = DeclareLaunchArgument( + "serial_port", + default_value="/dev/ttyUSB0", + description="USB serial port for ESP32 UWB tag (e.g. /dev/ttyUWB_TAG)", + ) + + uwb_node = Node( + package="saltybot_uwb_position", + executable="uwb_position", + name="uwb_position", + parameters=[ + config_path, + {"serial_port": LaunchConfiguration("serial_port")}, + ], + output="screen", + ) + + return LaunchDescription([port_arg, uwb_node]) diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/package.xml b/jetson/ros2_ws/src/saltybot_uwb_position/package.xml new file mode 100644 index 0000000..0c1b271 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_position/package.xml @@ -0,0 +1,36 @@ + + + + saltybot_uwb_position + 0.1.0 + + ROS2 UWB position node for Jetson (Issue #546). + Reads JSON range data from an ESP32 DW3000 UWB tag over USB serial, + trilaterates from 3+ fixed infrastructure anchors, publishes + PoseStamped on /saltybot/uwb/pose, per-anchor UwbRange on + /saltybot/uwb/range/<id>, JSON diagnostics on /saltybot/uwb/status, + and broadcasts the uwb_link→map TF2 transform. + + sl-perception + MIT + + ament_python + + rclpy + geometry_msgs + std_msgs + tf2_ros + saltybot_uwb_msgs + + + python3-numpy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/resource/saltybot_uwb_position b/jetson/ros2_ws/src/saltybot_uwb_position/resource/saltybot_uwb_position new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/__init__.py b/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/trilateration.py b/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/trilateration.py new file mode 100644 index 0000000..88ce544 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/trilateration.py @@ -0,0 +1,254 @@ +""" +trilateration.py — Pure-math helpers for UWB trilateration (Issue #546). + +No ROS2 dependencies — importable in unit tests without a ROS2 install. + +Algorithm: linear least-squares from N ≥ 3 anchors +──────────────────────────────────────────────────── +Given anchors A_i = (x_i, y_i, z_i) with measured ranges r_i, the robot +position p = (x, y, z) satisfies: + + (x - x_i)² + (y - y_i)² + (z - z_i)² = r_i² + +Subtract the equation for anchor 0 from each subsequent equation to +linearise: + + 2(x_i - x_0)x + 2(y_i - y_0)y + 2(z_i - z_0)z + = r_0² - r_i² + ‖a_i‖² - ‖a_0‖² + +This yields A·p = b where A is (N-1)×3 and b is (N-1)×1. +Solve with numpy least-squares (lstsq). + +2D mode (solve_z=False): + z is fixed (robot_z parameter, default 0). + Reduce to 2 unknowns (x, y): subtract z terms from b, drop the z column. + Needs N ≥ 3 anchors. + +Outlier rejection: + Compute residual for each anchor: |r_meas - ‖p - a_i‖|. + Reject anchors with residual > threshold. Repeat if enough remain. +""" + +from __future__ import annotations + +import math +from typing import List, Optional, Tuple + +import numpy as np + + +AnchorPos = Tuple[float, float, float] # (x, y, z) in map frame (metres) +RangeM = float # measured range (metres) + + +# ── Core trilateration ──────────────────────────────────────────────────────── + +def trilaterate( + anchors: List[AnchorPos], + ranges: List[RangeM], + fixed_z: Optional[float] = None, +) -> Tuple[float, float, float]: + """Least-squares trilateration from N ≥ 3 anchor ranges. + + Parameters + ---------- + anchors : list of (x, y, z) anchor positions in the map frame (metres) + ranges : measured distances from robot to each anchor (metres) + fixed_z : if not None, treat robot z as this value and solve 2D only + + Returns + ------- + (x, y, z) : estimated robot position in the map frame (metres). + When fixed_z is given, z = fixed_z. + + Raises + ------ + ValueError : fewer than 3 anchors provided + RuntimeError: linear system is rank-deficient (e.g., collinear anchors) + """ + n = len(anchors) + if n < 3: + raise ValueError(f"Need ≥ 3 anchors for trilateration, got {n}") + if len(ranges) != n: + raise ValueError("len(anchors) != len(ranges)") + + a = np.array(anchors, dtype=np.float64) # (N, 3) + r = np.array(ranges, dtype=np.float64) # (N,) + + # Reference anchor (index 0) + a0 = a[0] + r0 = r[0] + norm_a0_sq = float(np.dot(a0, a0)) + + if fixed_z is not None: + # 2D mode: solve for (x, y), z is known + z = fixed_z + A_rows = [] + b_rows = [] + for i in range(1, n): + ai = a[i] + ri = r[i] + norm_ai_sq = float(np.dot(ai, ai)) + # rhs adjusted for known z + rhs = (r0**2 - ri**2 + norm_ai_sq - norm_a0_sq + + 2.0 * (ai[2] - a0[2]) * z) + A_rows.append([2.0 * (ai[0] - a0[0]), + 2.0 * (ai[1] - a0[1])]) + b_rows.append(rhs) + + A_mat = np.array(A_rows, dtype=np.float64) + b_vec = np.array(b_rows, dtype=np.float64) + result, _, rank, _ = np.linalg.lstsq(A_mat, b_vec, rcond=None) + if rank < 2: + raise RuntimeError("Rank-deficient 2D trilateration system — collinear anchors?") + return float(result[0]), float(result[1]), z + + else: + # 3D mode: solve for (x, y, z) — needs N ≥ 4 for well-conditioned solve + A_rows = [] + b_rows = [] + for i in range(1, n): + ai = a[i] + ri = r[i] + norm_ai_sq = float(np.dot(ai, ai)) + rhs = r0**2 - ri**2 + norm_ai_sq - norm_a0_sq + A_rows.append([2.0 * (ai[0] - a0[0]), + 2.0 * (ai[1] - a0[1]), + 2.0 * (ai[2] - a0[2])]) + b_rows.append(rhs) + + A_mat = np.array(A_rows, dtype=np.float64) + b_vec = np.array(b_rows, dtype=np.float64) + result, _, rank, _ = np.linalg.lstsq(A_mat, b_vec, rcond=None) + if rank < 3: + raise RuntimeError("Rank-deficient 3D trilateration system — coplanar anchors?") + return float(result[0]), float(result[1]), float(result[2]) + + +# ── Outlier rejection ───────────────────────────────────────────────────────── + +def reject_outliers( + anchors: List[AnchorPos], + ranges: List[RangeM], + position: Tuple[float, float, float], + threshold_m: float = 0.4, +) -> List[int]: + """Return indices of inlier anchors (residual ≤ threshold_m). + + Parameters + ---------- + anchors : anchor positions in map frame + ranges : measured ranges (metres) + position : current position estimate (x, y, z) + threshold_m : max allowable range residual to be an inlier + + Returns + ------- + inlier_indices : sorted list of indices whose residual is within threshold + """ + px, py, pz = position + inliers = [] + for i, (anchor, r_meas) in enumerate(zip(anchors, ranges)): + ax, ay, az = anchor + r_pred = math.sqrt((px - ax)**2 + (py - ay)**2 + (pz - az)**2) + residual = abs(r_meas - r_pred) + if residual <= threshold_m: + inliers.append(i) + return inliers + + +def residuals( + anchors: List[AnchorPos], + ranges: List[RangeM], + position: Tuple[float, float, float], +) -> List[float]: + """Compute per-anchor range residual (metres) for diagnostics.""" + px, py, pz = position + res = [] + for anchor, r_meas in zip(anchors, ranges): + ax, ay, az = anchor + r_pred = math.sqrt((px - ax)**2 + (py - ay)**2 + (pz - az)**2) + res.append(abs(r_meas - r_pred)) + return res + + +# ── 3D Kalman filter (constant-velocity, position-only observations) ────────── + +class KalmanFilter3D: + """Constant-velocity Kalman filter for 3D UWB position. + + State: [x, y, z, vx, vy, vz]ᵀ + Observation: [x, y, z] (position only) + """ + + def __init__( + self, + process_noise: float = 0.1, + measurement_noise: float = 0.15, + dt: float = 0.1, + ) -> None: + self._q = process_noise + self._r = measurement_noise + self._dt = dt + self._x = np.zeros(6) + self._P = np.diag([1.0, 1.0, 1.0, 0.5, 0.5, 0.5]) + self._init = False + + def _build_F(self, dt: float) -> np.ndarray: + F = np.eye(6) + F[0, 3] = dt + F[1, 4] = dt + F[2, 5] = dt + return F + + def _build_Q(self, dt: float) -> np.ndarray: + q = self._q + dt2 = dt * dt + dt3 = dt2 * dt + dt4 = dt3 * dt + Q = np.zeros((6, 6)) + # Position-velocity cross terms (constant white-noise model) + for i in range(3): + Q[i, i ] = q * dt4 / 4.0 + Q[i, i+3] = q * dt3 / 2.0 + Q[i+3, i ] = q * dt3 / 2.0 + Q[i+3, i+3] = q * dt2 + return Q + + def predict(self, dt: float | None = None) -> None: + if dt is not None: + self._dt = dt + F = self._build_F(self._dt) + Q = self._build_Q(self._dt) + self._x = F @ self._x + self._P = F @ self._P @ F.T + Q + + def update(self, x_m: float, y_m: float, z_m: float) -> None: + if not self._init: + self._x[:3] = [x_m, y_m, z_m] + self._init = True + return + + H = np.zeros((3, 6)) + H[0, 0] = 1.0 + H[1, 1] = 1.0 + H[2, 2] = 1.0 + R = np.eye(3) * self._r + + z_vec = np.array([x_m, y_m, z_m]) + innov = z_vec - H @ self._x + S = H @ self._P @ H.T + R + K = self._P @ H.T @ np.linalg.inv(S) + self._x = self._x + K @ innov + self._P = (np.eye(6) - K @ H) @ self._P + + def position(self) -> Tuple[float, float, float]: + return float(self._x[0]), float(self._x[1]), float(self._x[2]) + + def velocity(self) -> Tuple[float, float, float]: + return float(self._x[3]), float(self._x[4]), float(self._x[5]) + + def reset(self) -> None: + self._x = np.zeros(6) + self._P = np.diag([1.0, 1.0, 1.0, 0.5, 0.5, 0.5]) + self._init = False diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/uwb_position_node.py b/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/uwb_position_node.py new file mode 100644 index 0000000..6031c3e --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/uwb_position_node.py @@ -0,0 +1,487 @@ +#!/usr/bin/env python3 +""" +uwb_position_node.py — ROS2 UWB position node for Jetson (Issue #546). + +Reads JSON range measurements from an ESP32 UWB tag (DW3000) over USB +serial, trilaterates position from 3+ fixed infrastructure anchors, and +publishes the robot's absolute position in the map frame. + +Serial protocol (one JSON object per line, UTF-8): +──────────────────────────────────────────────────── + Tag → Jetson (full frame, all anchors at once): + {"ts": 123456, "ranges": [ + {"id": 0, "d_mm": 1500, "rssi": -65.0}, + {"id": 1, "d_mm": 2100, "rssi": -68.0}, + {"id": 2, "d_mm": 1800, "rssi": -70.0} + ]} + + Alternate (per-anchor, one line per measurement): + {"id": 0, "d_mm": 1500, "rssi": -65.0} + + Both "d_mm" and "range_mm" are accepted. + +Anchor positions: +───────────────── + Fixed anchors are configured in uwb_position_params.yaml as flat arrays: + anchor_ids: [0, 1, 2, 3] + anchor_positions: [x0, y0, z0, x1, y1, z1, ...] # metres in map frame + +Publishes: +────────── + /saltybot/uwb/pose (geometry_msgs/PoseStamped) 10 Hz + /saltybot/uwb/range/ (saltybot_uwb_msgs/UwbRange) per anchor, on arrival + /saltybot/uwb/status (std_msgs/String) 10 Hz JSON diagnostics + +TF2: +──── + Broadcasts uwb_link → map from /saltybot/uwb/pose position. + +Outlier rejection: +────────────────── + After initial trilateration, anchors with range residual > outlier_threshold_m + are dropped. If ≥ 3 inliers remain (2D mode) the position is re-solved. + Anchors rejected in N_strikes_max consecutive frames are flagged in status. +""" + +import json +import math +import threading +import time +from typing import Dict, List, Optional, Tuple + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy + +from geometry_msgs.msg import ( + PoseStamped, TransformStamped, + Quaternion, +) +from std_msgs.msg import Header, String +from tf2_ros import TransformBroadcaster + +from saltybot_uwb_msgs.msg import UwbRange + +from saltybot_uwb_position.trilateration import ( + trilaterate, + reject_outliers, + residuals, + KalmanFilter3D, +) + +try: + import serial + _SERIAL_OK = True +except ImportError: + _SERIAL_OK = False + + +# ── Serial reader thread ────────────────────────────────────────────────────── + +class SerialJsonReader(threading.Thread): + """Background thread: reads newline-delimited JSON from a serial port.""" + + def __init__(self, port: str, baudrate: int, callback, logger) -> None: + super().__init__(daemon=True) + self._port = port + self._baudrate = baudrate + self._callback = callback + self._logger = logger + self._running = False + self._ser = None + + def run(self) -> None: + self._running = True + while self._running: + try: + self._ser = serial.Serial( + self._port, self._baudrate, timeout=1.0 + ) + self._logger.info(f"UWB serial: opened {self._port} @ {self._baudrate}") + self._read_loop() + except Exception as exc: + self._logger.warning( + f"UWB serial error ({self._port}): {exc} — retry in 2 s" + ) + if self._ser and self._ser.is_open: + self._ser.close() + time.sleep(2.0) + + def _read_loop(self) -> None: + while self._running: + try: + raw = self._ser.readline() + if not raw: + continue + line = raw.decode("utf-8", errors="replace").strip() + if not line: + continue + try: + obj = json.loads(line) + self._callback(obj) + except json.JSONDecodeError: + pass # ignore malformed lines silently + except Exception as exc: + self._logger.warning(f"UWB read error: {exc}") + break + + def stop(self) -> None: + self._running = False + if self._ser and self._ser.is_open: + self._ser.close() + + +# ── Node ────────────────────────────────────────────────────────────────────── + +class UwbPositionNode(Node): + """ROS2 UWB position node — trilateration from 3+ fixed anchors.""" + + _MIN_ANCHORS_2D = 3 + _MIN_ANCHORS_3D = 4 + + def __init__(self) -> None: + super().__init__("uwb_position") + + # ── Parameters ──────────────────────────────────────────────────────── + self.declare_parameter("serial_port", "/dev/ttyUSB0") + self.declare_parameter("baudrate", 115200) + + # Anchor configuration: flat arrays, 3 floats per anchor + self.declare_parameter("anchor_ids", [0, 1, 2]) + self.declare_parameter("anchor_positions", [ + 0.0, 0.0, 2.0, # anchor 0: x, y, z (metres in map frame) + 5.0, 0.0, 2.0, # anchor 1 + 5.0, 5.0, 2.0, # anchor 2 + ]) + + self.declare_parameter("solve_z", False) # 2D mode by default + self.declare_parameter("robot_z", 0.0) # m — robot floor height + self.declare_parameter("publish_rate", 10.0) # Hz — pose + status + self.declare_parameter("range_timeout_s", 1.5) # s — stale range threshold + self.declare_parameter("max_range_m", 30.0) # m — validity max + self.declare_parameter("min_range_m", 0.05) # m — validity min + self.declare_parameter("outlier_threshold_m", 0.4) # m — RANSAC threshold + self.declare_parameter("outlier_strikes_max", 5) # frames before flagging + self.declare_parameter("kf_process_noise", 0.05) # Kalman Q + self.declare_parameter("kf_meas_noise", 0.10) # Kalman R + self.declare_parameter("map_frame", "map") + self.declare_parameter("uwb_frame", "uwb_link") + + # Load params + self._port = self.get_parameter("serial_port").value + self._baudrate = self.get_parameter("baudrate").value + self._solve_z = self.get_parameter("solve_z").value + self._robot_z = self.get_parameter("robot_z").value + self._publish_rate = self.get_parameter("publish_rate").value + self._timeout = self.get_parameter("range_timeout_s").value + self._max_r = self.get_parameter("max_range_m").value + self._min_r = self.get_parameter("min_range_m").value + self._outlier_thr = self.get_parameter("outlier_threshold_m").value + self._strikes_max = self.get_parameter("outlier_strikes_max").value + self._map_frame = self.get_parameter("map_frame").value + self._uwb_frame = self.get_parameter("uwb_frame").value + + # Parse anchor config + anchor_ids_raw = self.get_parameter("anchor_ids").value + anchor_pos_flat = self.get_parameter("anchor_positions").value + self._anchor_ids, self._anchor_positions = self._parse_anchors( + anchor_ids_raw, anchor_pos_flat + ) + self.get_logger().info( + f"UWB anchors: {[f'#{i}@({p[0]:.1f},{p[1]:.1f},{p[2]:.1f})' for i,p in zip(self._anchor_ids, self._anchor_positions)]}" + ) + + # ── State ───────────────────────────────────────────────────────────── + self._lock = threading.Lock() + # anchor_id → (range_m, rssi, timestamp_monotonic) + self._ranges: Dict[int, Tuple[float, float, float]] = {} + self._strikes: Dict[int, int] = {aid: 0 for aid in self._anchor_ids} + self._last_fix: Optional[Tuple[float, float, float]] = None + self._has_fix = False + self._fix_age = 0.0 + + # Kalman filter + self._kf = KalmanFilter3D( + process_noise=self.get_parameter("kf_process_noise").value, + measurement_noise=self.get_parameter("kf_meas_noise").value, + dt=1.0 / self._publish_rate, + ) + + # Per-anchor outlier publishers (pre-create for configured anchors) + sensor_qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=5, + ) + self._range_pubs: Dict[int, object] = {} + for aid in self._anchor_ids: + self._range_pubs[aid] = self.create_publisher( + UwbRange, f"/saltybot/uwb/range/{aid}", sensor_qos + ) + + # Main publishers + self._pose_pub = self.create_publisher(PoseStamped, "/saltybot/uwb/pose", 10) + self._status_pub = self.create_publisher(String, "/saltybot/uwb/status", 10) + + # TF2 + self._tf_broadcaster = TransformBroadcaster(self) + + # ── Serial reader ────────────────────────────────────────────────────── + if _SERIAL_OK: + self._reader = SerialJsonReader( + self._port, self._baudrate, + callback=self._on_serial_json, + logger=self.get_logger(), + ) + self._reader.start() + else: + self.get_logger().warning( + "pyserial not installed — serial I/O disabled (simulation mode)" + ) + + # ── Publish timer ────────────────────────────────────────────────────── + self._prev_publish_t = self.get_clock().now().nanoseconds * 1e-9 + self.create_timer(1.0 / self._publish_rate, self._publish_cb) + + self.get_logger().info( + f"UwbPositionNode ready — port={self._port} " + f"anchors={len(self._anchor_ids)} " + f"mode={'3D' if self._solve_z else '2D'} " + f"rate={self._publish_rate:.0f}Hz" + ) + + # ── Anchor config parsing ───────────────────────────────────────────────── + + def _parse_anchors( + self, + ids_raw, + pos_flat, + ): + ids = list(ids_raw) + n = len(ids) + if len(pos_flat) < n * 3: + raise ValueError( + f"anchor_positions must have 3×anchor_ids entries. " + f"Got {len(pos_flat)} values for {n} anchors." + ) + positions = [] + for i in range(n): + base = i * 3 + positions.append(( + float(pos_flat[base]), + float(pos_flat[base + 1]), + float(pos_flat[base + 2]), + )) + return ids, positions + + # ── Serial JSON callback (called from reader thread) ────────────────────── + + def _on_serial_json(self, obj: dict) -> None: + """Parse incoming JSON and update range table.""" + now = time.monotonic() + + if "ranges" in obj: + # Full-frame format: {"ts": ..., "ranges": [{id, d_mm, rssi}, ...]} + for entry in obj["ranges"]: + self._ingest_range_entry(entry, now) + else: + # Per-anchor format: {"id": 0, "d_mm": 1500, "rssi": -65.0} + self._ingest_range_entry(obj, now) + + def _ingest_range_entry(self, entry: dict, timestamp: float) -> None: + """Store a single anchor range measurement after validity checks.""" + try: + anchor_id = int(entry.get("id", entry.get("anchor_id", -1))) + # Accept both "d_mm" and "range_mm" + raw_mm = int(entry.get("d_mm", entry.get("range_mm", 0))) + rssi = float(entry.get("rssi", 0.0)) + except (KeyError, TypeError, ValueError): + return + + if anchor_id not in self._anchor_ids: + return # unknown / unconfigured anchor + + range_m = raw_mm / 1000.0 + if range_m < self._min_r or range_m > self._max_r: + return # outside validity window + + with self._lock: + self._ranges[anchor_id] = (range_m, rssi, timestamp) + + # Publish per-anchor range topic immediately + self._publish_anchor_range(anchor_id, range_m, raw_mm, rssi) + + # ── Per-anchor range publisher ──────────────────────────────────────────── + + def _publish_anchor_range( + self, anchor_id: int, range_m: float, raw_mm: int, rssi: float + ) -> None: + if anchor_id not in self._range_pubs: + return + msg = UwbRange() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = self._map_frame + msg.anchor_id = anchor_id + msg.range_m = float(range_m) + msg.raw_mm = int(raw_mm) + msg.rssi = float(rssi) + msg.tag_id = "" + self._range_pubs[anchor_id].publish(msg) + + # ── Main publish callback (10 Hz) ───────────────────────────────────────── + + def _publish_cb(self) -> None: + now_ros = self.get_clock().now() + now_mono = time.monotonic() + + # Collect fresh ranges + with self._lock: + fresh = { + aid: entry + for aid, entry in self._ranges.items() + if (now_mono - entry[2]) <= self._timeout + } + + # Check minimum anchor count for chosen mode + min_anch = self._MIN_ANCHORS_3D if self._solve_z else self._MIN_ANCHORS_2D + state = "no_fix" + pos = None + used_ids: List[int] = [] + res_map: Dict[int, float] = {} + + if len(fresh) >= min_anch: + active_ids = list(fresh.keys()) + anch_pos = [self._anchor_positions[self._anchor_ids.index(i)] + for i in active_ids] + anch_r = [fresh[i][0] for i in active_ids] + + try: + fixed_z = None if self._solve_z else self._robot_z + raw_pos = trilaterate(anch_pos, anch_r, fixed_z=fixed_z) + + # Outlier rejection + inlier_idx = reject_outliers( + anch_pos, anch_r, raw_pos, + threshold_m=self._outlier_thr, + ) + res_all = residuals(anch_pos, anch_r, raw_pos) + for k, aid in enumerate(active_ids): + res_map[aid] = round(res_all[k], 3) + + # Update consecutive strike counters + for k, aid in enumerate(active_ids): + if k in inlier_idx: + self._strikes[aid] = 0 + else: + self._strikes[aid] = self._strikes.get(aid, 0) + 1 + + # Re-solve with inliers if any were rejected + if len(inlier_idx) < len(active_ids) and len(inlier_idx) >= min_anch: + inlier_anch = [anch_pos[k] for k in inlier_idx] + inlier_r = [anch_r[k] for k in inlier_idx] + raw_pos = trilaterate(inlier_anch, inlier_r, fixed_z=fixed_z) + + used_ids = [active_ids[k] for k in inlier_idx] + pos = raw_pos + + # Kalman predict + update + dt = now_ros.nanoseconds * 1e-9 - self._prev_publish_t + self._kf.predict(dt=dt) + self._kf.update(pos[0], pos[1], pos[2]) + pos = self._kf.position() + + self._last_fix = pos + self._has_fix = True + self._fix_age = 0.0 + state = "ok" if len(used_ids) >= min_anch else "degraded" + + except (ValueError, RuntimeError) as exc: + self.get_logger().warning(f"Trilateration failed: {exc}") + state = "degraded" + + elif self._has_fix: + # KF predict-only: coast on last known position + dt = now_ros.nanoseconds * 1e-9 - self._prev_publish_t + self._kf.predict(dt=dt) + pos = self._kf.position() + self._fix_age += 1.0 / self._publish_rate + state = "degraded" if self._fix_age < 5.0 else "no_fix" + + self._prev_publish_t = now_ros.nanoseconds * 1e-9 + + if pos is None: + self._publish_status(state, 0, [], {}) + return + + x, y, z = pos + + # ── Publish PoseStamped ──────────────────────────────────────────── + pose_msg = PoseStamped() + pose_msg.header.stamp = now_ros.to_msg() + pose_msg.header.frame_id = self._map_frame + pose_msg.pose.position.x = x + pose_msg.pose.position.y = y + pose_msg.pose.position.z = z + pose_msg.pose.orientation.w = 1.0 # no orientation from UWB alone + self._pose_pub.publish(pose_msg) + + # ── TF2: uwb_link → map ─────────────────────────────────────────── + tf_msg = TransformStamped() + tf_msg.header.stamp = now_ros.to_msg() + tf_msg.header.frame_id = self._map_frame + tf_msg.child_frame_id = self._uwb_frame + tf_msg.transform.translation.x = x + tf_msg.transform.translation.y = y + tf_msg.transform.translation.z = z + tf_msg.transform.rotation.w = 1.0 + self._tf_broadcaster.sendTransform(tf_msg) + + # ── Diagnostics ─────────────────────────────────────────────────── + self._publish_status(state, len(used_ids), used_ids, res_map) + + # ── Status publisher ────────────────────────────────────────────────────── + + def _publish_status( + self, + state: str, + n_anchors: int, + used_ids: List[int], + res_map: Dict[int, float], + ) -> None: + # Flag anchors with too many consecutive outlier strikes + flagged = [ + aid for aid, s in self._strikes.items() if s >= self._strikes_max + ] + pos = self._kf.position() if self._has_fix else None + status = { + "state": state, + "active_anchors": n_anchors, + "used_anchor_ids": used_ids, + "flagged_anchors": flagged, + "position": { + "x": round(pos[0], 3), + "y": round(pos[1], 3), + "z": round(pos[2], 3), + } if pos else None, + "residuals_m": res_map, + "fix_age_s": round(self._fix_age, 2), + } + self._status_pub.publish(String(data=json.dumps(status))) + + +# ── Entry point ─────────────────────────────────────────────────────────────── + +def main(args=None) -> None: + rclpy.init(args=args) + node = UwbPositionNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/setup.cfg b/jetson/ros2_ws/src/saltybot_uwb_position/setup.cfg new file mode 100644 index 0000000..b6b785a --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_position/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/saltybot_uwb_position + +[install] +install_scripts=$base/lib/saltybot_uwb_position diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/setup.py b/jetson/ros2_ws/src/saltybot_uwb_position/setup.py new file mode 100644 index 0000000..979af97 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_position/setup.py @@ -0,0 +1,32 @@ +import os +from glob import glob +from setuptools import setup + +package_name = "saltybot_uwb_position" + +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="ROS2 UWB position node — JSON serial bridge with trilateration (Issue #546)", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "uwb_position = saltybot_uwb_position.uwb_position_node:main", + ], + }, +) diff --git a/ui/diagnostics_panel.css b/ui/diagnostics_panel.css new file mode 100644 index 0000000..e73c624 --- /dev/null +++ b/ui/diagnostics_panel.css @@ -0,0 +1,230 @@ +/* diagnostics_panel.css — Saltybot System Diagnostics Dashboard (Issue #562) */ + +*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; } + +:root { + --bg0: #050510; + --bg1: #070712; + --bg2: #0a0a1a; + --border: #0c2a3a; + --border2: #1e3a5f; + --text-dim: #374151; + --text-mid: #6b7280; + --text-base: #9ca3af; + --text-hi: #d1d5db; + --cyan: #06b6d4; + --cyan-dim: #0e4f69; + --green: #22c55e; + --amber: #f59e0b; + --red: #ef4444; + --orange: #f97316; +} + +body { + font-family: 'Courier New', Courier, monospace; + font-size: 12px; + background: var(--bg0); + color: var(--text-base); + min-height: 100dvh; + display: flex; + flex-direction: column; +} + +/* ── Header ── */ +#header { + display: flex; + align-items: center; + justify-content: space-between; + padding: 6px 16px; + background: var(--bg1); + border-bottom: 1px solid var(--border); + flex-shrink: 0; + flex-wrap: wrap; + gap: 8px; +} +.logo { color: #f97316; font-weight: bold; letter-spacing: 0.15em; font-size: 13px; } + +#conn-bar { display: flex; align-items: center; gap: 6px; } +#conn-dot { + width: 8px; height: 8px; border-radius: 50%; + background: var(--text-dim); flex-shrink: 0; transition: background 0.3s; +} +#conn-dot.connected { background: var(--green); } +#conn-dot.error { background: var(--red); animation: blink 1s infinite; } + +@keyframes blink { 0%,100% { opacity:1; } 50% { opacity:0.4; } } + +#ws-input { + background: var(--bg2); border: 1px solid var(--border2); border-radius: 4px; + color: #67e8f9; padding: 2px 8px; font-family: monospace; font-size: 11px; width: 200px; +} +#ws-input:focus { outline: none; border-color: var(--cyan); } + +.hdr-btn { + padding: 3px 10px; border-radius: 4px; border: 1px solid var(--border2); + background: var(--bg2); color: #67e8f9; font-family: monospace; + font-size: 10px; font-weight: bold; cursor: pointer; transition: background 0.15s; +} +.hdr-btn:hover { background: var(--cyan-dim); } + +#refresh-info { + font-size: 10px; color: var(--text-mid); + display: flex; align-items: center; gap: 4px; +} +#refresh-dot { + width: 6px; height: 6px; border-radius: 50%; + background: var(--text-dim); transition: background 0.2s; +} +#refresh-dot.pulse { background: var(--cyan); animation: blink 0.4s; } + +/* ── Status bar ── */ +#status-bar { + display: flex; gap: 8px; align-items: center; + padding: 4px 16px; background: var(--bg1); + border-bottom: 1px solid var(--border); font-size: 10px; flex-wrap: wrap; +} +.sys-badge { + padding: 2px 8px; border-radius: 3px; font-weight: bold; + border: 1px solid; letter-spacing: 0.05em; +} +.badge-ok { background: #052e16; border-color: #166534; color: #4ade80; } +.badge-warn { background: #451a03; border-color: #92400e; color: #fcd34d; } +.badge-error { background: #450a0a; border-color: #991b1b; color: #f87171; animation: blink 1s infinite; } +.badge-stale { background: #111827; border-color: #374151; color: #6b7280; } +#last-update { color: var(--text-mid); margin-left: auto; } + +/* ── Dashboard grid ── */ +#dashboard { + flex: 1; + display: grid; + grid-template-columns: repeat(3, 1fr); + grid-template-rows: auto; + gap: 12px; + padding: 12px; + align-content: start; + overflow-y: auto; +} + +@media (max-width: 1024px) { #dashboard { grid-template-columns: repeat(2, 1fr); } } +@media (max-width: 640px) { #dashboard { grid-template-columns: 1fr; } } + +/* Spanning cards */ +.span2 { grid-column: span 2; } +.span3 { grid-column: 1 / -1; } + +/* ── Card ── */ +.card { + background: var(--bg1); + border: 1px solid var(--border); + border-radius: 8px; + padding: 10px 12px; + display: flex; + flex-direction: column; + gap: 8px; +} +.card.alert-warn { border-color: #92400e; } +.card.alert-error { border-color: #991b1b; } + +.card-title { + font-size: 9px; font-weight: bold; letter-spacing: 0.15em; + color: #0891b2; text-transform: uppercase; + display: flex; align-items: center; justify-content: space-between; +} +.card-title .badge { font-size: 9px; padding: 1px 6px; border-radius: 3px; } + +/* ── Gauge bar ── */ +.gauge-row { + display: flex; flex-direction: column; gap: 3px; +} +.gauge-label-row { + display: flex; justify-content: space-between; font-size: 10px; +} +.gauge-label { color: var(--text-mid); } +.gauge-value { font-family: monospace; } +.gauge-track { + width: 100%; height: 8px; + background: var(--bg2); border-radius: 4px; + overflow: hidden; border: 1px solid var(--border2); +} +.gauge-fill { + height: 100%; border-radius: 4px; + transition: width 0.5s ease, background 0.5s ease; +} + +/* ── Big metric ── */ +.big-metric { + display: flex; align-items: baseline; gap: 4px; +} +.big-num { font-size: 28px; font-weight: bold; font-family: monospace; } +.big-unit { font-size: 11px; color: var(--text-mid); } + +/* ── Sparkline ── */ +#batt-sparkline { + width: 100%; border-radius: 4px; + border: 1px solid var(--border2); + background: var(--bg2); + display: block; +} + +/* ── Node list ── */ +.node-row { + display: flex; align-items: center; justify-content: space-between; + padding: 3px 0; border-bottom: 1px solid var(--border); + font-size: 10px; +} +.node-row:last-child { border-bottom: none; } +.node-name { color: var(--text-base); font-family: monospace; } +.node-status { + padding: 1px 6px; border-radius: 3px; font-size: 9px; font-weight: bold; +} +.ns-ok { background: #052e16; color: #4ade80; } +.ns-warn { background: #451a03; color: #fcd34d; } +.ns-error { background: #450a0a; color: #f87171; } +.ns-stale { background: #111827; color: #6b7280; } + +/* ── Temp arc display ── */ +.temp-grid { + display: grid; grid-template-columns: repeat(2, 1fr); gap: 6px; +} +.temp-box { + background: var(--bg2); border: 1px solid var(--border2); + border-radius: 6px; padding: 6px; text-align: center; +} +.temp-label { font-size: 9px; color: var(--text-mid); margin-bottom: 2px; } +.temp-value { font-size: 22px; font-weight: bold; font-family: monospace; } +.temp-bar-track { + margin-top: 4px; width: 100%; height: 4px; + background: var(--bg0); border-radius: 2px; overflow: hidden; +} +.temp-bar-fill { height: 100%; border-radius: 2px; transition: width 0.5s; } + +/* ── Motor section ── */ +.motor-grid { + display: grid; grid-template-columns: repeat(2, 1fr); gap: 8px; +} +.motor-box { + background: var(--bg2); border: 1px solid var(--border2); + border-radius: 6px; padding: 8px; +} +.motor-label { font-size: 9px; color: var(--text-mid); margin-bottom: 4px; } + +/* ── WiFi bars ── */ +.rssi-bars { display: flex; align-items: flex-end; gap: 3px; } +.rssi-bar { width: 6px; border-radius: 2px 2px 0 0; } + +/* ── MQTT indicator ── */ +.mqtt-status { display: flex; align-items: center; gap: 6px; } +.mqtt-dot { + width: 10px; height: 10px; border-radius: 50%; + background: var(--text-dim); transition: background 0.3s; +} +.mqtt-dot.connected { background: var(--green); animation: none; } +.mqtt-dot.disconnected { background: var(--red); animation: blink 1s infinite; } + +/* ── Footer ── */ +#footer { + background: var(--bg1); border-top: 1px solid var(--border); + padding: 4px 16px; + display: flex; align-items: center; justify-content: space-between; + flex-shrink: 0; font-size: 10px; color: var(--text-dim); +} diff --git a/ui/diagnostics_panel.html b/ui/diagnostics_panel.html new file mode 100644 index 0000000..8aa5417 --- /dev/null +++ b/ui/diagnostics_panel.html @@ -0,0 +1,267 @@ + + + + + +Saltybot — System Diagnostics + + + + + + + + + +
+ SYSTEM + STALE + │ + BATTERY + — + │ + THERMAL + — + │ + NETWORK + — + Awaiting data… +
+ + +
+ + +
+
+ BATTERY — 4S LiPo (12.0–16.8 V) + — +
+ + +
+
+
VOLTAGE
+
+ — +
+
+
+
SOC
+
+ — +
+
+
+
CURRENT
+
+ — +
+
+
+ + +
+
+ Voltage + 12.0–16.8 V +
+
+
+
+
+ State of Charge + 0–100% +
+
+
+ + +
+
VOLTAGE HISTORY (last 2 min)
+ +
+
+ + +
+
TEMPERATURES
+
+
+
CPU (Jetson)
+
—
+
+
+
+
GPU (Jetson)
+
—
+
+
+
+
Board / STM32
+
—
+
+
+
+
Motor L
+
—
+
+
+ +
+
Motor R
+
—
+
+
+
+
+ Zones: <60°C green · 60–75°C amber · >75°C red +
+
+ + +
+
MOTOR CURRENT & CMD
+
+
+
LEFT WHEEL
+
—
+
+
+
+
+ CMD: — +
+
+
+
RIGHT WHEEL
+
—
+
+
+
+
+ CMD: — +
+
+
+
+ Balance state: — +
+
Thresholds: warn 8A · crit 12A
+
+ + +
+
RESOURCES
+
+
+ RAM + — +
+
+
+
+
+ GPU Memory + — +
+
+
+
+
+ Disk + — +
+
+
+
Warn ≥80% · Critical ≥95%
+
+ + +
+
NETWORK
+
+
+
+
RSSI
+
—
+
—
+
+
+
+
+ Rosbridge Latency + — +
+
+ +
+
MQTT BROKER
+
+
+ No data +
+
+ Via /diagnostics KeyValue: mqtt_connected +
+
+
+ + +
+
+ ROS2 NODE HEALTH — /diagnostics + 0 nodes +
+
+
+ Waiting for /diagnostics data… +
+
+
+ +
+ + + + + + + + diff --git a/ui/diagnostics_panel.js b/ui/diagnostics_panel.js new file mode 100644 index 0000000..616c38c --- /dev/null +++ b/ui/diagnostics_panel.js @@ -0,0 +1,592 @@ +/** + * diagnostics_panel.js — Saltybot System Diagnostics Dashboard (Issue #562) + * + * Connects to rosbridge WebSocket and subscribes to: + * /diagnostics diagnostic_msgs/DiagnosticArray — everything + * /saltybot/balance_state std_msgs/String (JSON) — motor cmd / state + * + * Diagnostic KeyValues decoded: + * battery_voltage_v, battery_current_a, battery_soc_pct + * cpu_temp_c, gpu_temp_c, board_temp_c + * motor_temp_l_c, motor_temp_r_c + * motor_current_l_a, motor_current_r_a + * ram_used_mb, ram_total_mb + * gpu_used_mb, gpu_total_mb + * disk_used_gb, disk_total_gb + * wifi_rssi_dbm, wifi_latency_ms + * mqtt_connected + * + * Auto-refresh: rosbridge subscriptions push at ~2 Hz; UI polls canvas draws + * at the same rate and updates DOM elements. + */ + +'use strict'; + +// ── Constants ───────────────────────────────────────────────────────────────── + +const LIPO_MIN = 12.0; // 4S empty +const LIPO_MAX = 16.8; // 4S full + +const BATT_HISTORY_MAX = 120; // ~2 min at 1 Hz + +// Alert thresholds +const THRESHOLDS = { + voltage_warn: 13.6, + voltage_crit: 13.0, + soc_warn: 25, + soc_crit: 10, + cpu_temp_warn: 75, + cpu_temp_crit: 90, + gpu_temp_warn: 75, + gpu_temp_crit: 90, + motor_temp_warn: 70, + motor_temp_crit: 85, + ram_pct_warn: 80, + ram_pct_crit: 95, + disk_pct_warn: 80, + disk_pct_crit: 95, + rssi_warn: -70, + rssi_crit: -80, + latency_warn: 150, + latency_crit: 500, + current_warn: 8.0, + current_crit: 12.0, +}; + +// ── State ───────────────────────────────────────────────────────────────────── + +let ros = null; +let diagSub = null; +let balanceSub = null; + +const state = { + // Battery + voltage: null, + current: null, + soc: null, + battHistory: [], // [{ts, v}] + + // Temperatures + cpuTemp: null, + gpuTemp: null, + boardTemp: null, + motorTempL: null, + motorTempR: null, + + // Motor + motorCurrentL: null, + motorCurrentR: null, + motorCmdL: null, + motorCmdR: null, + balanceState: 'UNKNOWN', + + // Resources + ramUsed: null, ramTotal: null, + gpuUsed: null, gpuTotal: null, + diskUsed: null, diskTotal: null, + + // Network + rssi: null, + latency: null, + + // MQTT + mqttConnected: null, + + // ROS nodes (DiagnosticStatus array) + nodes: [], + + // Overall health + overallLevel: 3, // 3=STALE by default + lastUpdate: null, +}; + +// ── Utility ─────────────────────────────────────────────────────────────────── + +function numOrNull(s) { + const n = parseFloat(s); + return isNaN(n) ? null : n; +} + +function pct(used, total) { + if (!total || total === 0) return 0; + return Math.min(100, Math.max(0, (used / total) * 100)); +} + +function socFromVoltage(v) { + if (v == null || v <= 0) return null; + return Math.max(0, Math.min(100, ((v - LIPO_MIN) / (LIPO_MAX - LIPO_MIN)) * 100)); +} + +function threshColor(value, warnThr, critThr, invert = false) { + // invert: lower is worse (e.g. voltage, SOC, RSSI) + if (value == null) return '#6b7280'; + const warn = invert ? value <= warnThr : value >= warnThr; + const crit = invert ? value <= critThr : value >= critThr; + if (crit) return '#ef4444'; + if (warn) return '#f59e0b'; + return '#22c55e'; +} + +function levelClass(level) { + return ['ns-ok', 'ns-warn', 'ns-error', 'ns-stale'][level] ?? 'ns-stale'; +} + +function levelLabel(level) { + return ['OK', 'WARN', 'ERROR', 'STALE'][level] ?? 'STALE'; +} + +function setBadgeClass(el, level) { + el.className = 'sys-badge ' + ['badge-ok','badge-warn','badge-error','badge-stale'][level ?? 3]; +} + +// ── Connection ──────────────────────────────────────────────────────────────── + +function connect() { + const url = document.getElementById('ws-input').value.trim(); + if (!url) return; + if (ros) ros.close(); + + ros = new ROSLIB.Ros({ url }); + + ros.on('connection', () => { + document.getElementById('conn-dot').className = 'connected'; + document.getElementById('conn-label').textContent = url; + setupSubscriptions(); + }); + + ros.on('error', (err) => { + document.getElementById('conn-dot').className = 'error'; + document.getElementById('conn-label').textContent = 'ERROR: ' + (err?.message || err); + teardown(); + }); + + ros.on('close', () => { + document.getElementById('conn-dot').className = ''; + document.getElementById('conn-label').textContent = 'Disconnected'; + teardown(); + }); +} + +function setupSubscriptions() { + // /diagnostics — throttle 500ms → ~2 Hz + diagSub = new ROSLIB.Topic({ + ros, name: '/diagnostics', + messageType: 'diagnostic_msgs/DiagnosticArray', + throttle_rate: 500, + }); + diagSub.subscribe(onDiagnostics); + + // /saltybot/balance_state + balanceSub = new ROSLIB.Topic({ + ros, name: '/saltybot/balance_state', + messageType: 'std_msgs/String', + throttle_rate: 500, + }); + balanceSub.subscribe(onBalanceState); +} + +function teardown() { + if (diagSub) { diagSub.unsubscribe(); diagSub = null; } + if (balanceSub) { balanceSub.unsubscribe(); balanceSub = null; } + state.overallLevel = 3; + state.lastUpdate = null; + render(); +} + +// ── Message handlers ────────────────────────────────────────────────────────── + +function onDiagnostics(msg) { + const statuses = msg.status ?? []; + state.nodes = []; + let overallLevel = 0; + + for (const status of statuses) { + const kv = {}; + for (const { key, value } of (status.values ?? [])) { + kv[key] = value; + } + + // Battery + if ('battery_voltage_v' in kv) state.voltage = numOrNull(kv.battery_voltage_v); + if ('battery_current_a' in kv) state.current = numOrNull(kv.battery_current_a); + if ('battery_soc_pct' in kv) state.soc = numOrNull(kv.battery_soc_pct); + + // Temperatures + if ('cpu_temp_c' in kv) state.cpuTemp = numOrNull(kv.cpu_temp_c); + if ('gpu_temp_c' in kv) state.gpuTemp = numOrNull(kv.gpu_temp_c); + if ('board_temp_c' in kv) state.boardTemp = numOrNull(kv.board_temp_c); + if ('motor_temp_l_c' in kv) state.motorTempL = numOrNull(kv.motor_temp_l_c); + if ('motor_temp_r_c' in kv) state.motorTempR = numOrNull(kv.motor_temp_r_c); + + // Motor current + if ('motor_current_l_a' in kv) state.motorCurrentL = numOrNull(kv.motor_current_l_a); + if ('motor_current_r_a' in kv) state.motorCurrentR = numOrNull(kv.motor_current_r_a); + + // Resources + if ('ram_used_mb' in kv) state.ramUsed = numOrNull(kv.ram_used_mb); + if ('ram_total_mb' in kv) state.ramTotal = numOrNull(kv.ram_total_mb); + if ('gpu_used_mb' in kv) state.gpuUsed = numOrNull(kv.gpu_used_mb); + if ('gpu_total_mb' in kv) state.gpuTotal = numOrNull(kv.gpu_total_mb); + if ('disk_used_gb' in kv) state.diskUsed = numOrNull(kv.disk_used_gb); + if ('disk_total_gb' in kv) state.diskTotal = numOrNull(kv.disk_total_gb); + + // Network + if ('wifi_rssi_dbm' in kv) state.rssi = numOrNull(kv.wifi_rssi_dbm); + if ('wifi_latency_ms' in kv) state.latency = numOrNull(kv.wifi_latency_ms); + + // MQTT + if ('mqtt_connected' in kv) state.mqttConnected = kv.mqtt_connected === 'true'; + + // Node health + state.nodes.push({ + name: status.name || status.hardware_id || 'unknown', + level: status.level ?? 3, + message: status.message || '', + }); + + overallLevel = Math.max(overallLevel, status.level ?? 0); + } + + state.overallLevel = overallLevel; + state.lastUpdate = new Date(); + + // Battery history + if (state.voltage != null) { + state.battHistory.push({ ts: Date.now(), v: state.voltage }); + if (state.battHistory.length > BATT_HISTORY_MAX) { + state.battHistory.shift(); + } + } + + // Derive SOC from voltage if not provided + if (state.soc == null && state.voltage != null) { + state.soc = socFromVoltage(state.voltage); + } + + render(); + flashRefreshDot(); +} + +function onBalanceState(msg) { + try { + const data = JSON.parse(msg.data); + state.balanceState = data.state ?? 'UNKNOWN'; + state.motorCmdL = data.motor_cmd ?? null; + state.motorCmdR = data.motor_cmd ?? null; // single motor_cmd field + } catch (_) {} +} + +// ── Refresh indicator ───────────────────────────────────────────────────────── + +function flashRefreshDot() { + const dot = document.getElementById('refresh-dot'); + dot.classList.add('pulse'); + setTimeout(() => dot.classList.remove('pulse'), 400); +} + +// ── Sparkline canvas ────────────────────────────────────────────────────────── + +function drawSparkline() { + const canvas = document.getElementById('batt-sparkline'); + if (!canvas) return; + const ctx = canvas.getContext('2d'); + const W = canvas.width = canvas.offsetWidth || 300; + const H = canvas.height; + + ctx.clearRect(0, 0, W, H); + ctx.fillStyle = '#0a0a1a'; + ctx.fillRect(0, 0, W, H); + + const data = state.battHistory; + if (data.length < 2) { + ctx.fillStyle = '#374151'; + ctx.font = '10px Courier New'; + ctx.textAlign = 'center'; + ctx.fillText('Awaiting battery data…', W / 2, H / 2 + 4); + return; + } + + const vMin = LIPO_MIN - 0.2; + const vMax = LIPO_MAX + 0.2; + + // Grid lines + ctx.strokeStyle = '#1e3a5f'; + ctx.lineWidth = 0.5; + [0.25, 0.5, 0.75].forEach((f) => { + const y = H - f * H; + ctx.beginPath(); ctx.moveTo(0, y); ctx.lineTo(W, y); ctx.stroke(); + }); + + // Plot + ctx.lineWidth = 1.5; + ctx.beginPath(); + data.forEach((pt, i) => { + const x = (i / (data.length - 1)) * W; + const y = H - ((pt.v - vMin) / (vMax - vMin)) * H; + i === 0 ? ctx.moveTo(x, y) : ctx.lineTo(x, y); + }); + + const lastV = data[data.length - 1].v; + ctx.strokeStyle = threshColor(lastV, THRESHOLDS.voltage_warn, THRESHOLDS.voltage_crit, true); + ctx.stroke(); + + // Fill under + ctx.lineTo(W, H); ctx.lineTo(0, H); ctx.closePath(); + ctx.fillStyle = 'rgba(6,182,212,0.06)'; + ctx.fill(); + + // Labels + ctx.fillStyle = '#374151'; + ctx.font = '9px Courier New'; + ctx.textAlign = 'left'; + ctx.fillText(`${LIPO_MAX}V`, 2, 10); + ctx.fillText(`${LIPO_MIN}V`, 2, H - 2); +} + +// ── Gauge helpers ───────────────────────────────────────────────────────────── + +function setGauge(barId, pctVal, color) { + const el = document.getElementById(barId); + if (!el) return; + el.style.width = `${Math.max(0, Math.min(100, pctVal))}%`; + el.style.background = color; +} + +function setText(id, text) { + const el = document.getElementById(id); + if (el) el.textContent = text ?? '—'; +} + +function setColor(id, color) { + const el = document.getElementById(id); + if (el) el.style.color = color; +} + +function setTempBox(id, tempC, warnT, critT) { + const box = document.getElementById(id + '-box'); + const val = document.getElementById(id + '-val'); + const bar = document.getElementById(id + '-bar'); + const color = threshColor(tempC, warnT, critT); + + if (val) { val.textContent = tempC != null ? tempC.toFixed(0) + '°C' : '—'; val.style.color = color; } + if (bar) { + const p = tempC != null ? Math.min(100, (tempC / 100) * 100) : 0; + bar.style.width = p + '%'; + bar.style.background = color; + } + if (box && tempC != null) { + box.style.borderColor = tempC >= critT ? '#991b1b' : tempC >= warnT ? '#92400e' : '#1e3a5f'; + } +} + +// ── RSSI bars ───────────────────────────────────────────────────────────────── + +function rssiToLevel(dBm) { + if (dBm == null) return { label: 'N/A', color: '#374151', bars: 0 }; + if (dBm >= -50) return { label: 'Excellent', color: '#22c55e', bars: 5 }; + if (dBm >= -60) return { label: 'Good', color: '#3b82f6', bars: 4 }; + if (dBm >= -70) return { label: 'Fair', color: '#f59e0b', bars: 3 }; + if (dBm >= -80) return { label: 'Weak', color: '#ef4444', bars: 2 }; + return { label: 'Poor', color: '#7f1d1d', bars: 1 }; +} + +function drawRssiBars() { + const container = document.getElementById('rssi-bars'); + if (!container) return; + const lv = rssiToLevel(state.rssi); + const BAR_H = [4, 8, 12, 16, 20]; + + container.innerHTML = ''; + for (let i = 0; i < 5; i++) { + const bar = document.createElement('div'); + bar.className = 'rssi-bar'; + bar.style.height = BAR_H[i] + 'px'; + bar.style.width = '7px'; + bar.style.background = i < lv.bars ? lv.color : '#1f2937'; + container.appendChild(bar); + } +} + +// ── Node list renderer ──────────────────────────────────────────────────────── + +function renderNodes() { + const container = document.getElementById('node-list'); + if (!container) return; + + if (state.nodes.length === 0) { + container.innerHTML = '
No /diagnostics data yet
'; + return; + } + + container.innerHTML = state.nodes.map((n) => ` +
+ ${n.name} +
+ ${n.message ? `${n.message}` : ''} + ${levelLabel(n.level)} +
+
+ `).join(''); +} + +// ── Overall status bar ──────────────────────────────────────────────────────── + +function updateStatusBar() { + const lvl = state.overallLevel; + const badge = document.getElementById('system-badge'); + const labels = ['HEALTHY', 'DEGRADED', 'FAULT', 'STALE']; + if (badge) { + badge.textContent = labels[lvl] ?? 'STALE'; + setBadgeClass(badge, lvl); + } + + // Individual badges + updateBattBadge(); + updateTempBadge(); + updateNetBadge(); + + const upd = document.getElementById('last-update'); + if (upd && state.lastUpdate) { + upd.textContent = 'Updated ' + state.lastUpdate.toLocaleTimeString(); + } +} + +function updateBattBadge() { + const el = document.getElementById('batt-badge'); + if (!el) return; + const v = state.voltage; + if (v == null) { el.textContent = 'NO DATA'; setBadgeClass(el, 3); return; } + if (v <= THRESHOLDS.voltage_crit) { el.textContent = 'CRITICAL'; setBadgeClass(el, 2); } + else if (v <= THRESHOLDS.voltage_warn) { el.textContent = 'LOW'; setBadgeClass(el, 1); } + else { el.textContent = 'OK'; setBadgeClass(el, 0); } +} + +function updateTempBadge() { + const el = document.getElementById('temp-badge'); + if (!el) return; + const temps = [state.cpuTemp, state.gpuTemp, state.motorTempL, state.motorTempR].filter(t => t != null); + if (temps.length === 0) { el.textContent = 'NO DATA'; setBadgeClass(el, 3); return; } + const max = Math.max(...temps); + if (max >= THRESHOLDS.cpu_temp_crit) { el.textContent = 'CRITICAL'; setBadgeClass(el, 2); } + else if (max >= THRESHOLDS.cpu_temp_warn) { el.textContent = 'HOT'; setBadgeClass(el, 1); } + else { el.textContent = 'OK'; setBadgeClass(el, 0); } +} + +function updateNetBadge() { + const el = document.getElementById('net-badge'); + if (!el) return; + const r = state.rssi; + if (r == null) { el.textContent = 'NO DATA'; setBadgeClass(el, 3); return; } + if (r <= THRESHOLDS.rssi_crit) { el.textContent = 'POOR'; setBadgeClass(el, 2); } + else if (r <= THRESHOLDS.rssi_warn) { el.textContent = 'WEAK'; setBadgeClass(el, 1); } + else { el.textContent = 'OK'; setBadgeClass(el, 0); } +} + +// ── Main render ─────────────────────────────────────────────────────────────── + +function render() { + // ── Battery ── + const v = state.voltage; + const soc = state.soc ?? socFromVoltage(v); + const vColor = threshColor(v, THRESHOLDS.voltage_warn, THRESHOLDS.voltage_crit, true); + const sColor = threshColor(soc, THRESHOLDS.soc_warn, THRESHOLDS.soc_crit, true); + + setText('batt-voltage', v != null ? v.toFixed(2) + ' V' : '—'); + setColor('batt-voltage', vColor); + setText('batt-soc', soc != null ? soc.toFixed(0) + ' %' : '—'); + setColor('batt-soc', sColor); + setText('batt-current', state.current != null ? state.current.toFixed(2) + ' A' : '—'); + + setGauge('batt-soc-bar', soc ?? 0, sColor); + setGauge('batt-volt-bar', v != null ? ((v - LIPO_MIN) / (LIPO_MAX - LIPO_MIN)) * 100 : 0, vColor); + + drawSparkline(); + + // ── Temperatures ── + setTempBox('cpu-temp', state.cpuTemp, THRESHOLDS.cpu_temp_warn, THRESHOLDS.cpu_temp_crit); + setTempBox('gpu-temp', state.gpuTemp, THRESHOLDS.gpu_temp_warn, THRESHOLDS.gpu_temp_crit); + setTempBox('board-temp', state.boardTemp, 60, 80); + setTempBox('motor-temp-l', state.motorTempL, THRESHOLDS.motor_temp_warn, THRESHOLDS.motor_temp_crit); + setTempBox('motor-temp-r', state.motorTempR, THRESHOLDS.motor_temp_warn, THRESHOLDS.motor_temp_crit); + + // ── Motor current ── + const curL = state.motorCurrentL; + const curR = state.motorCurrentR; + const curColorL = threshColor(curL, THRESHOLDS.current_warn, THRESHOLDS.current_crit); + const curColorR = threshColor(curR, THRESHOLDS.current_warn, THRESHOLDS.current_crit); + + setText('motor-cur-l', curL != null ? curL.toFixed(2) + ' A' : '—'); + setColor('motor-cur-l', curColorL); + setText('motor-cur-r', curR != null ? curR.toFixed(2) + ' A' : '—'); + setColor('motor-cur-r', curColorR); + setGauge('motor-bar-l', curL != null ? (curL / THRESHOLDS.current_crit) * 100 : 0, curColorL); + setGauge('motor-bar-r', curR != null ? (curR / THRESHOLDS.current_crit) * 100 : 0, curColorR); + setText('motor-cmd-l', state.motorCmdL != null ? state.motorCmdL.toString() : '—'); + setText('motor-cmd-r', state.motorCmdR != null ? state.motorCmdR.toString() : '—'); + setText('balance-state', state.balanceState); + + // ── Resources ── + const ramPct = pct(state.ramUsed, state.ramTotal); + const gpuPct = pct(state.gpuUsed, state.gpuTotal); + const diskPct = pct(state.diskUsed, state.diskTotal); + + setText('ram-val', + state.ramUsed != null ? `${state.ramUsed.toFixed(0)} / ${state.ramTotal?.toFixed(0) ?? '?'} MB` : '—'); + setGauge('ram-bar', ramPct, threshColor(ramPct, THRESHOLDS.ram_pct_warn, THRESHOLDS.ram_pct_crit)); + + setText('gpu-val', + state.gpuUsed != null ? `${state.gpuUsed.toFixed(0)} / ${state.gpuTotal?.toFixed(0) ?? '?'} MB` : '—'); + setGauge('gpu-bar', gpuPct, threshColor(gpuPct, 70, 90)); + + setText('disk-val', + state.diskUsed != null ? `${state.diskUsed.toFixed(1)} / ${state.diskTotal?.toFixed(1) ?? '?'} GB` : '—'); + setGauge('disk-bar', diskPct, threshColor(diskPct, THRESHOLDS.disk_pct_warn, THRESHOLDS.disk_pct_crit)); + + // ── WiFi ── + const rLevel = rssiToLevel(state.rssi); + setText('rssi-val', state.rssi != null ? state.rssi + ' dBm' : '—'); + setColor('rssi-val', rLevel.color); + setText('rssi-label', rLevel.label); + setColor('rssi-label', rLevel.color); + setText('latency-val', state.latency != null ? state.latency.toFixed(0) + ' ms' : '—'); + setColor('latency-val', threshColor(state.latency, THRESHOLDS.latency_warn, THRESHOLDS.latency_crit)); + drawRssiBars(); + + // ── MQTT ── + const mqttDot = document.getElementById('mqtt-dot'); + const mqttLbl = document.getElementById('mqtt-label'); + if (mqttDot) { + mqttDot.className = 'mqtt-dot ' + (state.mqttConnected ? 'connected' : 'disconnected'); + } + if (mqttLbl) { + mqttLbl.textContent = state.mqttConnected === null ? 'No data' + : state.mqttConnected ? 'Broker connected' + : 'Broker disconnected'; + mqttLbl.style.color = state.mqttConnected ? '#4ade80' : '#f87171'; + } + + // ── Nodes ── + renderNodes(); + + // ── Status bar ── + updateStatusBar(); +} + +// ── Init ────────────────────────────────────────────────────────────────────── + +document.getElementById('btn-connect').addEventListener('click', connect); +document.getElementById('ws-input').addEventListener('keydown', (e) => { + if (e.key === 'Enter') connect(); +}); + +const stored = localStorage.getItem('diag_ws_url'); +if (stored) document.getElementById('ws-input').value = stored; +document.getElementById('ws-input').addEventListener('change', (e) => { + localStorage.setItem('diag_ws_url', e.target.value); +}); + +// Initial render (blank state) +render(); + +// Periodic sparkline resize + redraw on window resize +window.addEventListener('resize', drawSparkline);