seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 5 Actions Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/main' into salty/uwb-tag-display-wireless

Browse Source
This commit is contained in:
salty 2026-03-14 12:11:28 -04:00
commit 66a4bbe25b
23 changed files with 3800 additions and 290 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

628
chassis/uwb_anchor_mount.scad
View File

@ -1,275 +1,463 @@
// ============================================================ // ============================================================
// uwb_anchor_mount.scad — Stem-Mounted UWB Anchor Rev A // uwb_anchor_mount.scad — Wall/Ceiling UWB Anchor Mount Bracket
// Agent: sl-mechanical 2026-03-01 // Issue: #564 Agent: sl-mechanical Date: 2026-03-14
// Closes issues #57, #62 // (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: // Parametric wall or ceiling mount bracket for ESP32 UWB Pro anchor.
// • Split D-collar with M4 clamping bolts + M4 set screw // Designed for fixed-infrastructure deployment: anchors screw into
// • Anti-rotation flat tab that keys against a small pin // wall or ceiling drywall/timber with standard M4 or #6 wood screws,
// OR printed key tab that registers on the stem flat (if stem // at a user-defined tilt angle so the UWB antenna faces the desired
// has a ground flat) — see ANTI_ROT_MODE parameter // coverage zone.
// • 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
// //
// Components per mount: // Architecture:
// 2× collar_half print in PLA/PETG, flat-face-down // Wall base → flat backplate with 2× screw holes (wall or ceiling)
// 1× module_bracket print in PLA/PETG, flat-face-down // 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: // RENDER options:
// "assembly" single mount assembled (default) // "assembly" full assembly at TILT_DEG (default)
// "collar_front" front collar half for slicing (×2 per mount × 2 mounts = 4) // "wall_base_stl" Part 1
// "collar_rear" rear collar half // "tilt_arm_stl" Part 2
// "bracket" module bracket (×2 mounts) // "anchor_cradle_stl" Part 3
// "pair" both mounts on 350 mm stem section // "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"; RENDER = "assembly";
// ── ⚠ Verify with calipers ─────────────────────────────────── if (RENDER == "assembly") assembly_preview();
MAWB_L = 50.0; // PCB length else if (RENDER == "wall_base_stl") wall_base();
MAWB_W = 25.0; // PCB width else if (RENDER == "tilt_arm_stl") tilt_arm();
MAWB_H = 10.0; // PCB + components else if (RENDER == "anchor_cradle_stl") anchor_cradle();
MAWB_HOLE_X = 43.0; // M2 mounting hole X span else if (RENDER == "cable_clip_stl") cable_clip();
MAWB_HOLE_Y = 20.0; // M2 mounting hole Y span
M2_D = 2.2; // M2 clearance
// ── Stem ───────────────────────────────────────────────────── // ============================================================
STEM_OD = 25.0; // ASSEMBLY PREVIEW
STEM_BORE = 25.4; // +0.4 clearance // ============================================================
WALL = 2.0; // wall thickness (used in thumbscrew recess) 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 ─────────────────────────────────────────────────── // Wall base
COL_OD = 52.0; color("OliveDrab", 0.85)
COL_H = 30.0; // taller than sensor-head collar for rigidity wall_base();
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;
// Anti-rotation flat tab: a 3 mm wall tab that protrudes radially // Tilt arm at TILT_DEG, pivoting at knuckle
// and bears against the bracket arm, preventing axial rotation color("SteelBlue", 0.85)
// without needing a stem flat. translate([0, KNUCKLE_T, 0])
ANTI_ROT_T = 3.0; // tab thickness (radial) rotate([TILT_DEG, 0, 0])
ANTI_ROT_W = 8.0; // tab width (tangential) tilt_arm();
ANTI_ROT_Z = 4.0; // distance from collar base
// USB cable channel: groove on collar outer surface, runs Z direction // Anchor cradle at arm end
// Cable routes from anchor module down to base color("DarkSlateGray", 0.85)
USB_CHAN_W = 9.0; // channel width (fits USB-A cable Ø6 mm) translate([0, KNUCKLE_T, 0])
USB_CHAN_D = 5.0; // channel depth rotate([TILT_DEG, 0, 0])
translate([0, ARM_T, ARM_L])
anchor_cradle();
// ── Module bracket ─────────────────────────────────────────── // PCB ghost
ARM_L = 20.0; // arm length from collar OD to bracket face %color("ForestGreen", 0.38)
ARM_W = MAWB_W + 6.0; // bracket width (Y, includes side walls) translate([0, KNUCKLE_T, 0])
ARM_H = 6.0; // arm thickness (Z) rotate([TILT_DEG, 0, 0])
BRKT_TILT = 10.0; // tilt outward from vertical (antenna faces horizon) 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) // Cable clip at arm mid-point
BRKT_SIDE_T = 2.0; // bracket side walls 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; // PART 1 — WALL BASE
// ============================================================
// USB port access notch in bracket side wall (8×5 mm) // Flat backplate screws to wall or ceiling with 2× countersunk
USB_NOTCH_W = 10.0; // M4/#6 wood screws on BASE_SCREW_SPC (35 mm) centres.
USB_NOTCH_H = 7.0; // Two pivot ears straddle the tilt arm; M3 pivot bolt passes through
// both ears and arm knuckle.
// ── Spacing ─────────────────────────────────────────────────── // Detent arc on inner face of +X ear: 7 notches at 15° steps (0–90°)
ANCHOR_SPACING = 250.0; // centre-to-centre Z separation // so tilt can be set without a protractor.
// Shallow rear recess accepts a printed installation-zone label.
$fn = 64; //
e = 0.01; // Dual-use: flat face to wall (vertical screw axis) or flat face
// to ceiling (horizontal screw axis) — same part either way.
// ───────────────────────────────────────────────────────────── //
// collar_half(side) // Print: backplate flat on bed, PETG, 5 perims, 40 % gyroid.
// split at Y=0 plane. Bracket arm on front (+Y) half. module wall_base() {
// Print flat-face-down. ear_h = ARM_W + 3.0;
// ───────────────────────────────────────────────────────────── ear_t = 6.0;
module collar_half(side = "front") { ear_sep = ARM_W + 1.0;
y_front = (side == "front");
difference() { difference() {
union() { union() {
// D-shaped body // ── Backplate ────────────────────────────────────────────────
intersection() { translate([-BASE_W/2, -BASE_T, -BASE_H/2])
cylinder(d=COL_OD, h=COL_H); cube([BASE_W, BASE_T, BASE_H]);
translate([-COL_OD/2, y_front ? 0 : -COL_OD/2, 0])
cube([COL_OD, COL_OD/2, COL_H]);
}
// Anti-rotation tab (front half only, at +X side) // ── Two pivot ears ────────────────────────────────────────────
if (y_front) { for (ex = [-(ear_sep/2 + ear_t), ear_sep/2])
translate([COL_OD/2, -ANTI_ROT_W/2, ANTI_ROT_Z]) translate([ex, -BASE_T + e, -ear_h/2])
cube([ANTI_ROT_T, ANTI_ROT_W, cube([ear_t, KNUCKLE_T + e, ear_h]);
COL_H - ANTI_ROT_Z - 4]);
}
// Bracket arm attachment boss (front half only, top centre) // ── Stiffening gussets ────────────────────────────────────────
if (y_front) { for (ex = [-(ear_sep/2 + ear_t), ear_sep/2])
translate([-ARM_W/2, COL_OD/2, COL_H * 0.3]) hull() {
cube([ARM_W, ARM_L, COL_H * 0.4]); 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 ───────────────────────────────────────── // ── 2× countersunk wall screws ────────────────────────────────────
translate([0,0,-e]) for (sz = [-BASE_SCREW_SPC/2, BASE_SCREW_SPC/2]) {
cylinder(d=STEM_BORE, h=COL_H + 2*e); translate([0, -BASE_T - e, sz])
rotate([-90, 0, 0])
// ── M4 clamping bolt holes (Y direction) ────────────── cylinder(d = BASE_SCREW_D, h = BASE_T + 2*e);
for (bx=[-COL_BOLT_X, COL_BOLT_X]) { translate([0, -BASE_T - e, sz])
translate([bx, y_front ? COL_OD/2 : 0, COL_H/2]) rotate([-90, 0, 0])
rotate([90,0,0]) cylinder(d1 = BASE_SCREW_HD, d2 = BASE_SCREW_D,
cylinder(d=COL_BOLT_D, h=COL_OD/2 + e); h = BASE_SCREW_HH + 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);
}
} }
// ── M4 hex nut pockets (rear half) ──────────────────── // ── Pivot bolt bore (M3, through both ears) ───────────────────────
if (!y_front) { translate([-(ear_sep/2 + ear_t + e), KNUCKLE_T * 0.55, 0])
for (bx=[-COL_BOLT_X, COL_BOLT_X]) { rotate([0, 90, 0])
translate([bx, -(COL_OD/4 + e), COL_H/2]) cylinder(d = PIVOT_D, h = ear_sep + 2*ear_t + 2*e);
rotate([90,0,0])
cylinder(d=COL_NUT_W/cos(30), h=COL_NUT_H + e,
$fn=6);
}
}
// ── Set screw (height lock, front half) ─────────────── // ── M3 nyloc nut pocket (outer face of one ear) ───────────────────
if (y_front) { translate([ear_sep/2 + ear_t - PIVOT_NUT_H - 0.4,
translate([0, COL_OD/2, COL_H * 0.8]) KNUCKLE_T * 0.55, 0])
rotate([90,0,0]) rotate([0, 90, 0])
cylinder(d=COL_BOLT_D, cylinder(d = PIVOT_NUT_AF / cos(30),
h=COL_OD/2 - STEM_BORE/2 + e); h = PIVOT_NUT_H + 0.5, $fn = 6);
}
// ── USB cable routing channel (rear half, −X side) ──── // ── Detent arc — 7 notches at 15° steps on +X ear inner face ─────
if (!y_front) { for (da = [0 : 15 : 90])
translate([-COL_OD/2, -USB_CHAN_W/2, -e]) translate([ear_sep/2 - e,
cube([USB_CHAN_D, USB_CHAN_W, COL_H + 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) ─ // ── Installation label recess (rear face of backplate) ────────────
if (y_front) { translate([0, -BASE_T - e, 0])
for (dx=[-ARM_W/4, ARM_W/4]) rotate([-90, 0, 0])
translate([dx, COL_OD/2 + ARM_L/2, COL_H * 0.35]) cube([BASE_W - 12, BASE_H - 16, 1.6], center = true);
cylinder(d=COL_BOLT_D, h=COL_H * 0.35 + e);
} // ── Lightening pocket ─────────────────────────────────────────────
translate([0, -BASE_T + 1.5, 0])
cube([BASE_W - 14, BASE_T - 3, BASE_H - 20], center = true);
} }
} }
// ───────────────────────────────────────────────────────────── // ============================================================
// module_bracket() // PART 2 — TILT ARM
// Bolts to collar arm boss. Holds MaUWB PCB facing outward. // ============================================================
// Tilted BRKT_TILT° from vertical — antenna clears stem. // Pivoting arm linking wall_base pivot ears to anchor_cradle.
// Print flat-face-down (back wall on bed). // Knuckle end (Z=0): M3 pivot bore + spring-plunger detent pocket
// ───────────────────────────────────────────────────────────── // that indexes into the base ear detent arc notches.
module module_bracket() { // Cradle end (Z=ARM_L): 2× M3 bolt attachment to cradle back wall.
bk = BRKT_BACK_T; // USB-C cable channel runs along outer (+Y) face, full arm length.
sd = BRKT_SIDE_T; //
// Print: knuckle face flat on bed, PETG, 5 perims, 40 % gyroid.
module tilt_arm() {
total_h = ARM_L + 10;
difference() { difference() {
union() { union() {
// ── Back wall (mounts to collar arm boss) ───────── // ── Arm body ─────────────────────────────────────────────────
cube([ARM_W, bk, MAWB_H + M2_STNDFF + 6]); translate([-ARM_W/2, 0, 0])
cube([ARM_W, ARM_T, total_h]);
// ── Side walls ──────────────────────────────────── // ── Knuckle boss (rounded pivot end) ─────────────────────────
for (sx=[0, ARM_W - sd]) translate([0, ARM_T/2, 0])
translate([sx, bk, 0]) rotate([90, 0, 0])
cube([sd, MAWB_L + 2, MAWB_H + M2_STNDFF + 6]); cylinder(d = ARM_W, h = ARM_T, center = true);
// ── M2 standoff posts (PCB mounts to these) ─────── // ── Cradle attach stub (Z = ARM_L) ────────────────────────────
for (hx=[0, MAWB_HOLE_X], hy=[0, MAWB_HOLE_Y]) translate([-ARM_W/2, 0, ARM_L])
translate([(ARM_W - MAWB_HOLE_X)/2 + hx, cube([ARM_W, ARM_T + CRADLE_BACK_T, ARM_T]);
bk + (MAWB_L - MAWB_HOLE_Y)/2 + hy,
0])
cylinder(d=M2_STNDFF_OD, h=M2_STNDFF);
} }
// ── M2 bores through standoffs ──────────────────────── // ── M3 pivot bore ─────────────────────────────────────────────────
for (hx=[0, MAWB_HOLE_X], hy=[0, MAWB_HOLE_Y]) translate([-ARM_W/2 - e, ARM_T/2, 0])
translate([(ARM_W - MAWB_HOLE_X)/2 + hx, rotate([0, 90, 0])
bk + (MAWB_L - MAWB_HOLE_Y)/2 + hy, cylinder(d = PIVOT_D, h = ARM_W + 2*e);
-e])
cylinder(d=M2_D, h=M2_STNDFF + e);
// ── Antenna clearance cutout in back wall ───────────── // ── Detent plunger pocket (3 mm spring-ball, outer +Y face) ──────
// Open slot near top of back wall so antenna is unobstructed translate([0, ARM_T + e, 0])
translate([sd, -e, M2_STNDFF + 2]) rotate([90, 0, 0])
cube([ARM_W - 2*sd, bk + 2*e, MAWB_H]); cylinder(d = 3.2, h = 4 + e);
// ── USB port access notch on one side wall ──────────── // ── USB-C cable channel (outer +Y face, mid-arm length) ───────────
translate([-e, bk + 2, M2_STNDFF - 1]) translate([-USBC_CHAN_W/2, ARM_T - e, ARM_T + 4])
cube([sd + 2*e, USB_NOTCH_W, USB_NOTCH_H]); cube([USBC_CHAN_W, USBC_CHAN_H, ARM_L - ARM_T - 8]);
// ── Mounting holes to collar arm boss (×2) ──────────── // ── Cradle attach bolt holes (2× M3 at cradle stub) ───────────────
for (dx=[-ARM_W/4, ARM_W/4]) for (bx = [-ARM_W/4, ARM_W/4])
translate([ARM_W/2 + dx, bk + ARM_L/2, -e]) translate([bx, ARM_T/2, ARM_L + ARM_T/2])
cylinder(d=COL_BOLT_D, h=6 + e); 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() // PART 3 — ANCHOR CRADLE
// ───────────────────────────────────────────────────────────── // ============================================================
module single_anchor_assembly(show_phantom=false) { // Open-front U-cradle for ESP32 UWB Pro PCB.
// Collar // PCB retained on 4× M2.5 standoffs matching UWB_HOLE_X × UWB_HOLE_Y.
color("SteelBlue", 0.9) collar_half("front"); // Back wall features:
color("CornflowerBlue", 0.9) mirror([0,1,0]) collar_half("rear"); // • 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 difference() {
color("LightSteelBlue", 0.85) union() {
translate([0, COL_OD/2 + ARM_L, COL_H * 0.3]) // ── Cradle body ───────────────────────────────────────────────
rotate([BRKT_TILT, 0, 0]) translate([-outer_l/2, 0, 0])
translate([-ARM_W/2, 0, 0]) cube([outer_l, outer_w, total_z]);
module_bracket();
// Phantom UWB PCB // ── Front retaining lip ───────────────────────────────────────
if (show_phantom) translate([-outer_l/2, outer_w - CRADLE_LIP_T, 0])
color("ForestGreen", 0.4) cube([outer_l, CRADLE_LIP_T, CRADLE_LIP_H]);
translate([-MAWB_L/2,
COL_OD/2 + ARM_L + BRKT_BACK_T, // ── Arm attachment tabs (behind back wall) ─────────────────────
COL_H * 0.3 + M2_STNDFF]) for (tx = [-ARM_W/4, ARM_W/4])
cube([MAWB_L, MAWB_W, MAWB_H]); 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 // PART 4 — CABLE CLIP
// ───────────────────────────────────────────────────────────── // ============================================================
if (RENDER == "assembly") { // Snap-on C-clip retaining USB-C cable along tilt arm outer face.
single_anchor_assembly(show_phantom=true); // 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") { difference() {
collar_half("front"); union() {
// ── Body plate ────────────────────────────────────────────────
translate([-CLIP_BODY_W/2, 0, 0])
cube([CLIP_BODY_W, CLIP_T, CLIP_BODY_H]);
} else if (RENDER == "collar_rear") { // ── Cable channel (C-shape, opens toward +Y) ─────────────────
collar_half("rear"); 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") { // ── Snap tongues (straddle arm, -Y side of body) ─────────────
module_bracket(); 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") { // ── Snap barbs ────────────────────────────────────────────────
// Both anchors at 250 mm spacing on a stem stub for (tx = [-CLIP_BODY_W/2 + 1.5,
color("Silver", 0.2) CLIP_BODY_W/2 - 1.5 - snap_t])
translate([0, 0, -50]) translate([tx + snap_t/2, -ARM_T - 1, CLIP_BODY_H/2])
cylinder(d=STEM_OD, h=ANCHOR_SPACING + COL_H + 100); rotate([0, 90, 0])
cylinder(d = 2, h = snap_t, center = true);
}
// Lower anchor (Z = 0) // ── Arm slot (arm body passes between tongues) ─────────────────────
single_anchor_assembly(show_phantom=true); translate([0, -ARM_T - 1 - e, CLIP_BODY_H/2])
cube([CLIP_BODY_W - 6, ARM_T + 2, CLIP_BODY_H - 4], center = true);
// Upper anchor (Z = ANCHOR_SPACING) }
translate([0, 0, ANCHOR_SPACING])
single_anchor_assembly(show_phantom=true);
} }

30
esp32/uwb_anchor/platformio.ini Normal file
View File

@ -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

413
esp32/uwb_anchor/src/main.cpp Normal file
View File

@ -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:<anchor_id>,<range_mm>,<rssi_dbm>\r\n (on each successful range)
*
* AT commands (host → anchor)
* ───────────────────────────
* AT+RANGE? → returns last buffered +RANGE line
* AT+RANGE_ADDR=<hex_addr> → pair with specific tag (filter others)
* AT+RANGE_ADDR= → clear pairing (accept all tags)
* AT+ID? → returns +ID:<anchor_id>
*
* 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 <Arduino.h>
#include <SPI.h>
#include <math.h>
#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();
}

19
esp32/uwb_anchor/udev/99-uwb-anchors.rules Normal file
View File

@ -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"

113
include/jlink.h
View File

@ -3,9 +3,10 @@
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#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 * Issue #120: replaces jetson_cmd ASCII-over-USB-CDC with a dedicated
* hardware UART at 921600 baud using DMA circular RX and IDLE interrupt. * 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) * 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) * 0x06 DFU_ENTER - no payload; request OTA DFU reboot (denied while armed)
* 0x07 ESTOP - no payload; engage emergency stop * 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) * 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) * 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: * STM32 to Jetson telemetry:
* 0x80 STATUS - jlink_tlm_status_t (20 bytes), sent at JLINK_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 * 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) * 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) * 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 * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
* when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and * 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_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_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_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) ---- */ /* ---- Telemetry IDs (STM32 to Jetson) ---- */
#define JLINK_TLM_STATUS 0x80u #define JLINK_TLM_STATUS 0x80u
#define JLINK_TLM_POWER 0x81u /* jlink_tlm_power_t (11 bytes) */ #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_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_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) ---- */ /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) {
@ -98,15 +110,15 @@ typedef struct __attribute__((packed)) {
uint32_t idle_ms; /* ms since last cmd_vel activity */ uint32_t idle_ms; /* ms since last cmd_vel activity */
} jlink_tlm_power_t; /* 11 bytes */ } 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)) { 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) */ int16_t ibat_ma; /* DMA-sampled LPF-filtered Ibat (mA, + = discharge) */
uint16_t vbat_raw_mv; /* unfiltered last-tick average (mV) */ uint16_t vbat_raw_mv; /* unfiltered last-tick average (mV) */
uint8_t flags; /* bit0=low, bit1=critical, bit2=4S, bit3=adc_ready */ uint8_t flags; /* bit0=low, bit1=critical, bit2=4S, bit3=adc_ready */
int8_t cal_offset; /* vbat_offset_mv / 4 (±127 → ±508 mV) */ int8_t cal_offset; /* vbat_offset_mv / 4 (+-127 -> +-508 mV) */
uint8_t lpf_shift; /* IIR shift factor (α = 1/2^lpf_shift) */ uint8_t lpf_shift; /* IIR shift factor (alpha = 1/2^lpf_shift) */
uint8_t soc_pct; /* voltage-based SoC 0–100, 255 = unknown */ uint8_t soc_pct; /* voltage-based SoC 0-100, 255 = unknown */
} jlink_tlm_battery_t; /* 10 bytes */ } jlink_tlm_battery_t; /* 10 bytes */
/* ---- Telemetry PID_RESULT payload (13 bytes, packed) Issue #531 ---- */ /* ---- 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)) */ uint8_t rx_err_pct; /* bus error rate 0-100% (rx_err*100/(rx_ok+rx_err)) */
} jlink_tlm_gimbal_state_t; /* 10 bytes */ } 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) ---- */ /* ---- Volatile state (read from main loop) ---- */
typedef struct { typedef struct {
/* Drive command - updated on JLINK_CMD_DRIVE */ /* 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_pan_x10; /* pan angle deg x10 */
volatile int16_t gimbal_tilt_x10; /* tilt angle deg x10 */ volatile int16_t gimbal_tilt_x10; /* tilt angle deg x10 */
volatile uint16_t gimbal_speed; /* servo speed 0-4095 (0=max) */ 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; } JLinkState;
extern volatile JLinkState jlink_state; 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 ---- */ /* ---- 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); 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); 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); void jlink_process(void);
void jlink_send_telemetry(const jlink_tlm_status_t *status);
/*
* 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_power_telemetry(const jlink_tlm_power_t *power); 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_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) * 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); 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 */ #endif /* JLINK_H */

66
jetson/ros2_ws/src/saltybot_health_monitor/config/sensor_health_params.yaml Normal file
View File

@ -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

50
jetson/ros2_ws/src/saltybot_health_monitor/launch/sensor_health.launch.py Normal file
View File

@ -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,
])

