diff --git a/chassis/uwb_anchor_mount.scad b/chassis/uwb_anchor_mount.scad index 6c77443..8213f04 100644 --- a/chassis/uwb_anchor_mount.scad +++ b/chassis/uwb_anchor_mount.scad @@ -1,275 +1,463 @@ // ============================================================ -// uwb_anchor_mount.scad — Stem-Mounted UWB Anchor Rev A -// Agent: sl-mechanical 2026-03-01 -// Closes issues #57, #62 +// uwb_anchor_mount.scad — Wall/Ceiling UWB Anchor Mount Bracket +// Issue: #564 Agent: sl-mechanical Date: 2026-03-14 +// (supersedes Rev A stem-collar mount — see git history) // ============================================================ -// Clamp-on bracket for 2× MaUWB ESP32-S3 anchor modules on -// SaltyBot 25 mm OD vertical stem. -// Anchors spaced ANCHOR_SPACING = 250 mm apart. // -// Features: -// • Split D-collar with M4 clamping bolts + M4 set screw -// • Anti-rotation flat tab that keys against a small pin -// OR printed key tab that registers on the stem flat (if stem -// has a ground flat) — see ANTI_ROT_MODE parameter -// • Module bracket: faces outward, tilted 10° from vertical -// so antenna clears stem and faces horizon -// • USB cable channel (power from Orin via USB-A) on collar -// • Tool-free capture: M4 thumbscrews (slot-head, hand-tighten) -// • UWB antenna area: NO material within 10 mm of PCB top face +// Parametric wall or ceiling mount bracket for ESP32 UWB Pro anchor. +// Designed for fixed-infrastructure deployment: anchors screw into +// wall or ceiling drywall/timber with standard M4 or #6 wood screws, +// at a user-defined tilt angle so the UWB antenna faces the desired +// coverage zone. // -// Components per mount: -// 2× collar_half print in PLA/PETG, flat-face-down -// 1× module_bracket print in PLA/PETG, flat-face-down +// Architecture: +// Wall base → flat backplate with 2× screw holes (wall or ceiling) +// Tilt knuckle → single-axis articulating joint; 15° detent steps +// locked with M3 nyloc bolt; range 0–90° +// Anchor cradle→ U-cradle holding ESP32 UWB Pro PCB on M2.5 standoffs +// USB-C channel→ routed groove on tilt arm + exit slot in cradle back wall +// Label slot → rear window slot for printed anchor-ID card strip +// +// Part catalogue: +// Part 1 — wall_base() Backplate + 2-ear pivot block + detent arc +// Part 2 — tilt_arm() Pivoting arm with knuckle + cradle stub +// Part 3 — anchor_cradle() PCB cradle, standoffs, USB-C slot, label window +// Part 4 — cable_clip() Snap-on USB-C cable guide for tilt arm +// Part 5 — assembly_preview() +// +// Hardware BOM: +// 2× M4 × 30 mm wood screws (or #6 drywall screws) wall fasteners +// 1× M3 × 20 mm SHCS + M3 nyloc nut tilt pivot bolt +// 4× M2.5 × 8 mm SHCS PCB-to-cradle +// 4× M2.5 hex nuts captured in standoffs +// 1× USB-C cable anchor power +// +// ESP32 UWB Pro interface (verify with calipers): +// PCB size : UWB_L × UWB_W × UWB_H (55 × 28 × 10 mm default) +// Mounting holes : M2.5, 4× corners on UWB_HOLE_X × UWB_HOLE_Y pattern +// USB-C port : centred on short edge, UWB_USBC_W × UWB_USBC_H +// Antenna area : top face rear half — 10 mm keep-out of bracket material +// +// Tilt angles (15° detent steps, set TILT_DEG before export): +// 0° → horizontal face-up (ceiling, antenna faces down) +// 30° → 30° downward (wall near ceiling) [default] +// 45° → diagonal (wall mid-height) +// 90° → vertical face-out (wall, antenna faces forward) // // RENDER options: -// "assembly" single mount assembled (default) -// "collar_front" front collar half for slicing (×2 per mount × 2 mounts = 4) -// "collar_rear" rear collar half -// "bracket" module bracket (×2 mounts) -// "pair" both mounts on 350 mm stem section +// "assembly" full assembly at TILT_DEG (default) +// "wall_base_stl" Part 1 +// "tilt_arm_stl" Part 2 +// "anchor_cradle_stl" Part 3 +// "cable_clip_stl" Part 4 +// +// Export commands: +// openscad uwb_anchor_mount.scad -D 'RENDER="wall_base_stl"' -o uwb_wall_base.stl +// openscad uwb_anchor_mount.scad -D 'RENDER="tilt_arm_stl"' -o uwb_tilt_arm.stl +// openscad uwb_anchor_mount.scad -D 'RENDER="anchor_cradle_stl"' -o uwb_anchor_cradle.stl +// openscad uwb_anchor_mount.scad -D 'RENDER="cable_clip_stl"' -o uwb_cable_clip.stl // ============================================================ +$fn = 64; +e = 0.01; + +// ── Tilt angle (override per anchor, 0–90°, 15° steps) ─────────────────────── +TILT_DEG = 30; + +// ── ESP32 UWB Pro PCB dimensions (verify with calipers) ────────────────────── +UWB_L = 55.0; // PCB length +UWB_W = 28.0; // PCB width +UWB_H = 10.0; // PCB + components height +UWB_HOLE_X = 47.5; // M2.5 hole X span +UWB_HOLE_Y = 21.0; // M2.5 hole Y span +UWB_USBC_W = 9.5; // USB-C receptacle width +UWB_USBC_H = 4.0; // USB-C receptacle height +UWB_ANTENNA_L = 20.0; // antenna area at PCB rear (keep-out) + +// ── Wall base geometry ──────────────────────────────────────────────────────── +BASE_W = 60.0; +BASE_H = 50.0; +BASE_T = 5.0; +BASE_SCREW_D = 4.5; // M4 clearance +BASE_SCREW_HD = 8.5; // countersink OD +BASE_SCREW_HH = 3.5; // countersink depth +BASE_SCREW_SPC = 35.0; // Z span between screw holes +KNUCKLE_T = BASE_T + 4.0; // pivot ear depth (Y) + +// ── Tilt arm geometry ───────────────────────────────────────────────────────── +ARM_W = 12.0; +ARM_T = 5.0; +ARM_L = 35.0; +PIVOT_D = 3.3; // M3 clearance +PIVOT_NUT_AF = 5.5; +PIVOT_NUT_H = 2.4; +DETENT_D = 3.2; // detent notch diameter +DETENT_R = 8.0; // detent notch radius from pivot + +// ── Anchor cradle geometry ──────────────────────────────────────────────────── +CRADLE_WALL_T = 3.5; +CRADLE_BACK_T = 4.0; +CRADLE_FLOOR_T = 3.0; +CRADLE_LIP_H = 4.0; +CRADLE_LIP_T = 2.5; +STANDOFF_H = 3.0; +STANDOFF_OD = 5.5; +LABEL_W = UWB_L - 4.0; +LABEL_H = UWB_W * 0.55; +LABEL_T = 1.2; // label card thickness + +// ── USB-C cable routing ─────────────────────────────────────────────────────── +USBC_CHAN_W = 11.0; +USBC_CHAN_H = 7.0; + +// ── Cable clip ──────────────────────────────────────────────────────────────── +CLIP_CABLE_D = 4.5; +CLIP_T = 2.0; +CLIP_BODY_W = 16.0; +CLIP_BODY_H = 10.0; + +// ── Fasteners ───────────────────────────────────────────────────────────────── +M2P5_D = 2.7; +M3_D = 3.3; +M3_NUT_AF = 5.5; +M3_NUT_H = 2.4; + +// ============================================================ +// RENDER DISPATCH +// ============================================================ RENDER = "assembly"; -// ── ⚠ Verify with calipers ─────────────────────────────────── -MAWB_L = 50.0; // PCB length -MAWB_W = 25.0; // PCB width -MAWB_H = 10.0; // PCB + components -MAWB_HOLE_X = 43.0; // M2 mounting hole X span -MAWB_HOLE_Y = 20.0; // M2 mounting hole Y span -M2_D = 2.2; // M2 clearance +if (RENDER == "assembly") assembly_preview(); +else if (RENDER == "wall_base_stl") wall_base(); +else if (RENDER == "tilt_arm_stl") tilt_arm(); +else if (RENDER == "anchor_cradle_stl") anchor_cradle(); +else if (RENDER == "cable_clip_stl") cable_clip(); -// ── Stem ───────────────────────────────────────────────────── -STEM_OD = 25.0; -STEM_BORE = 25.4; // +0.4 clearance -WALL = 2.0; // wall thickness (used in thumbscrew recess) +// ============================================================ +// ASSEMBLY PREVIEW +// ============================================================ +module assembly_preview() { + // Ghost wall surface + %color("Wheat", 0.22) + translate([-BASE_W/2, -10, -BASE_H/2]) + cube([BASE_W, 10, BASE_H + 40]); -// ── Collar ─────────────────────────────────────────────────── -COL_OD = 52.0; -COL_H = 30.0; // taller than sensor-head collar for rigidity -COL_BOLT_X = 19.0; // M4 bolt CL from stem axis -COL_BOLT_D = 4.5; // M4 clearance -THUMB_HEAD_D= 8.0; // M4 thumbscrew head OD (slot for access) -COL_NUT_W = 7.0; // M4 hex nut A/F -COL_NUT_H = 3.4; + // Wall base + color("OliveDrab", 0.85) + wall_base(); -// Anti-rotation flat tab: a 3 mm wall tab that protrudes radially -// and bears against the bracket arm, preventing axial rotation -// without needing a stem flat. -ANTI_ROT_T = 3.0; // tab thickness (radial) -ANTI_ROT_W = 8.0; // tab width (tangential) -ANTI_ROT_Z = 4.0; // distance from collar base + // Tilt arm at TILT_DEG, pivoting at knuckle + color("SteelBlue", 0.85) + translate([0, KNUCKLE_T, 0]) + rotate([TILT_DEG, 0, 0]) + tilt_arm(); -// USB cable channel: groove on collar outer surface, runs Z direction -// Cable routes from anchor module down to base -USB_CHAN_W = 9.0; // channel width (fits USB-A cable Ø6 mm) -USB_CHAN_D = 5.0; // channel depth + // Anchor cradle at arm end + color("DarkSlateGray", 0.85) + translate([0, KNUCKLE_T, 0]) + rotate([TILT_DEG, 0, 0]) + translate([0, ARM_T, ARM_L]) + anchor_cradle(); -// ── Module bracket ─────────────────────────────────────────── -ARM_L = 20.0; // arm length from collar OD to bracket face -ARM_W = MAWB_W + 6.0; // bracket width (Y, includes side walls) -ARM_H = 6.0; // arm thickness (Z) -BRKT_TILT = 10.0; // tilt outward from vertical (antenna faces horizon) + // PCB ghost + %color("ForestGreen", 0.38) + translate([0, KNUCKLE_T, 0]) + rotate([TILT_DEG, 0, 0]) + translate([-UWB_L/2, + ARM_T + CRADLE_BACK_T, + ARM_L + CRADLE_FLOOR_T + STANDOFF_H]) + cube([UWB_L, UWB_W, UWB_H]); -BRKT_BACK_T = 3.0; // bracket back wall (module sits against this) -BRKT_SIDE_T = 2.0; // bracket side walls + // Cable clip at arm mid-point + color("DimGray", 0.70) + translate([ARM_W/2, KNUCKLE_T, 0]) + rotate([TILT_DEG, 0, 0]) + translate([0, ARM_T + e, ARM_L/2]) + rotate([0, -90, 90]) + cable_clip(); +} -M2_STNDFF = 3.0; // M2 standoff height -M2_STNDFF_OD= 4.5; - -// USB port access notch in bracket side wall (8×5 mm) -USB_NOTCH_W = 10.0; -USB_NOTCH_H = 7.0; - -// ── Spacing ─────────────────────────────────────────────────── -ANCHOR_SPACING = 250.0; // centre-to-centre Z separation - -$fn = 64; -e = 0.01; - -// ───────────────────────────────────────────────────────────── -// collar_half(side) -// split at Y=0 plane. Bracket arm on front (+Y) half. -// Print flat-face-down. -// ───────────────────────────────────────────────────────────── -module collar_half(side = "front") { - y_front = (side == "front"); +// ============================================================ +// PART 1 — WALL BASE +// ============================================================ +// Flat backplate screws to wall or ceiling with 2× countersunk +// M4/#6 wood screws on BASE_SCREW_SPC (35 mm) centres. +// Two pivot ears straddle the tilt arm; M3 pivot bolt passes through +// both ears and arm knuckle. +// Detent arc on inner face of +X ear: 7 notches at 15° steps (0–90°) +// so tilt can be set without a protractor. +// Shallow rear recess accepts a printed installation-zone label. +// +// Dual-use: flat face to wall (vertical screw axis) or flat face +// to ceiling (horizontal screw axis) — same part either way. +// +// Print: backplate flat on bed, PETG, 5 perims, 40 % gyroid. +module wall_base() { + ear_h = ARM_W + 3.0; + ear_t = 6.0; + ear_sep = ARM_W + 1.0; difference() { union() { - // D-shaped body - intersection() { - cylinder(d=COL_OD, h=COL_H); - translate([-COL_OD/2, y_front ? 0 : -COL_OD/2, 0]) - cube([COL_OD, COL_OD/2, COL_H]); - } + // ── Backplate ──────────────────────────────────────────────── + translate([-BASE_W/2, -BASE_T, -BASE_H/2]) + cube([BASE_W, BASE_T, BASE_H]); - // Anti-rotation tab (front half only, at +X side) - if (y_front) { - translate([COL_OD/2, -ANTI_ROT_W/2, ANTI_ROT_Z]) - cube([ANTI_ROT_T, ANTI_ROT_W, - COL_H - ANTI_ROT_Z - 4]); - } + // ── Two pivot ears ──────────────────────────────────────────── + for (ex = [-(ear_sep/2 + ear_t), ear_sep/2]) + translate([ex, -BASE_T + e, -ear_h/2]) + cube([ear_t, KNUCKLE_T + e, ear_h]); - // Bracket arm attachment boss (front half only, top centre) - if (y_front) { - translate([-ARM_W/2, COL_OD/2, COL_H * 0.3]) - cube([ARM_W, ARM_L, COL_H * 0.4]); - } + // ── Stiffening gussets ──────────────────────────────────────── + for (ex = [-(ear_sep/2 + ear_t), ear_sep/2]) + hull() { + translate([ex, -BASE_T, -ear_h/4]) + cube([ear_t, BASE_T - 1, ear_h/2]); + translate([ex + (ex < 0 ? ear_t*0.6 : 0), + -BASE_T, -ear_h/6]) + cube([ear_t * 0.4, 1, ear_h/3]); + } } - // ── Stem bore ───────────────────────────────────────── - translate([0,0,-e]) - cylinder(d=STEM_BORE, h=COL_H + 2*e); - - // ── M4 clamping bolt holes (Y direction) ────────────── - for (bx=[-COL_BOLT_X, COL_BOLT_X]) { - translate([bx, y_front ? COL_OD/2 : 0, COL_H/2]) - rotate([90,0,0]) - cylinder(d=COL_BOLT_D, h=COL_OD/2 + e); - // Thumbscrew head recess on outer face (front only — access side) - if (y_front) { - translate([bx, COL_OD/2 - WALL, COL_H/2]) - rotate([90,0,0]) - cylinder(d=THUMB_HEAD_D, h=8 + e); - } + // ── 2× countersunk wall screws ──────────────────────────────────── + for (sz = [-BASE_SCREW_SPC/2, BASE_SCREW_SPC/2]) { + translate([0, -BASE_T - e, sz]) + rotate([-90, 0, 0]) + cylinder(d = BASE_SCREW_D, h = BASE_T + 2*e); + translate([0, -BASE_T - e, sz]) + rotate([-90, 0, 0]) + cylinder(d1 = BASE_SCREW_HD, d2 = BASE_SCREW_D, + h = BASE_SCREW_HH + e); } - // ── M4 hex nut pockets (rear half) ──────────────────── - if (!y_front) { - for (bx=[-COL_BOLT_X, COL_BOLT_X]) { - translate([bx, -(COL_OD/4 + e), COL_H/2]) - rotate([90,0,0]) - cylinder(d=COL_NUT_W/cos(30), h=COL_NUT_H + e, - $fn=6); - } - } + // ── Pivot bolt bore (M3, through both ears) ─────────────────────── + translate([-(ear_sep/2 + ear_t + e), KNUCKLE_T * 0.55, 0]) + rotate([0, 90, 0]) + cylinder(d = PIVOT_D, h = ear_sep + 2*ear_t + 2*e); - // ── Set screw (height lock, front half) ─────────────── - if (y_front) { - translate([0, COL_OD/2, COL_H * 0.8]) - rotate([90,0,0]) - cylinder(d=COL_BOLT_D, - h=COL_OD/2 - STEM_BORE/2 + e); - } + // ── M3 nyloc nut pocket (outer face of one ear) ─────────────────── + translate([ear_sep/2 + ear_t - PIVOT_NUT_H - 0.4, + KNUCKLE_T * 0.55, 0]) + rotate([0, 90, 0]) + cylinder(d = PIVOT_NUT_AF / cos(30), + h = PIVOT_NUT_H + 0.5, $fn = 6); - // ── USB cable routing channel (rear half, −X side) ──── - if (!y_front) { - translate([-COL_OD/2, -USB_CHAN_W/2, -e]) - cube([USB_CHAN_D, USB_CHAN_W, COL_H + 2*e]); - } + // ── Detent arc — 7 notches at 15° steps on +X ear inner face ───── + for (da = [0 : 15 : 90]) + translate([ear_sep/2 - e, + KNUCKLE_T * 0.55 + DETENT_R * sin(da), + DETENT_R * cos(da)]) + rotate([0, 90, 0]) + cylinder(d = DETENT_D, h = ear_t * 0.45 + e); - // ── M4 hole through arm boss (Z direction, for bracket bolt) ─ - if (y_front) { - for (dx=[-ARM_W/4, ARM_W/4]) - translate([dx, COL_OD/2 + ARM_L/2, COL_H * 0.35]) - cylinder(d=COL_BOLT_D, h=COL_H * 0.35 + e); - } + // ── Installation label recess (rear face of backplate) ──────────── + translate([0, -BASE_T - e, 0]) + rotate([-90, 0, 0]) + cube([BASE_W - 12, BASE_H - 16, 1.6], center = true); + + // ── Lightening pocket ───────────────────────────────────────────── + translate([0, -BASE_T + 1.5, 0]) + cube([BASE_W - 14, BASE_T - 3, BASE_H - 20], center = true); } } -// ───────────────────────────────────────────────────────────── -// module_bracket() -// Bolts to collar arm boss. Holds MaUWB PCB facing outward. -// Tilted BRKT_TILT° from vertical — antenna clears stem. -// Print flat-face-down (back wall on bed). -// ───────────────────────────────────────────────────────────── -module module_bracket() { - bk = BRKT_BACK_T; - sd = BRKT_SIDE_T; +// ============================================================ +// PART 2 — TILT ARM +// ============================================================ +// Pivoting arm linking wall_base pivot ears to anchor_cradle. +// Knuckle end (Z=0): M3 pivot bore + spring-plunger detent pocket +// that indexes into the base ear detent arc notches. +// Cradle end (Z=ARM_L): 2× M3 bolt attachment to cradle back wall. +// USB-C cable channel runs along outer (+Y) face, full arm length. +// +// Print: knuckle face flat on bed, PETG, 5 perims, 40 % gyroid. +module tilt_arm() { + total_h = ARM_L + 10; difference() { union() { - // ── Back wall (mounts to collar arm boss) ───────── - cube([ARM_W, bk, MAWB_H + M2_STNDFF + 6]); + // ── Arm body ───────────────────────────────────────────────── + translate([-ARM_W/2, 0, 0]) + cube([ARM_W, ARM_T, total_h]); - // ── Side walls ──────────────────────────────────── - for (sx=[0, ARM_W - sd]) - translate([sx, bk, 0]) - cube([sd, MAWB_L + 2, MAWB_H + M2_STNDFF + 6]); + // ── Knuckle boss (rounded pivot end) ───────────────────────── + translate([0, ARM_T/2, 0]) + rotate([90, 0, 0]) + cylinder(d = ARM_W, h = ARM_T, center = true); - // ── M2 standoff posts (PCB mounts to these) ─────── - for (hx=[0, MAWB_HOLE_X], hy=[0, MAWB_HOLE_Y]) - translate([(ARM_W - MAWB_HOLE_X)/2 + hx, - bk + (MAWB_L - MAWB_HOLE_Y)/2 + hy, - 0]) - cylinder(d=M2_STNDFF_OD, h=M2_STNDFF); + // ── Cradle attach stub (Z = ARM_L) ──────────────────────────── + translate([-ARM_W/2, 0, ARM_L]) + cube([ARM_W, ARM_T + CRADLE_BACK_T, ARM_T]); } - // ── M2 bores through standoffs ──────────────────────── - for (hx=[0, MAWB_HOLE_X], hy=[0, MAWB_HOLE_Y]) - translate([(ARM_W - MAWB_HOLE_X)/2 + hx, - bk + (MAWB_L - MAWB_HOLE_Y)/2 + hy, - -e]) - cylinder(d=M2_D, h=M2_STNDFF + e); + // ── M3 pivot bore ───────────────────────────────────────────────── + translate([-ARM_W/2 - e, ARM_T/2, 0]) + rotate([0, 90, 0]) + cylinder(d = PIVOT_D, h = ARM_W + 2*e); - // ── Antenna clearance cutout in back wall ───────────── - // Open slot near top of back wall so antenna is unobstructed - translate([sd, -e, M2_STNDFF + 2]) - cube([ARM_W - 2*sd, bk + 2*e, MAWB_H]); + // ── Detent plunger pocket (3 mm spring-ball, outer +Y face) ────── + translate([0, ARM_T + e, 0]) + rotate([90, 0, 0]) + cylinder(d = 3.2, h = 4 + e); - // ── USB port access notch on one side wall ──────────── - translate([-e, bk + 2, M2_STNDFF - 1]) - cube([sd + 2*e, USB_NOTCH_W, USB_NOTCH_H]); + // ── USB-C cable channel (outer +Y face, mid-arm length) ─────────── + translate([-USBC_CHAN_W/2, ARM_T - e, ARM_T + 4]) + cube([USBC_CHAN_W, USBC_CHAN_H, ARM_L - ARM_T - 8]); - // ── Mounting holes to collar arm boss (×2) ──────────── - for (dx=[-ARM_W/4, ARM_W/4]) - translate([ARM_W/2 + dx, bk + ARM_L/2, -e]) - cylinder(d=COL_BOLT_D, h=6 + e); + // ── Cradle attach bolt holes (2× M3 at cradle stub) ─────────────── + for (bx = [-ARM_W/4, ARM_W/4]) + translate([bx, ARM_T/2, ARM_L + ARM_T/2]) + rotate([90, 0, 0]) + cylinder(d = M3_D, h = ARM_T + CRADLE_BACK_T + 2*e); + + // ── M3 nut pockets (front of cradle stub) ───────────────────────── + for (bx = [-ARM_W/4, ARM_W/4]) + translate([bx, ARM_T/2, ARM_L + ARM_T/2]) + rotate([-90, 0, 0]) + cylinder(d = M3_NUT_AF / cos(30), + h = M3_NUT_H + 0.5, $fn = 6); + + // ── Lightening pocket ───────────────────────────────────────────── + translate([0, ARM_T/2, ARM_L/2]) + cube([ARM_W - 4, ARM_T - 2, ARM_L - 18], center = true); } } -// ───────────────────────────────────────────────────────────── -// single_anchor_assembly() -// ───────────────────────────────────────────────────────────── -module single_anchor_assembly(show_phantom=false) { - // Collar - color("SteelBlue", 0.9) collar_half("front"); - color("CornflowerBlue", 0.9) mirror([0,1,0]) collar_half("rear"); +// ============================================================ +// PART 3 — ANCHOR CRADLE +// ============================================================ +// Open-front U-cradle for ESP32 UWB Pro PCB. +// PCB retained on 4× M2.5 standoffs matching UWB_HOLE_X × UWB_HOLE_Y. +// Back wall features: +// • USB-C exit slot — aligns with PCB USB-C port +// • USB-C groove — cable routes from slot toward arm channel +// • Label card slot — insert printed strip for anchor ID +// • Antenna keep-out — back wall material removed above antenna area +// Front lip prevents PCB from sliding forward. +// Two attachment tabs bolt to tilt_arm cradle stub. +// +// Print: back wall flat on bed, PETG, 5 perims, 40 % gyroid. +module anchor_cradle() { + outer_l = UWB_L + 2*CRADLE_WALL_T; + outer_w = UWB_W + CRADLE_FLOOR_T; + pcb_z = CRADLE_FLOOR_T + STANDOFF_H; + total_z = pcb_z + UWB_H + 2; - // Bracket tilted BRKT_TILT° outward from top of arm boss - color("LightSteelBlue", 0.85) - translate([0, COL_OD/2 + ARM_L, COL_H * 0.3]) - rotate([BRKT_TILT, 0, 0]) - translate([-ARM_W/2, 0, 0]) - module_bracket(); + difference() { + union() { + // ── Cradle body ─────────────────────────────────────────────── + translate([-outer_l/2, 0, 0]) + cube([outer_l, outer_w, total_z]); - // Phantom UWB PCB - if (show_phantom) - color("ForestGreen", 0.4) - translate([-MAWB_L/2, - COL_OD/2 + ARM_L + BRKT_BACK_T, - COL_H * 0.3 + M2_STNDFF]) - cube([MAWB_L, MAWB_W, MAWB_H]); + // ── Front retaining lip ─────────────────────────────────────── + translate([-outer_l/2, outer_w - CRADLE_LIP_T, 0]) + cube([outer_l, CRADLE_LIP_T, CRADLE_LIP_H]); + + // ── Arm attachment tabs (behind back wall) ───────────────────── + for (tx = [-ARM_W/4, ARM_W/4]) + translate([tx - 4, -CRADLE_BACK_T, 0]) + cube([8, CRADLE_BACK_T + 1, total_z]); + } + + // ── PCB pocket ──────────────────────────────────────────────────── + translate([-UWB_L/2, 0, pcb_z]) + cube([UWB_L, UWB_W + 1, UWB_H + 4]); + + // ── USB-C exit slot (through back wall, aligned to PCB port) ───── + translate([0, -CRADLE_BACK_T - e, + pcb_z + UWB_H/2 - UWB_USBC_H/2]) + cube([UWB_USBC_W + 2, CRADLE_BACK_T + 2*e, UWB_USBC_H + 2], + center = [true, false, false]); + + // ── USB-C cable routing groove (outer back wall face) ───────────── + translate([0, -CRADLE_BACK_T - e, -e]) + cube([USBC_CHAN_W, USBC_CHAN_H, pcb_z + UWB_H/2 + USBC_CHAN_H], + center = [true, false, false]); + + // ── Label card slot (insert from below, rear face upper half) ───── + // Paper/laminate card strip identifying this anchor instance + translate([0, -CRADLE_BACK_T - e, pcb_z + UWB_H/2]) + cube([LABEL_W, LABEL_T + 0.3, LABEL_H], + center = [true, false, false]); + + // ── Antenna keep-out: remove back wall above antenna area ───────── + translate([0, -e, pcb_z + UWB_H - UWB_ANTENNA_L]) + cube([UWB_L - 4, CRADLE_BACK_T + 2*e, UWB_ANTENNA_L + 4], + center = [true, false, false]); + + // ── Arm bolt holes through attachment tabs ──────────────────────── + for (tx = [-ARM_W/4, ARM_W/4]) + translate([tx, ARM_T/2 - CRADLE_BACK_T, total_z/2]) + rotate([-90, 0, 0]) + cylinder(d = M3_D, h = ARM_T + CRADLE_BACK_T + 2*e); + + // ── Lightening slots in side walls ──────────────────────────────── + for (side_x = [-outer_l/2 - e, outer_l/2 - CRADLE_WALL_T - e]) + translate([side_x, 2, pcb_z + 2]) + cube([CRADLE_WALL_T + 2*e, UWB_W - 4, UWB_H - 4]); + } + + // ── M2.5 standoff bosses (positive, inside cradle floor) ────────────── + for (hx = [-UWB_HOLE_X/2, UWB_HOLE_X/2]) + for (hy = [(outer_w - UWB_W)/2 + (UWB_W - UWB_HOLE_Y)/2, + (outer_w - UWB_W)/2 + (UWB_W - UWB_HOLE_Y)/2 + UWB_HOLE_Y]) + difference() { + translate([hx, hy, CRADLE_FLOOR_T - e]) + cylinder(d = STANDOFF_OD, h = STANDOFF_H + e); + translate([hx, hy, CRADLE_FLOOR_T - 2*e]) + cylinder(d = M2P5_D, h = STANDOFF_H + 4); + } } -// ───────────────────────────────────────────────────────────── -// Render selector -// ───────────────────────────────────────────────────────────── -if (RENDER == "assembly") { - single_anchor_assembly(show_phantom=true); +// ============================================================ +// PART 4 — CABLE CLIP +// ============================================================ +// Snap-on C-clip retaining USB-C cable along tilt arm outer face. +// Presses onto ARM_T-wide arm with PETG snap tongues. +// Open-front cable channel for push-in cable insertion. +// Print ×2–3 per anchor, spaced 25 mm along arm. +// +// Print: clip-opening face down, PETG, 3 perims, 20 % infill. +module cable_clip() { + ch_r = CLIP_CABLE_D/2 + CLIP_T; + snap_t = 1.6; -} else if (RENDER == "collar_front") { - collar_half("front"); + difference() { + union() { + // ── Body plate ──────────────────────────────────────────────── + translate([-CLIP_BODY_W/2, 0, 0]) + cube([CLIP_BODY_W, CLIP_T, CLIP_BODY_H]); -} else if (RENDER == "collar_rear") { - collar_half("rear"); + // ── Cable channel (C-shape, opens toward +Y) ───────────────── + translate([0, CLIP_T + ch_r, CLIP_BODY_H/2]) + rotate([0, 90, 0]) + difference() { + cylinder(r = ch_r, h = CLIP_BODY_W, center = true); + cylinder(r = CLIP_CABLE_D/2, + h = CLIP_BODY_W + 2*e, center = true); + // open insertion slot + translate([0, ch_r + e, 0]) + cube([CLIP_CABLE_D * 0.85, + ch_r * 2 + 2*e, + CLIP_BODY_W + 2*e], center = true); + } -} else if (RENDER == "bracket") { - module_bracket(); + // ── Snap tongues (straddle arm, -Y side of body) ───────────── + for (tx = [-CLIP_BODY_W/2 + 1.5, + CLIP_BODY_W/2 - 1.5 - snap_t]) + translate([tx, -ARM_T - 1, 0]) + cube([snap_t, ARM_T + 1 + CLIP_T, CLIP_BODY_H]); -} else if (RENDER == "pair") { - // Both anchors at 250 mm spacing on a stem stub - color("Silver", 0.2) - translate([0, 0, -50]) - cylinder(d=STEM_OD, h=ANCHOR_SPACING + COL_H + 100); + // ── Snap barbs ──────────────────────────────────────────────── + for (tx = [-CLIP_BODY_W/2 + 1.5, + CLIP_BODY_W/2 - 1.5 - snap_t]) + translate([tx + snap_t/2, -ARM_T - 1, CLIP_BODY_H/2]) + rotate([0, 90, 0]) + cylinder(d = 2, h = snap_t, center = true); + } - // Lower anchor (Z = 0) - single_anchor_assembly(show_phantom=true); - - // Upper anchor (Z = ANCHOR_SPACING) - translate([0, 0, ANCHOR_SPACING]) - single_anchor_assembly(show_phantom=true); + // ── Arm slot (arm body passes between tongues) ───────────────────── + translate([0, -ARM_T - 1 - e, CLIP_BODY_H/2]) + cube([CLIP_BODY_W - 6, ARM_T + 2, CLIP_BODY_H - 4], center = true); + } } - diff --git a/include/jlink.h b/include/jlink.h index c008754..7e2289e 100644 --- a/include/jlink.h +++ b/include/jlink.h @@ -3,9 +3,10 @@ #include #include +#include "pid_flash.h" /* pid_sched_entry_t, PID_SCHED_MAX_BANDS */ /* - * JLink — Jetson serial binary protocol over USART1 (PB6=TX, PB7=RX). + * JLink -- Jetson serial binary protocol over USART1 (PB6=TX, PB7=RX). * * Issue #120: replaces jetson_cmd ASCII-over-USB-CDC with a dedicated * hardware UART at 921600 baud using DMA circular RX and IDLE interrupt. @@ -28,14 +29,21 @@ * 0x05 PID_SET - float kp, float ki, float kd (12 bytes, IEEE-754 LE) * 0x06 DFU_ENTER - no payload; request OTA DFU reboot (denied while armed) * 0x07 ESTOP - no payload; engage emergency stop + * 0x08 AUDIO - int16 PCM samples (up to 126 samples) + * 0x09 SLEEP - no payload; request STOP-mode sleep * 0x0A PID_SAVE - no payload; save current Kp/Ki/Kd to flash (Issue #531) * 0x0B GIMBAL_POS - int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547) + * 0x0C SCHED_GET - no payload; reply with TLM_SCHED (Issue #550) + * 0x0D SCHED_SET - uint8 num_bands + N*16-byte pid_sched_entry_t (Issue #550) + * 0x0E SCHED_SAVE - float kp, ki, kd (12 bytes); save sched+single to flash (Issue #550) * * STM32 to Jetson telemetry: - * 0x80 STATUS - jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ - * 0x81 POWER - jlink_tlm_power_t (11 bytes), sent at PM_TLM_HZ + * 0x80 STATUS - jlink_tlm_status_t (20 bytes), sent at JLINK_TLM_HZ + * 0x81 POWER - jlink_tlm_power_t (11 bytes), sent at PM_TLM_HZ + * 0x82 BATTERY - jlink_tlm_battery_t (10 bytes, Issue #533) * 0x83 PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531) * 0x84 GIMBAL_STATE - jlink_tlm_gimbal_state_t (10 bytes, Issue #547) + * 0x85 SCHED - jlink_tlm_sched_t (1+N*16 bytes), sent on SCHED_GET (Issue #550) * * Priority: CRSF RC always takes precedence. Jetson steer/speed only applied * when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and @@ -62,13 +70,17 @@ #define JLINK_CMD_SLEEP 0x09u /* no payload; request STOP-mode sleep */ #define JLINK_CMD_PID_SAVE 0x0Au /* no payload; save Kp/Ki/Kd to flash (Issue #531) */ #define JLINK_CMD_GIMBAL_POS 0x0Bu /* int16 pan_x10, int16 tilt_x10, uint16 speed (Issue #547) */ +#define JLINK_CMD_SCHED_GET 0x0Cu /* no payload; reply TLM_SCHED (Issue #550) */ +#define JLINK_CMD_SCHED_SET 0x0Du /* uint8 num_bands + N*16-byte entries (Issue #550) */ +#define JLINK_CMD_SCHED_SAVE 0x0Eu /* float kp,ki,kd; save sched+single to flash (Issue #550) */ /* ---- Telemetry IDs (STM32 to Jetson) ---- */ #define JLINK_TLM_STATUS 0x80u #define JLINK_TLM_POWER 0x81u /* jlink_tlm_power_t (11 bytes) */ #define JLINK_TLM_BATTERY 0x82u /* jlink_tlm_battery_t (10 bytes, Issue #533) */ -#define JLINK_TLM_PID_RESULT 0x83u /* jlink_tlm_pid_result_t (13 bytes) Issue #531 */ +#define JLINK_TLM_PID_RESULT 0x83u /* jlink_tlm_pid_result_t (13 bytes, Issue #531) */ #define JLINK_TLM_GIMBAL_STATE 0x84u /* jlink_tlm_gimbal_state_t (10 bytes, Issue #547) */ +#define JLINK_TLM_SCHED 0x85u /* jlink_tlm_sched_t (1+N*16 bytes, Issue #550) */ /* ---- Telemetry STATUS payload (20 bytes, packed) ---- */ typedef struct __attribute__((packed)) { @@ -98,15 +110,15 @@ typedef struct __attribute__((packed)) { uint32_t idle_ms; /* ms since last cmd_vel activity */ } jlink_tlm_power_t; /* 11 bytes */ -/* ---- Telemetry BATTERY payload (10 bytes, packed) — Issue #533 ---- */ +/* ---- Telemetry BATTERY payload (10 bytes, packed) Issue #533 ---- */ typedef struct __attribute__((packed)) { - uint16_t vbat_mv; /* DMA-sampled LPF-filtered Vbat (mV) */ + uint16_t vbat_mv; /* DMA-sampled LPF-filtered Vbat (mV) */ int16_t ibat_ma; /* DMA-sampled LPF-filtered Ibat (mA, + = discharge) */ - uint16_t vbat_raw_mv; /* unfiltered last-tick average (mV) */ - uint8_t flags; /* bit0=low, bit1=critical, bit2=4S, bit3=adc_ready */ - int8_t cal_offset; /* vbat_offset_mv / 4 (±127 → ±508 mV) */ - uint8_t lpf_shift; /* IIR shift factor (α = 1/2^lpf_shift) */ - uint8_t soc_pct; /* voltage-based SoC 0–100, 255 = unknown */ + uint16_t vbat_raw_mv; /* unfiltered last-tick average (mV) */ + uint8_t flags; /* bit0=low, bit1=critical, bit2=4S, bit3=adc_ready */ + int8_t cal_offset; /* vbat_offset_mv / 4 (+-127 -> +-508 mV) */ + uint8_t lpf_shift; /* IIR shift factor (alpha = 1/2^lpf_shift) */ + uint8_t soc_pct; /* voltage-based SoC 0-100, 255 = unknown */ } jlink_tlm_battery_t; /* 10 bytes */ /* ---- Telemetry PID_RESULT payload (13 bytes, packed) Issue #531 ---- */ @@ -129,6 +141,13 @@ typedef struct __attribute__((packed)) { uint8_t rx_err_pct; /* bus error rate 0-100% (rx_err*100/(rx_ok+rx_err)) */ } jlink_tlm_gimbal_state_t; /* 10 bytes */ +/* ---- Telemetry SCHED payload (1 + N*16 bytes, packed) Issue #550 ---- */ +/* Sent in response to JLINK_CMD_SCHED_GET; N = num_bands (1..PID_SCHED_MAX_BANDS). */ +typedef struct __attribute__((packed)) { + uint8_t num_bands; /* number of valid entries */ + pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* up to 6 x 16 = 96 bytes */ +} jlink_tlm_sched_t; /* 1 + 96 = 97 bytes max */ + /* ---- Volatile state (read from main loop) ---- */ typedef struct { /* Drive command - updated on JLINK_CMD_DRIVE */ @@ -161,55 +180,37 @@ typedef struct { volatile int16_t gimbal_pan_x10; /* pan angle deg x10 */ volatile int16_t gimbal_tilt_x10; /* tilt angle deg x10 */ volatile uint16_t gimbal_speed; /* servo speed 0-4095 (0=max) */ + + /* PID schedule commands (Issue #550) - set by parser, cleared by main loop */ + volatile uint8_t sched_get_req; /* SCHED_GET: main loop calls jlink_send_sched_telemetry() */ + volatile uint8_t sched_save_req; /* SCHED_SAVE: main loop calls pid_schedule_flash_save() */ + volatile float sched_save_kp; /* kp for single-PID record in SCHED_SAVE */ + volatile float sched_save_ki; + volatile float sched_save_kd; } JLinkState; extern volatile JLinkState jlink_state; +/* ---- SCHED_SET receive buffer -- Issue #550 ---- */ +/* + * Populated by the parser on JLINK_CMD_SCHED_SET. Main loop reads via + * jlink_get_sched_set() and calls pid_schedule_set_table() before clearing. + */ +typedef struct { + volatile uint8_t ready; /* set by parser, cleared by main loop */ + volatile uint8_t num_bands; + pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* copied from frame */ +} JLinkSchedSetBuf; + /* ---- API ---- */ -/* - * jlink_init() - configure USART1 (PB6=TX, PB7=RX) at 921600 baud with - * DMA2_Stream2_Channel4 circular RX (128-byte buffer) and IDLE interrupt. - * Call once before safety_init(). - */ void jlink_init(void); - -/* - * jlink_is_active(now_ms) - returns true if a valid frame arrived within - * JLINK_HB_TIMEOUT_MS. Returns false if no frame ever received. - */ bool jlink_is_active(uint32_t now_ms); - -/* - * jlink_send_telemetry(status) - build and transmit a JLINK_TLM_STATUS frame - * over USART1 TX (blocking, ~0.2ms at 921600). Call at JLINK_TLM_HZ. - */ -void jlink_send_telemetry(const jlink_tlm_status_t *status); - -/* - * jlink_process() - drain DMA circular buffer and parse frames. - * Call from main loop every iteration (not ISR). Lightweight: O(bytes_pending). - */ void jlink_process(void); - -/* - * jlink_send_power_telemetry(power) - build and transmit a JLINK_TLM_POWER - * frame (17 bytes) at PM_TLM_HZ. Call from main loop when not in STOP mode. - */ +void jlink_send_telemetry(const jlink_tlm_status_t *status); void jlink_send_power_telemetry(const jlink_tlm_power_t *power); - -/* - * jlink_send_pid_result(result) - build and transmit a JLINK_TLM_PID_RESULT - * frame (19 bytes) to confirm PID flash save outcome (Issue #531). - */ -void jlink_send_pid_result(const jlink_tlm_pid_result_t *result); - -/* - * jlink_send_battery_telemetry(batt) - build and transmit JLINK_TLM_BATTERY - * (0x82) frame (16 bytes) at BATTERY_ADC_PUBLISH_HZ (1 Hz). - * Called by battery_adc_publish(); not normally called directly. - */ void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt); +void jlink_send_pid_result(const jlink_tlm_pid_result_t *result); /* * jlink_send_gimbal_state(state) - transmit JLINK_TLM_GIMBAL_STATE (0x84) @@ -217,4 +218,18 @@ void jlink_send_battery_telemetry(const jlink_tlm_battery_t *batt); */ void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state); +/* + * jlink_send_sched_telemetry(tlm) - transmit JLINK_TLM_SCHED (0x85) in + * response to SCHED_GET. tlm->num_bands determines actual frame size. + * Issue #550. + */ +void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm); + +/* + * jlink_get_sched_set() - return pointer to the most-recently received + * SCHED_SET payload buffer (static storage in jlink.c). Main loop calls + * pid_schedule_set_table() from this buffer, then clears ready. Issue #550. + */ +JLinkSchedSetBuf *jlink_get_sched_set(void); + #endif /* JLINK_H */ diff --git a/jetson/ros2_ws/src/saltybot_health_monitor/config/sensor_health_params.yaml b/jetson/ros2_ws/src/saltybot_health_monitor/config/sensor_health_params.yaml new file mode 100644 index 0000000..f203362 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_health_monitor/config/sensor_health_params.yaml @@ -0,0 +1,66 @@ +sensor_health_node: + ros__parameters: + # Publish rate for DiagnosticArray and JSON summary + check_hz: 1.0 + + # MQTT broker for saltybot/health topic + mqtt_broker: "localhost" + mqtt_port: 1883 + mqtt_topic: "saltybot/health" + mqtt_enabled: true + +# Per-sensor thresholds and configuration +# Each entry: topic, name, warn_s (WARN threshold), error_s (ERROR threshold), critical +# +# critical=true: system cannot operate without this sensor +# warn_s: topic age (s) that triggers WARN level +# error_s: topic age (s) that triggers ERROR level + +sensors: + - topic: "/camera/color/image_raw" + name: "camera_color" + warn_s: 1.0 + error_s: 3.0 + critical: true + + - topic: "/camera/depth/image_rect_raw" + name: "camera_depth" + warn_s: 1.0 + error_s: 3.0 + critical: true + + - topic: "/camera/color/camera_info" + name: "camera_info" + warn_s: 2.0 + error_s: 5.0 + critical: false + + - topic: "/scan" + name: "lidar" + warn_s: 1.0 + error_s: 3.0 + critical: true + + - topic: "/imu/data" + name: "imu" + warn_s: 0.5 + error_s: 2.0 + critical: true + + - topic: "/saltybot/uwb/range" + name: "uwb" + warn_s: 2.0 + error_s: 5.0 + critical: false + + - topic: "/saltybot/battery" + name: "battery" + warn_s: 3.0 + error_s: 8.0 + critical: false + + - topic: "/saltybot/motor_daemon/status" + name: "motor_daemon" + warn_s: 2.0 + error_s: 5.0 + critical: true diff --git a/jetson/ros2_ws/src/saltybot_health_monitor/launch/sensor_health.launch.py b/jetson/ros2_ws/src/saltybot_health_monitor/launch/sensor_health.launch.py new file mode 100644 index 0000000..c333907 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_health_monitor/launch/sensor_health.launch.py @@ -0,0 +1,50 @@ +"""sensor_health.launch.py — Launch sensor health monitor node (Issue #566).""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + pkg = get_package_share_directory("saltybot_health_monitor") + + check_hz_arg = DeclareLaunchArgument( + "check_hz", + default_value="1.0", + description="Health check publish rate (Hz)", + ) + mqtt_broker_arg = DeclareLaunchArgument( + "mqtt_broker", + default_value="localhost", + description="MQTT broker hostname", + ) + mqtt_enabled_arg = DeclareLaunchArgument( + "mqtt_enabled", + default_value="true", + description="Enable MQTT publishing to saltybot/health", + ) + + sensor_health_node = Node( + package="saltybot_health_monitor", + executable="sensor_health_node", + name="sensor_health_node", + output="screen", + parameters=[ + os.path.join(pkg, "config", "sensor_health_params.yaml"), + { + "check_hz": LaunchConfiguration("check_hz"), + "mqtt_broker": LaunchConfiguration("mqtt_broker"), + "mqtt_enabled": LaunchConfiguration("mqtt_enabled"), + }, + ], + ) + + return LaunchDescription([ + check_hz_arg, + mqtt_broker_arg, + mqtt_enabled_arg, + sensor_health_node, + ]) diff --git a/jetson/ros2_ws/src/saltybot_health_monitor/package.xml b/jetson/ros2_ws/src/saltybot_health_monitor/package.xml index b49a494..95dab9b 100644 --- a/jetson/ros2_ws/src/saltybot_health_monitor/package.xml +++ b/jetson/ros2_ws/src/saltybot_health_monitor/package.xml @@ -1,27 +1,24 @@ - saltybot_health_monitor - 0.1.0 + 0.2.0 - ROS2 system health monitor for SaltyBot. Central node that monitors heartbeats - from all critical nodes, detects when nodes go down (>5s silent), triggers - auto-restart, publishes /saltybot/system_health JSON, and alerts face display - on critical failures. + ROS2 health monitor for SaltyBot. Tracks node heartbeats and sensor topic + staleness. Publishes DiagnosticArray on /saltybot/diagnostics and MQTT on + saltybot/health. Issue #566. - sl-controls - MIT + sl-jetson + Apache-2.0 rclpy std_msgs geometry_msgs + sensor_msgs + diagnostic_msgs ament_python - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest + pytest ament_python diff --git a/jetson/ros2_ws/src/saltybot_health_monitor/saltybot_health_monitor/sensor_health_node.py b/jetson/ros2_ws/src/saltybot_health_monitor/saltybot_health_monitor/sensor_health_node.py new file mode 100644 index 0000000..960132d --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_health_monitor/saltybot_health_monitor/sensor_health_node.py @@ -0,0 +1,385 @@ +#!/usr/bin/env python3 +"""sensor_health_node.py — ROS2 sensor topic health monitor (Issue #566). + +Monitors all sensor topics for staleness. Each sensor is checked against +configurable WARN and ERROR timeouts. Results are published as a ROS2 +DiagnosticArray and as an MQTT JSON summary. + +Monitored topics (configurable via sensor_health_params.yaml): + /camera/color/image_raw — RealSense colour stream + /camera/depth/image_rect_raw — RealSense depth stream + /camera/color/camera_info — RealSense camera info + /scan — LiDAR 2-D scan + /imu/data — IMU (BNO055 via JLink) + /saltybot/uwb/range — UWB ranging + /saltybot/battery — Battery telemetry (JSON string) + /saltybot/motor_daemon/status — Motor daemon status + +Published topics: + /saltybot/diagnostics (diagnostic_msgs/DiagnosticArray) — full per-sensor status + /saltybot/sensor_health (std_msgs/String) — JSON summary + +MQTT: + Topic: saltybot/health + Payload: JSON {timestamp, overall, sensors:{name: {status, age_s, hz}}} + +Parameters (config/sensor_health_params.yaml): + check_hz 1.0 Health check publish rate + mqtt_broker localhost + mqtt_port 1883 + mqtt_topic saltybot/health + mqtt_enabled true + sensors list of {topic, name, warn_s, error_s, critical} +""" + +from __future__ import annotations + +import json +import time +import threading +from dataclasses import dataclass, field +from typing import Dict, List, Optional + +import rclpy +from rclpy.node import Node +from rclpy.qos import ( + DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +) + +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue +from sensor_msgs.msg import CameraInfo, Image, Imu, LaserScan +from std_msgs.msg import String + + +# ── Diagnostic level aliases ─────────────────────────────────────────────────── + +OK = DiagnosticStatus.OK # 0 +WARN = DiagnosticStatus.WARN # 1 +ERROR = DiagnosticStatus.ERROR # 2 + +_LEVEL_NAMES = {OK: "OK", WARN: "WARN", ERROR: "ERROR"} + +# ── Default sensor configuration ────────────────────────────────────────────── + +DEFAULT_SENSORS: List[dict] = [ + {"topic": "/camera/color/image_raw", "name": "camera_color", "warn_s": 1.0, "error_s": 3.0, "critical": True}, + {"topic": "/camera/depth/image_rect_raw", "name": "camera_depth", "warn_s": 1.0, "error_s": 3.0, "critical": True}, + {"topic": "/camera/color/camera_info", "name": "camera_info", "warn_s": 2.0, "error_s": 5.0, "critical": False}, + {"topic": "/scan", "name": "lidar", "warn_s": 1.0, "error_s": 3.0, "critical": True}, + {"topic": "/imu/data", "name": "imu", "warn_s": 0.5, "error_s": 2.0, "critical": True}, + {"topic": "/saltybot/uwb/range", "name": "uwb", "warn_s": 2.0, "error_s": 5.0, "critical": False}, + {"topic": "/saltybot/battery", "name": "battery", "warn_s": 3.0, "error_s": 8.0, "critical": False}, + {"topic": "/saltybot/motor_daemon/status", "name": "motor_daemon", "warn_s": 2.0, "error_s": 5.0, "critical": True}, +] + + +# ── SensorWatcher ───────────────────────────────────────────────────────────── + +class SensorWatcher: + """Tracks message receipt timestamps and rate for a single topic. + + Thread-safe: callback may fire from any executor thread. + """ + + def __init__(self, topic: str, name: str, + warn_s: float, error_s: float, critical: bool) -> None: + self.topic = topic + self.name = name + self.warn_s = warn_s + self.error_s = error_s + self.critical = critical + + self._last_rx: float = 0.0 # monotonic; 0 = never received + self._count: int = 0 + self._rate_hz: float = 0.0 + self._rate_ts: float = time.monotonic() + self._rate_cnt: int = 0 + self._lock = threading.Lock() + + # ── Callback (called from subscription) ──────────────────────────────── + + def on_message(self, _msg) -> None: + """Record receipt of any message on this topic.""" + now = time.monotonic() + with self._lock: + self._last_rx = now + self._count += 1 + self._rate_cnt += 1 + # Update rate estimate every ~2 s + elapsed = now - self._rate_ts + if elapsed >= 2.0: + self._rate_hz = self._rate_cnt / elapsed + self._rate_cnt = 0 + self._rate_ts = now + + # ── Status query ─────────────────────────────────────────────────────── + + def age_s(self, now: Optional[float] = None) -> float: + """Seconds since last message (∞ if never received).""" + if now is None: + now = time.monotonic() + with self._lock: + if self._last_rx == 0.0: + return float("inf") + return now - self._last_rx + + def rate_hz(self) -> float: + with self._lock: + return self._rate_hz + + def msg_count(self) -> int: + with self._lock: + return self._count + + def level(self, now: Optional[float] = None) -> int: + age = self.age_s(now) + if age >= self.error_s: + return ERROR + if age >= self.warn_s: + return WARN + return OK + + def diagnostic_status(self, now: Optional[float] = None) -> DiagnosticStatus: + if now is None: + now = time.monotonic() + age = self.age_s(now) + lvl = self.level(now) + hz = self.rate_hz() + cnt = self.msg_count() + + if age == float("inf"): + msg = f"ERROR: no data received on {self.topic}" + elif lvl == ERROR: + msg = f"ERROR: stale {age:.1f}s (threshold {self.error_s:.1f}s)" + elif lvl == WARN: + msg = f"WARN: stale {age:.1f}s (threshold {self.warn_s:.1f}s)" + else: + msg = f"OK ({hz:.1f} Hz)" + + age_str = "inf" if age == float("inf") else f"{age:.2f}" + status = DiagnosticStatus() + status.level = lvl + status.name = self.name + status.message = msg + status.hardware_id = self.topic + status.values = [ + KeyValue(key="topic", value=self.topic), + KeyValue(key="age_s", value=age_str), + KeyValue(key="rate_hz", value=f"{hz:.2f}"), + KeyValue(key="count", value=str(cnt)), + KeyValue(key="warn_s", value=str(self.warn_s)), + KeyValue(key="error_s", value=str(self.error_s)), + KeyValue(key="critical", value=str(self.critical)), + ] + return status + + def summary_dict(self, now: Optional[float] = None) -> dict: + if now is None: + now = time.monotonic() + age = self.age_s(now) + return { + "status": _LEVEL_NAMES[self.level(now)], + "age_s": round(age, 2) if age != float("inf") else None, + "rate_hz": round(self.rate_hz(), 2), + "count": self.msg_count(), + "critical": self.critical, + } + + +# ── SensorHealthNode ─────────────────────────────────────────────────────────── + +class SensorHealthNode(Node): + """Monitor all sensor topics; publish DiagnosticArray + MQTT JSON.""" + + def __init__(self) -> None: + super().__init__("sensor_health_node") + + # ── Parameters ───────────────────────────────────────────────────── + self.declare_parameter("check_hz", 1.0) + self.declare_parameter("mqtt_broker", "localhost") + self.declare_parameter("mqtt_port", 1883) + self.declare_parameter("mqtt_topic", "saltybot/health") + self.declare_parameter("mqtt_enabled", True) + + check_hz = self.get_parameter("check_hz").value + self._mqtt_broker = self.get_parameter("mqtt_broker").value + self._mqtt_port = int(self.get_parameter("mqtt_port").value) + self._mqtt_topic = self.get_parameter("mqtt_topic").value + mqtt_enabled = self.get_parameter("mqtt_enabled").value + + # ── Build sensor watchers ─────────────────────────────────────────── + self._watchers: Dict[str, SensorWatcher] = {} + for cfg in DEFAULT_SENSORS: + w = SensorWatcher( + topic = cfg["topic"], + name = cfg["name"], + warn_s = float(cfg.get("warn_s", 1.0)), + error_s = float(cfg.get("error_s", 3.0)), + critical = bool(cfg.get("critical", False)), + ) + self._watchers[cfg["name"]] = w + + # ── Subscriptions — one per sensor type ──────────────────────────── + # Best-effort QoS for sensor data (sensors may use BEST_EFFORT publishers) + be_qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, depth=1, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + ) + rel_qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, depth=5, + reliability=ReliabilityPolicy.RELIABLE, + ) + + self._subscribe(Image, "/camera/color/image_raw", "camera_color", be_qos) + self._subscribe(Image, "/camera/depth/image_rect_raw", "camera_depth", be_qos) + self._subscribe(CameraInfo, "/camera/color/camera_info", "camera_info", be_qos) + self._subscribe(LaserScan, "/scan", "lidar", be_qos) + self._subscribe(Imu, "/imu/data", "imu", be_qos) + self._subscribe(String, "/saltybot/uwb/range", "uwb", rel_qos) + self._subscribe(String, "/saltybot/battery", "battery", rel_qos) + self._subscribe(String, "/saltybot/motor_daemon/status", "motor_daemon", rel_qos) + + # ── Publishers ───────────────────────────────────────────────────── + self._pub_diag = self.create_publisher( + DiagnosticArray, "/saltybot/diagnostics", 10) + self._pub_health = self.create_publisher( + String, "/saltybot/sensor_health", 10) + + # ── MQTT ─────────────────────────────────────────────────────────── + self._mqtt_client = None + if mqtt_enabled: + self._connect_mqtt() + + # ── Timer ────────────────────────────────────────────────────────── + self.create_timer(1.0 / max(0.1, check_hz), self._publish_diagnostics) + + sensor_list = ", ".join(self._watchers.keys()) + self.get_logger().info( + f"[sensor_health] monitoring {len(self._watchers)} sensors: {sensor_list}" + ) + + # ── Subscription helper ──────────────────────────────────────────────── + + def _subscribe(self, msg_type, topic: str, name: str, qos) -> None: + if name not in self._watchers: + return + watcher = self._watchers[name] + self.create_subscription(msg_type, topic, watcher.on_message, qos) + + # ── MQTT ─────────────────────────────────────────────────────────────── + + def _connect_mqtt(self) -> None: + try: + import paho.mqtt.client as mqtt # type: ignore + self._mqtt_client = mqtt.Client(client_id="saltybot_sensor_health") + self._mqtt_client.on_connect = self._on_mqtt_connect + self._mqtt_client.connect_async(self._mqtt_broker, self._mqtt_port, 60) + self._mqtt_client.loop_start() + except ImportError: + self.get_logger().warn( + "[sensor_health] paho-mqtt not installed — MQTT publishing disabled" + ) + except Exception as e: + self.get_logger().warn(f"[sensor_health] MQTT connect failed: {e}") + + def _on_mqtt_connect(self, client, userdata, flags, rc) -> None: + if rc == 0: + self.get_logger().info( + f"[sensor_health] MQTT connected to {self._mqtt_broker}:{self._mqtt_port}" + ) + else: + self.get_logger().warn(f"[sensor_health] MQTT connect rc={rc}") + + def _publish_mqtt(self, payload: str) -> None: + if self._mqtt_client is None: + return + try: + self._mqtt_client.publish(self._mqtt_topic, payload, qos=0, retain=True) + except Exception as e: + self.get_logger().warn(f"[sensor_health] MQTT publish error: {e}") + + # ── Diagnostic publisher ─────────────────────────────────────────────── + + def _publish_diagnostics(self) -> None: + now = time.monotonic() + wall = time.time() + + # Build DiagnosticArray + diag_array = DiagnosticArray() + diag_array.header.stamp = self.get_clock().now().to_msg() + + sensor_summaries: dict = {} + worst_level = OK + + for name, watcher in self._watchers.items(): + ds = watcher.diagnostic_status(now) + diag_array.status.append(ds) + + if ds.level > worst_level: + worst_level = ds.level + + sensor_summaries[name] = watcher.summary_dict(now) + + # Overall system status + n_error = sum(1 for w in self._watchers.values() if w.level(now) == ERROR) + n_warn = sum(1 for w in self._watchers.values() if w.level(now) == WARN) + crit_err = [n for n, w in self._watchers.items() + if w.critical and w.level(now) == ERROR] + + overall = "OK" + if crit_err: + overall = "ERROR" + elif n_error > 0: + overall = "ERROR" + elif n_warn > 0: + overall = "WARN" + + # Publish DiagnosticArray + self._pub_diag.publish(diag_array) + + # JSON summary + summary = { + "timestamp": wall, + "overall": overall, + "n_ok": len(self._watchers) - n_error - n_warn, + "n_warn": n_warn, + "n_error": n_error, + "critical_err": crit_err, + "sensors": sensor_summaries, + } + payload = json.dumps(summary) + self._pub_health.publish(String(data=payload)) + self._publish_mqtt(payload) + + # Log on transitions or errors + if worst_level >= ERROR: + self.get_logger().warn( + f"[sensor_health] {overall}: {n_error} error(s), {n_warn} warn(s)" + + (f" — critical: {crit_err}" if crit_err else "") + ) + + def destroy_node(self) -> None: + if self._mqtt_client is not None: + try: + self._mqtt_client.loop_stop() + self._mqtt_client.disconnect() + except Exception: + pass + super().destroy_node() + + +def main(args=None) -> None: + rclpy.init(args=args) + node = SensorHealthNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_health_monitor/setup.py b/jetson/ros2_ws/src/saltybot_health_monitor/setup.py index 1b08bbb..5179230 100644 --- a/jetson/ros2_ws/src/saltybot_health_monitor/setup.py +++ b/jetson/ros2_ws/src/saltybot_health_monitor/setup.py @@ -4,27 +4,34 @@ package_name = "saltybot_health_monitor" setup( name=package_name, - version="0.1.0", + version="0.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), (f"share/{package_name}", ["package.xml"]), - (f"share/{package_name}/launch", ["launch/health_monitor.launch.py"]), - (f"share/{package_name}/config", ["config/health_config.yaml"]), + (f"share/{package_name}/launch", [ + "launch/health_monitor.launch.py", + "launch/sensor_health.launch.py", + ]), + (f"share/{package_name}/config", [ + "config/health_config.yaml", + "config/sensor_health_params.yaml", + ]), ], - install_requires=["setuptools", "pyyaml"], + install_requires=["setuptools", "pyyaml", "paho-mqtt"], zip_safe=True, - maintainer="sl-controls", - maintainer_email="sl-controls@saltylab.local", + maintainer="sl-jetson", + maintainer_email="sl-jetson@saltylab.local", description=( - "System health monitor: tracks node heartbeats, detects down nodes, " - "triggers auto-restart, publishes system health status" + "System health monitor: node heartbeats + sensor topic staleness " + "detection with DiagnosticArray and MQTT (Issue #566)" ), - license="MIT", + license="Apache-2.0", tests_require=["pytest"], entry_points={ "console_scripts": [ "health_monitor_node = saltybot_health_monitor.health_monitor_node:main", + "sensor_health_node = saltybot_health_monitor.sensor_health_node:main", ], }, ) diff --git a/jetson/ros2_ws/src/saltybot_health_monitor/test/test_sensor_health.py b/jetson/ros2_ws/src/saltybot_health_monitor/test/test_sensor_health.py new file mode 100644 index 0000000..0ea909a --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_health_monitor/test/test_sensor_health.py @@ -0,0 +1,344 @@ +"""test_sensor_health.py — Unit tests for sensor_health_node (Issue #566). + +Runs entirely offline: no ROS2 runtime, no hardware, no MQTT broker required. +""" + +import json +import sys +import time +import types +import unittest +from unittest.mock import MagicMock, patch + +# ── Stub ROS2 and sensor_msgs so tests run offline ──────────────────────────── + +def _install_stubs(): + # rclpy + rclpy = types.ModuleType("rclpy") + rclpy.init = lambda **_: None + rclpy.spin = lambda _: None + rclpy.shutdown = lambda: None + + node_mod = types.ModuleType("rclpy.node") + class _Node: + def __init__(self, *a, **kw): pass + def declare_parameter(self, *a, **kw): pass + def get_parameter(self, name): + defaults = { + "check_hz": 1.0, "mqtt_broker": "localhost", + "mqtt_port": 1883, "mqtt_topic": "saltybot/health", + "mqtt_enabled": False, + } + m = MagicMock(); m.value = defaults.get(name, False) + return m + def get_logger(self): return MagicMock() + def get_clock(self): return MagicMock() + def create_subscription(self, *a, **kw): pass + def create_publisher(self, *a, **kw): return MagicMock() + def create_timer(self, *a, **kw): pass + def destroy_node(self): pass + node_mod.Node = _Node + rclpy.node = node_mod + + qos_mod = types.ModuleType("rclpy.qos") + for attr in ("QoSProfile", "HistoryPolicy", "ReliabilityPolicy", "DurabilityPolicy"): + setattr(qos_mod, attr, MagicMock()) + rclpy.qos = qos_mod + + sys.modules.setdefault("rclpy", rclpy) + sys.modules.setdefault("rclpy.node", rclpy.node) + sys.modules.setdefault("rclpy.qos", rclpy.qos) + + # diagnostic_msgs + diag = types.ModuleType("diagnostic_msgs") + diag_msg = types.ModuleType("diagnostic_msgs.msg") + + class _DiagStatus: + OK = 0 + WARN = 1 + ERROR = 2 + def __init__(self): + self.level = 0; self.name = ""; self.message = "" + self.hardware_id = ""; self.values = [] + + class _DiagArray: + def __init__(self): + self.header = MagicMock(); self.status = [] + + class _KeyValue: + def __init__(self, key="", value=""): + self.key = key; self.value = value + + diag_msg.DiagnosticStatus = _DiagStatus + diag_msg.DiagnosticArray = _DiagArray + diag_msg.KeyValue = _KeyValue + diag.msg = diag_msg + sys.modules.setdefault("diagnostic_msgs", diag) + sys.modules.setdefault("diagnostic_msgs.msg", diag_msg) + + # sensor_msgs + sens = types.ModuleType("sensor_msgs") + sens_msg = types.ModuleType("sensor_msgs.msg") + for cls_name in ("Image", "CameraInfo", "Imu", "LaserScan"): + setattr(sens_msg, cls_name, MagicMock) + sens.msg = sens_msg + sys.modules.setdefault("sensor_msgs", sens) + sys.modules.setdefault("sensor_msgs.msg", sens_msg) + + # std_msgs + std = types.ModuleType("std_msgs") + std_msg = types.ModuleType("std_msgs.msg") + class _String: + def __init__(self, data=""): self.data = data + std_msg.String = _String + std.msg = std_msg + sys.modules.setdefault("std_msgs", std) + sys.modules.setdefault("std_msgs.msg", std_msg) + + +_install_stubs() + +from saltybot_health_monitor.sensor_health_node import ( # noqa: E402 + SensorWatcher, OK, WARN, ERROR, _LEVEL_NAMES, +) + + +# ── SensorWatcher: initial state ────────────────────────────────────────────── + +class TestSensorWatcherInitial(unittest.TestCase): + + def setUp(self): + self.w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=True) + + def test_initial_age_is_inf(self): + self.assertEqual(self.w.age_s(), float("inf")) + + def test_initial_level_is_error(self): + # Never received → age=inf ≥ error_s → ERROR + self.assertEqual(self.w.level(), ERROR) + + def test_initial_count_zero(self): + self.assertEqual(self.w.msg_count(), 0) + + def test_initial_rate_zero(self): + self.assertAlmostEqual(self.w.rate_hz(), 0.0) + + def test_name_stored(self): + self.assertEqual(self.w.name, "lidar") + + def test_topic_stored(self): + self.assertEqual(self.w.topic, "/scan") + + def test_critical_stored(self): + self.assertTrue(self.w.critical) + + +# ── SensorWatcher: after receiving messages ─────────────────────────────────── + +class TestSensorWatcherAfterMessage(unittest.TestCase): + + def setUp(self): + self.w = SensorWatcher("/imu/data", "imu", warn_s=0.5, error_s=2.0, critical=True) + self.w.on_message(None) # simulate message receipt + + def test_age_is_small_after_message(self): + self.assertLess(self.w.age_s(), 0.1) + + def test_level_ok_immediately_after(self): + self.assertEqual(self.w.level(), OK) + + def test_count_increments(self): + self.w.on_message(None) + self.assertEqual(self.w.msg_count(), 2) + + def test_multiple_messages(self): + for _ in range(10): + self.w.on_message(None) + self.assertEqual(self.w.msg_count(), 11) + + +# ── SensorWatcher: staleness thresholds ────────────────────────────────────── + +class TestSensorWatcherStaleness(unittest.TestCase): + + def _make_stale(self, seconds_ago: float) -> SensorWatcher: + """Return a watcher whose last_rx was `seconds_ago` seconds in the past.""" + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + # Backdate the last_rx directly + with w._lock: + w._last_rx -= seconds_ago + return w + + def test_ok_when_fresh(self): + w = self._make_stale(0.5) + self.assertEqual(w.level(), OK) + + def test_warn_at_warn_threshold(self): + w = self._make_stale(1.1) + self.assertEqual(w.level(), WARN) + + def test_error_at_error_threshold(self): + w = self._make_stale(3.1) + self.assertEqual(w.level(), ERROR) + + def test_age_matches_backdated_time(self): + w = self._make_stale(2.0) + self.assertAlmostEqual(w.age_s(), 2.0, delta=0.1) + + def test_warn_level_between_thresholds(self): + w = self._make_stale(2.0) # 1.0 < 2.0 < 3.0 + self.assertEqual(w.level(), WARN) + + +# ── SensorWatcher: diagnostic_status output ────────────────────────────────── + +class TestSensorWatcherDiagStatus(unittest.TestCase): + + def test_never_received_is_error(self): + w = SensorWatcher("/camera/color/image_raw", "camera_color", + warn_s=1.0, error_s=3.0, critical=True) + ds = w.diagnostic_status() + self.assertEqual(ds.level, ERROR) + self.assertIn("no data", ds.message) + + def test_ok_status_message(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + ds = w.diagnostic_status() + self.assertEqual(ds.level, OK) + self.assertIn("OK", ds.message) + + def test_warn_status_message(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + with w._lock: + w._last_rx -= 1.5 + ds = w.diagnostic_status() + self.assertEqual(ds.level, WARN) + self.assertIn("WARN", ds.message) + + def test_hardware_id_is_topic(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + ds = w.diagnostic_status() + self.assertEqual(ds.hardware_id, "/scan") + + def test_kv_keys_present(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + ds = w.diagnostic_status() + kv_keys = {kv.key for kv in ds.values} + for expected in ("topic", "age_s", "rate_hz", "count", "warn_s", "error_s"): + self.assertIn(expected, kv_keys) + + def test_age_inf_displayed_as_inf(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + ds = w.diagnostic_status() + kv = {kv.key: kv.value for kv in ds.values} + self.assertEqual(kv["age_s"], "inf") + + +# ── SensorWatcher: summary_dict output ─────────────────────────────────────── + +class TestSensorWatcherSummaryDict(unittest.TestCase): + + def test_never_received_age_is_none(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + d = w.summary_dict() + self.assertIsNone(d["age_s"]) + self.assertEqual(d["status"], "ERROR") + + def test_ok_status_string(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + d = w.summary_dict() + self.assertEqual(d["status"], "OK") + + def test_warn_status_string(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + with w._lock: + w._last_rx -= 1.5 + d = w.summary_dict() + self.assertEqual(d["status"], "WARN") + + def test_error_status_string(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + with w._lock: + w._last_rx -= 5.0 + d = w.summary_dict() + self.assertEqual(d["status"], "ERROR") + + def test_age_rounded(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + d = w.summary_dict() + self.assertIsInstance(d["age_s"], float) + + def test_critical_flag(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=True) + self.assertTrue(w.summary_dict()["critical"]) + + def test_non_critical_flag(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + self.assertFalse(w.summary_dict()["critical"]) + + def test_count_in_summary(self): + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + for _ in range(5): + w.on_message(None) + self.assertEqual(w.summary_dict()["count"], 5) + + +# ── Level name mapping ───────────────────────────────────────────────────────── + +class TestLevelNames(unittest.TestCase): + + def test_ok_name(self): + self.assertEqual(_LEVEL_NAMES[OK], "OK") + + def test_warn_name(self): + self.assertEqual(_LEVEL_NAMES[WARN], "WARN") + + def test_error_name(self): + self.assertEqual(_LEVEL_NAMES[ERROR], "ERROR") + + +# ── Thread safety ───────────────────────────────────────────────────────────── + +class TestSensorWatcherThreadSafety(unittest.TestCase): + + def test_concurrent_messages(self): + import threading + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + N = 500 + threads = [threading.Thread(target=w.on_message, args=(None,)) for _ in range(N)] + for t in threads: + t.start() + for t in threads: + t.join() + self.assertEqual(w.msg_count(), N) + + def test_concurrent_reads(self): + import threading + w = SensorWatcher("/scan", "lidar", warn_s=1.0, error_s=3.0, critical=False) + w.on_message(None) + errors = [] + def read_loop(): + for _ in range(100): + try: + w.level() + w.age_s() + w.rate_hz() + except Exception as e: + errors.append(e) + threads = [threading.Thread(target=read_loop) for _ in range(5)] + for t in threads: t.start() + for t in threads: t.join() + self.assertEqual(errors, []) + + +if __name__ == "__main__": + unittest.main()