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

feat: ROS2 sensor health monitor (Issue #566) #572

Merged
sl-jetson merged 2 commits from sl-jetson/issue-566-health-monitor into main 2026-03-14 11:49:56 -04:00
Conversation 0 Commits 2 Files Changed 8 +1342 -290
8 changed files with 1342 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
// 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);
}
}

113
include/jlink.h
View File

@ -3,9 +3,10 @@
#include <stdint.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
* 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 */

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-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>saltybot_health_monitor</name>
<version>0.1.0</version>
<version>0.2.0</version>
<description>
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.
</description>
<maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
<license>MIT</license>
<maintainer email="sl-jetson@saltylab.local">sl-jetson</maintainer>
<license>Apache-2.0</license>
<depend>rclpy</depend>
<depend>std_msgs</depend>
<depend>geometry_msgs</depend>
<depend>sensor_msgs</depend>
<depend>diagnostic_msgs</depend>
<buildtool_depend>ament_python</buildtool_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>
<test_depend>pytest</test_depend>
<export>
<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(
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",
],
},
)

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