21
jetson/ros2_ws/src/saltybot_health_monitor/package.xml
View File

@ -1,27 +1,24 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3"> <package format="3">
<name>saltybot_health_monitor</name> <name>saltybot_health_monitor</name>
<version>0.1.0</version> <version>0.2.0</version>
<description> <description>
ROS2 system health monitor for SaltyBot. Central node that monitors heartbeats ROS2 health monitor for SaltyBot. Tracks node heartbeats and sensor topic
from all critical nodes, detects when nodes go down (>5s silent), triggers staleness. Publishes DiagnosticArray on /saltybot/diagnostics and MQTT on
auto-restart, publishes /saltybot/system_health JSON, and alerts face display saltybot/health. Issue #566.
on critical failures.
</description> </description>
<maintainer email="sl-controls@saltylab.local">sl-controls</maintainer> <maintainer email="sl-jetson@saltylab.local">sl-jetson</maintainer>
<license>MIT</license> <license>Apache-2.0</license>
<depend>rclpy</depend> <depend>rclpy</depend>
<depend>std_msgs</depend> <depend>std_msgs</depend>
<depend>geometry_msgs</depend> <depend>geometry_msgs</depend>
<depend>sensor_msgs</depend>
<depend>diagnostic_msgs</depend>
<buildtool_depend>ament_python</buildtool_depend> <buildtool_depend>ament_python</buildtool_depend>
<test_depend>ament_copyright</test_depend> <test_depend>pytest</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export> <export>
<build_type>ament_python</build_type> <build_type>ament_python</build_type>

385
jetson/ros2_ws/src/saltybot_health_monitor/saltybot_health_monitor/sensor_health_node.py Normal file
View File

@ -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()

25
jetson/ros2_ws/src/saltybot_health_monitor/setup.py
View File

@ -4,27 +4,34 @@ package_name = "saltybot_health_monitor"
setup( setup(
name=package_name, name=package_name,
version="0.1.0", version="0.2.0",
packages=[package_name], packages=[package_name],
data_files=[ data_files=[
("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
(f"share/{package_name}", ["package.xml"]), (f"share/{package_name}", ["package.xml"]),
(f"share/{package_name}/launch", ["launch/health_monitor.launch.py"]), (f"share/{package_name}/launch", [
(f"share/{package_name}/config", ["config/health_config.yaml"]), "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, zip_safe=True,
maintainer="sl-controls", maintainer="sl-jetson",
maintainer_email="sl-controls@saltylab.local", maintainer_email="sl-jetson@saltylab.local",
description=( description=(
"System health monitor: tracks node heartbeats, detects down nodes, " "System health monitor: node heartbeats + sensor topic staleness "
"triggers auto-restart, publishes system health status" "detection with DiagnosticArray and MQTT (Issue #566)"
), ),
license="MIT", license="Apache-2.0",
tests_require=["pytest"], tests_require=["pytest"],
entry_points={ entry_points={
"console_scripts": [ "console_scripts": [
"health_monitor_node = saltybot_health_monitor.health_monitor_node:main", "health_monitor_node = saltybot_health_monitor.health_monitor_node:main",
"sensor_health_node = saltybot_health_monitor.sensor_health_node:main",
], ],
}, },
) )

344
jetson/ros2_ws/src/saltybot_health_monitor/test/test_sensor_health.py Normal file
View File

@ -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()

58
jetson/ros2_ws/src/saltybot_uwb_position/config/uwb_position_params.yaml Normal file
View File

@ -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)

35
jetson/ros2_ws/src/saltybot_uwb_position/launch/uwb_position.launch.py Normal file
View File

@ -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])

36
jetson/ros2_ws/src/saltybot_uwb_position/package.xml Normal file
View File

@ -0,0 +1,36 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>saltybot_uwb_position</name>
<version>0.1.0</version>
<description>
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/&lt;id&gt;, JSON diagnostics on /saltybot/uwb/status,
and broadcasts the uwb_link→map TF2 transform.
</description>
<maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
<license>MIT</license>
<buildtool_depend>ament_python</buildtool_depend>
<depend>rclpy</depend>
<depend>geometry_msgs</depend>
<depend>std_msgs</depend>
<depend>tf2_ros</depend>
<depend>saltybot_uwb_msgs</depend>
<!-- numpy is a system dep on Jetson (python3-numpy) -->
<exec_depend>python3-numpy</exec_depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

0
jetson/ros2_ws/src/saltybot_uwb_position/resource/saltybot_uwb_position Normal file
View File

0
jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/__init__.py Normal file
View File

254
jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/trilateration.py Normal file
View File

@ -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

487
jetson/ros2_ws/src/saltybot_uwb_position/saltybot_uwb_position/uwb_position_node.py Normal file
View File

@ -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/<id> (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()

5
jetson/ros2_ws/src/saltybot_uwb_position/setup.cfg Normal file
View File

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

32
jetson/ros2_ws/src/saltybot_uwb_position/setup.py Normal file
View File

@ -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",
],
},
)

230
ui/diagnostics_panel.css Normal file
View File

@ -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);
}

267
ui/diagnostics_panel.html Normal file
View File

@ -0,0 +1,267 @@
<!DOCTYPE html>
<html lang="en">
<head>
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
<title>Saltybot — System Diagnostics</title>
<link rel="stylesheet" href="diagnostics_panel.css">
<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script>
</head>
<body>
<!-- ── Header ── -->
<div id="header">
<div class="logo">⚡ SALTYBOT — DIAGNOSTICS</div>
<div id="conn-bar">
<div id="conn-dot"></div>
<input id="ws-input" type="text" value="ws://localhost:9090" placeholder="ws://robot-ip:9090" />
<button id="btn-connect" class="hdr-btn">CONNECT</button>
<span id="conn-label" style="color:#4b5563;font-size:10px">Not connected</span>
</div>
<div id="refresh-info">
<div id="refresh-dot"></div>
<span>auto 2 Hz</span>
</div>
</div>
<!-- ── System status bar ── -->
<div id="status-bar">
<span style="color:#6b7280;font-size:10px">SYSTEM</span>
<span class="sys-badge badge-stale" id="system-badge">STALE</span>
<span style="color:#4b5563">│</span>
<span style="color:#6b7280;font-size:10px">BATTERY</span>
<span class="sys-badge badge-stale" id="batt-badge">—</span>
<span style="color:#4b5563">│</span>
<span style="color:#6b7280;font-size:10px">THERMAL</span>
<span class="sys-badge badge-stale" id="temp-badge">—</span>
<span style="color:#4b5563">│</span>
<span style="color:#6b7280;font-size:10px">NETWORK</span>
<span class="sys-badge badge-stale" id="net-badge">—</span>
<span id="last-update">Awaiting data…</span>
</div>
<!-- ── Dashboard grid ── -->
<div id="dashboard">
<!-- ╔═══════════════════ BATTERY ═══════════════════╗ -->
<div class="card span2">
<div class="card-title">
BATTERY — 4S LiPo (12.0–16.8 V)
<span class="badge badge-stale" id="batt-badge-2" style="font-size:9px">—</span>
</div>
<!-- Big readout row -->
<div style="display:flex;gap:16px;align-items:flex-end;flex-wrap:wrap">
<div>
<div style="font-size:9px;color:#6b7280;margin-bottom:2px">VOLTAGE</div>
<div class="big-metric">
<span class="big-num" id="batt-voltage" style="color:#22c55e">—</span>
</div>
</div>
<div>
<div style="font-size:9px;color:#6b7280;margin-bottom:2px">SOC</div>
<div class="big-metric">
<span class="big-num" id="batt-soc" style="color:#22c55e">—</span>
</div>
</div>
<div>
<div style="font-size:9px;color:#6b7280;margin-bottom:2px">CURRENT</div>
<div class="big-metric">
<span class="big-num" id="batt-current" style="color:#06b6d4;font-size:20px">—</span>
</div>
</div>
</div>
<!-- Gauge bars -->
<div class="gauge-row">
<div class="gauge-label-row">
<span class="gauge-label">Voltage</span>
<span class="gauge-value" id="batt-voltage-2" style="color:#6b7280">12.0–16.8 V</span>
</div>
<div class="gauge-track"><div class="gauge-fill" id="batt-volt-bar" style="width:0%"></div></div>
</div>
<div class="gauge-row">
<div class="gauge-label-row">
<span class="gauge-label">State of Charge</span>
<span class="gauge-value" style="color:#6b7280">0–100%</span>
</div>
<div class="gauge-track"><div class="gauge-fill" id="batt-soc-bar" style="width:0%"></div></div>
</div>
<!-- Sparkline history -->
<div>
<div style="font-size:9px;color:#6b7280;margin-bottom:4px">VOLTAGE HISTORY (last 2 min)</div>
<canvas id="batt-sparkline" height="56"></canvas>
</div>
</div>
<!-- ╔═══════════════════ TEMPERATURES ═══════════════════╗ -->
<div class="card">
<div class="card-title">TEMPERATURES</div>
<div class="temp-grid">
<div class="temp-box" id="cpu-temp-box">
<div class="temp-label">CPU (Jetson)</div>
<div class="temp-value" id="cpu-temp-val">—</div>
<div class="temp-bar-track"><div class="temp-bar-fill" id="cpu-temp-bar" style="width:0%"></div></div>
</div>
<div class="temp-box" id="gpu-temp-box">
<div class="temp-label">GPU (Jetson)</div>
<div class="temp-value" id="gpu-temp-val">—</div>
<div class="temp-bar-track"><div class="temp-bar-fill" id="gpu-temp-bar" style="width:0%"></div></div>
</div>
<div class="temp-box" id="board-temp-box">
<div class="temp-label">Board / STM32</div>
<div class="temp-value" id="board-temp-val">—</div>
<div class="temp-bar-track"><div class="temp-bar-fill" id="board-temp-bar" style="width:0%"></div></div>
</div>
<div class="temp-box" id="motor-temp-l-box">
<div class="temp-label">Motor L</div>
<div class="temp-value" id="motor-temp-l-val">—</div>
<div class="temp-bar-track"><div class="temp-bar-fill" id="motor-temp-l-bar" style="width:0%"></div></div>
</div>
<!-- spacer to keep 2-col grid balanced if motor-temp-r is alone -->
<div class="temp-box" id="motor-temp-r-box">
<div class="temp-label">Motor R</div>
<div class="temp-value" id="motor-temp-r-val">—</div>
<div class="temp-bar-track"><div class="temp-bar-fill" id="motor-temp-r-bar" style="width:0%"></div></div>
</div>
</div>
<div style="font-size:9px;color:#374151">
Zones: &lt;60°C green · 60–75°C amber · &gt;75°C red
</div>
</div>
<!-- ╔═══════════════════ MOTOR CURRENT ═══════════════════╗ -->
<div class="card">
<div class="card-title">MOTOR CURRENT &amp; CMD</div>
<div class="motor-grid">
<div class="motor-box">
<div class="motor-label">LEFT WHEEL</div>
<div style="font-size:20px;font-weight:bold;font-family:monospace" id="motor-cur-l">—</div>
<div class="gauge-track" style="margin-top:4px">
<div class="gauge-fill" id="motor-bar-l" style="width:0%"></div>
</div>
<div style="font-size:9px;color:#4b5563;margin-top:4px">
CMD: <span id="motor-cmd-l" style="color:#6b7280">—</span>
</div>
</div>
<div class="motor-box">
<div class="motor-label">RIGHT WHEEL</div>
<div style="font-size:20px;font-weight:bold;font-family:monospace" id="motor-cur-r">—</div>
<div class="gauge-track" style="margin-top:4px">
<div class="gauge-fill" id="motor-bar-r" style="width:0%"></div>
</div>
<div style="font-size:9px;color:#4b5563;margin-top:4px">
CMD: <span id="motor-cmd-r" style="color:#6b7280">—</span>
</div>
</div>
</div>
<div style="font-size:10px;color:#6b7280">
Balance state: <span id="balance-state" style="color:#9ca3af;font-family:monospace">—</span>
</div>
<div style="font-size:9px;color:#374151">Thresholds: warn 8A · crit 12A</div>
</div>
<!-- ╔═══════════════════ MEMORY / DISK ═══════════════════╗ -->
<div class="card">
<div class="card-title">RESOURCES</div>
<div class="gauge-row">
<div class="gauge-label-row">
<span class="gauge-label">RAM</span>
<span class="gauge-value" id="ram-val" style="color:#9ca3af">—</span>
</div>
<div class="gauge-track"><div class="gauge-fill" id="ram-bar" style="width:0%"></div></div>
</div>
<div class="gauge-row">
<div class="gauge-label-row">
<span class="gauge-label">GPU Memory</span>
<span class="gauge-value" id="gpu-val" style="color:#9ca3af">—</span>
</div>
<div class="gauge-track"><div class="gauge-fill" id="gpu-bar" style="width:0%"></div></div>
</div>
<div class="gauge-row">
<div class="gauge-label-row">
<span class="gauge-label">Disk</span>
<span class="gauge-value" id="disk-val" style="color:#9ca3af">—</span>
</div>
<div class="gauge-track"><div class="gauge-fill" id="disk-bar" style="width:0%"></div></div>
</div>
<div style="font-size:9px;color:#374151">Warn ≥80% · Critical ≥95%</div>
</div>
<!-- ╔═══════════════════ WIFI / LATENCY ═══════════════════╗ -->
<div class="card">
<div class="card-title">NETWORK</div>
<div style="display:flex;align-items:center;gap:12px;flex-wrap:wrap">
<div id="rssi-bars" class="rssi-bars"></div>
<div>
<div style="font-size:9px;color:#6b7280">RSSI</div>
<div style="font-size:20px;font-weight:bold;font-family:monospace" id="rssi-val">—</div>
<div style="font-size:9px" id="rssi-label" style="color:#6b7280">—</div>
</div>
</div>
<div class="gauge-row">
<div class="gauge-label-row">
<span class="gauge-label">Rosbridge Latency</span>
<span class="gauge-value" id="latency-val" style="color:#9ca3af">—</span>
</div>
</div>
<!-- MQTT status -->
<div style="margin-top:4px;padding-top:8px;border-top:1px solid #0c2a3a">
<div style="font-size:9px;color:#6b7280;margin-bottom:4px">MQTT BROKER</div>
<div class="mqtt-status">
<div class="mqtt-dot" id="mqtt-dot"></div>
<span id="mqtt-label" style="color:#6b7280">No data</span>
</div>
<div style="font-size:9px;color:#374151;margin-top:4px">
Via /diagnostics KeyValue: mqtt_connected
</div>
</div>
</div>
<!-- ╔═══════════════════ ROS2 NODE HEALTH ═══════════════════╗ -->
<div class="card span3">
<div class="card-title">
ROS2 NODE HEALTH — /diagnostics
<span style="font-size:9px;color:#4b5563" id="node-count">0 nodes</span>
</div>
<div id="node-list">
<div style="color:#374151;font-size:10px;text-align:center;padding:12px">
Waiting for /diagnostics data…
</div>
</div>
</div>
</div>
<!-- ── Footer ── -->
<div id="footer">
<span>rosbridge: <code id="footer-ws">ws://localhost:9090</code></span>
<span>diagnostics dashboard — issue #562 · auto 2Hz</span>
</div>
<script src="diagnostics_panel.js"></script>
<script>
// Sync footer WS URL
document.getElementById('ws-input').addEventListener('input', (e) => {
document.getElementById('footer-ws').textContent = e.target.value;
});
// Keep node count updated after render
const origRenderNodes = window.renderNodes;
const nodeCountEl = document.getElementById('node-count');
const _origOnDiag = window.onDiagnostics;
// Node count updates via MutationObserver on the node-list div
const nl = document.getElementById('node-list');
if (nl) {
new MutationObserver(() => {
const rows = nl.querySelectorAll('.node-row');
if (nodeCountEl) nodeCountEl.textContent = rows.length + ' node' + (rows.length !== 1 ? 's' : '');
}).observe(nl, { childList: true, subtree: false });
}
</script>
</body>
</html>

592
ui/diagnostics_panel.js Normal file
View File

@ -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 = '<div style="color:#374151;font-size:10px;text-align:center;padding:8px">No /diagnostics data yet</div>';
return;
}
container.innerHTML = state.nodes.map((n) => `
<div class="node-row">
<span class="node-name">${n.name}</span>
<div style="display:flex;align-items:center;gap:6px">
${n.message ? `<span style="color:#4b5563;font-size:9px;max-width:120px;overflow:hidden;text-overflow:ellipsis;white-space:nowrap" title="${n.message}">${n.message}</span>` : ''}
<span class="node-status ${levelClass(n.level)}">${levelLabel(n.level)}</span>
</div>
</div>
`).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);