From 929c9ecd74628aa8cc7167b9b005212be8988801 Mon Sep 17 00:00:00 2001 From: sl-mechanical Date: Sat, 14 Mar 2026 11:51:40 -0400 Subject: [PATCH 1/2] feat: UWB anchor mount bracket (Issue #564) --- chassis/uwb_anchor_mount.scad | 464 +++++++++++++--------------------- 1 file changed, 171 insertions(+), 293 deletions(-) diff --git a/chassis/uwb_anchor_mount.scad b/chassis/uwb_anchor_mount.scad index 8213f04..6db7ed5 100644 --- a/chassis/uwb_anchor_mount.scad +++ b/chassis/uwb_anchor_mount.scad @@ -11,38 +11,38 @@ // coverage zone. // // 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 +// Wall base -> flat backplate with 2x screw holes (wall or ceiling) +// Tilt knuckle -> single-axis articulating joint; 15deg detent steps +// locked with M3 nyloc bolt; range 0-90deg +// 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() +// 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 +// 2x M4 x 30mm wood screws (or #6 drywall screws) wall fasteners +// 1x M3 x 20mm SHCS + M3 nyloc nut tilt pivot bolt +// 4x M2.5 x 8mm SHCS PCB-to-cradle +// 4x M2.5 hex nuts captured in standoffs +// 1x 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 +// PCB size : UWB_L x UWB_W x UWB_H (55 x 28 x 10 mm default) +// Mounting holes : M2.5, 4x corners on UWB_HOLE_X x UWB_HOLE_Y pattern +// USB-C port : centred on short edge, UWB_USBC_W x UWB_USBC_H +// Antenna area : top face rear half -- 10mm 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) +// Tilt angles (15deg detent steps, set TILT_DEG before export): +// 0deg -> horizontal face-up (ceiling, antenna faces down) +// 30deg -> 30deg downward tilt (wall near ceiling) [default] +// 45deg -> diagonal (wall mid-height) +// 90deg -> vertical face-out (wall, antenna faces forward) // // RENDER options: // "assembly" full assembly at TILT_DEG (default) @@ -61,40 +61,40 @@ $fn = 64; e = 0.01; -// ── Tilt angle (override per anchor, 0–90°, 15° steps) ─────────────────────── +// -- Tilt angle (override per anchor, 0-90deg, 15deg 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) +// -- ESP32 UWB Pro PCB dimensions (verify with calipers) --------------------- +UWB_L = 55.0; +UWB_W = 28.0; +UWB_H = 10.0; +UWB_HOLE_X = 47.5; +UWB_HOLE_Y = 21.0; +UWB_USBC_W = 9.5; +UWB_USBC_H = 4.0; +UWB_ANTENNA_L = 20.0; -// ── 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) +// -- Wall base geometry ------------------------------------------------------- +BASE_W = 60.0; +BASE_H = 50.0; +BASE_T = 5.0; +BASE_SCREW_D = 4.5; +BASE_SCREW_HD = 8.5; +BASE_SCREW_HH = 3.5; +BASE_SCREW_SPC = 35.0; +KNUCKLE_T = BASE_T + 4.0; -// ── Tilt arm geometry ───────────────────────────────────────────────────────── +// -- Tilt arm geometry -------------------------------------------------------- ARM_W = 12.0; ARM_T = 5.0; ARM_L = 35.0; -PIVOT_D = 3.3; // M3 clearance +PIVOT_D = 3.3; 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 +DETENT_D = 3.2; +DETENT_R = 8.0; -// ── Anchor cradle geometry ──────────────────────────────────────────────────── +// -- Anchor cradle geometry --------------------------------------------------- CRADLE_WALL_T = 3.5; CRADLE_BACK_T = 4.0; CRADLE_FLOOR_T = 3.0; @@ -104,19 +104,19 @@ 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 +LABEL_T = 1.2; -// ── USB-C cable routing ─────────────────────────────────────────────────────── +// -- USB-C routing ------------------------------------------------------------ USBC_CHAN_W = 11.0; USBC_CHAN_H = 7.0; -// ── Cable clip ──────────────────────────────────────────────────────────────── +// -- Cable clip --------------------------------------------------------------- CLIP_CABLE_D = 4.5; CLIP_T = 2.0; CLIP_BODY_W = 16.0; CLIP_BODY_H = 10.0; -// ── Fasteners ───────────────────────────────────────────────────────────────── +// -- Fasteners ---------------------------------------------------------------- M2P5_D = 2.7; M3_D = 3.3; M3_NUT_AF = 5.5; @@ -137,61 +137,35 @@ else if (RENDER == "cable_clip_stl") cable_clip(); // 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]); - - // Wall base - color("OliveDrab", 0.85) - wall_base(); - - // Tilt arm at TILT_DEG, pivoting at knuckle + color("OliveDrab", 0.85) wall_base(); color("SteelBlue", 0.85) - translate([0, KNUCKLE_T, 0]) - rotate([TILT_DEG, 0, 0]) - tilt_arm(); - - // Anchor cradle at arm end + translate([0, KNUCKLE_T, 0]) rotate([TILT_DEG,0,0]) tilt_arm(); color("DarkSlateGray", 0.85) - translate([0, KNUCKLE_T, 0]) - rotate([TILT_DEG, 0, 0]) - translate([0, ARM_T, ARM_L]) - anchor_cradle(); - - // PCB ghost + translate([0, KNUCKLE_T, 0]) rotate([TILT_DEG,0,0]) + translate([0, ARM_T, ARM_L]) anchor_cradle(); %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]); - - // Cable clip at arm mid-point + 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]); 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(); + 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(); } // ============================================================ -// PART 1 — WALL BASE +// 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. +// Flat backplate, 2x countersunk M4/#6 wood screws on 35mm centres. +// Two pivot ears straddle the tilt arm; M3 pivot bolt through both. +// Detent arc on +X ear inner face: 7 notches at 15deg steps (0-90deg). +// Shallow rear recess for installation-zone label strip. +// Same part for wall mount and ceiling mount. // -// 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. +// Print: backplate flat on bed, PETG, 5 perims, 40% gyroid. module wall_base() { ear_h = ARM_W + 3.0; ear_t = 6.0; @@ -199,144 +173,92 @@ module wall_base() { difference() { union() { - // ── Backplate ──────────────────────────────────────────────── translate([-BASE_W/2, -BASE_T, -BASE_H/2]) cube([BASE_W, BASE_T, BASE_H]); - - // ── 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]); - - // ── Stiffening gussets ──────────────────────────────────────── + translate([ex, -BASE_T+e, -ear_h/2]) + cube([ear_t, KNUCKLE_T+e, ear_h]); 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]); + cube([ear_t, BASE_T-1, ear_h/2]); + translate([ex + (ex<0 ? ear_t*0.5 : 0), -BASE_T, -ear_h/6]) + cube([ear_t*0.5, 1, ear_h/3]); } } - - // ── 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); + 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); } - - // ── 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); - - // ── 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); - - // ── Detent arc — 7 notches at 15° steps on +X ear inner face ───── + 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); + 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); 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); - - // ── 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); + 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); + translate([0, -BASE_T-e, 0]) rotate([-90,0,0]) + cube([BASE_W-12, BASE_H-16, 1.6], center=true); + translate([0, -BASE_T+1.5, 0]) + cube([BASE_W-14, BASE_T-3, BASE_H-20], center=true); } } // ============================================================ -// PART 2 — TILT ARM +// 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. +// Pivoting arm linking wall_base ears to anchor_cradle. +// Knuckle (Z=0): M3 pivot bore + spring-plunger detent pocket (3mm). +// Cradle end (Z=ARM_L): 2x M3 bolt attachment stub. +// USB-C cable channel groove on outer +Y face, full arm length. // -// Print: knuckle face flat on bed, PETG, 5 perims, 40 % gyroid. +// Print: knuckle face flat on bed, PETG, 5 perims, 40% gyroid. module tilt_arm() { total_h = ARM_L + 10; - difference() { union() { - // ── Arm body ───────────────────────────────────────────────── - translate([-ARM_W/2, 0, 0]) - cube([ARM_W, ARM_T, total_h]); - - // ── Knuckle boss (rounded pivot end) ───────────────────────── - translate([0, ARM_T/2, 0]) - rotate([90, 0, 0]) - cylinder(d = ARM_W, h = ARM_T, center = true); - - // ── Cradle attach stub (Z = ARM_L) ──────────────────────────── + translate([-ARM_W/2, 0, 0]) cube([ARM_W, ARM_T, total_h]); + translate([0, ARM_T/2, 0]) rotate([90,0,0]) + cylinder(d=ARM_W, h=ARM_T, center=true); translate([-ARM_W/2, 0, ARM_L]) - cube([ARM_W, ARM_T + CRADLE_BACK_T, ARM_T]); + cube([ARM_W, ARM_T+CRADLE_BACK_T, ARM_T]); } - - // ── 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); - - // ── 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-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]); - - // ── Cradle attach bolt holes (2× M3 at cradle stub) ─────────────── + translate([-ARM_W/2-e, ARM_T/2, 0]) rotate([0,90,0]) + cylinder(d=PIVOT_D, h=ARM_W+2*e); + translate([0, ARM_T+e, 0]) rotate([90,0,0]) + cylinder(d=3.2, h=4+e); + translate([-USBC_CHAN_W/2, ARM_T-e, ARM_T+4]) + cube([USBC_CHAN_W, USBC_CHAN_H, ARM_L-ARM_T-8]); 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) ───────────────────────── + 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); 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([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); translate([0, ARM_T/2, ARM_L/2]) - cube([ARM_W - 4, ARM_T - 2, ARM_L - 18], center = true); + cube([ARM_W-4, ARM_T-2, ARM_L-18], center=true); } } // ============================================================ -// PART 3 — ANCHOR CRADLE +// 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. +// 4x M2.5 standoffs on UWB_HOLE_X x UWB_HOLE_Y pattern. +// Back wall: USB-C exit slot + routing groove, label card slot, +// antenna keep-out cutout (material removed above antenna area). +// Front retaining lip prevents PCB sliding out. +// Two attachment tabs bolt to tilt_arm cradle stub via M3. // -// Print: back wall flat on bed, PETG, 5 perims, 40 % gyroid. +// Label card slot: insert paper/laminate strip to ID this anchor +// (e.g. "UWB-A3 NE-CORNER"), accessible from open cradle end. +// +// 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; @@ -345,119 +267,75 @@ module anchor_cradle() { difference() { union() { - // ── Cradle body ─────────────────────────────────────────────── - translate([-outer_l/2, 0, 0]) - cube([outer_l, outer_w, total_z]); - - // ── Front retaining lip ─────────────────────────────────────── - translate([-outer_l/2, outer_w - CRADLE_LIP_T, 0]) + translate([-outer_l/2, 0, 0]) cube([outer_l, outer_w, total_z]); + 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]); + 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 ──────────────────────── + translate([-UWB_L/2, 0, pcb_z]) cube([UWB_L, UWB_W+1, UWB_H+4]); + 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]); + 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]); + translate([0, -CRADLE_BACK_T-e, pcb_z+UWB_H/2]) + cube([LABEL_W, LABEL_T+0.3, LABEL_H], center=[true,false,false]); + 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]); 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]); + 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); + 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]) + 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); + 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); } } // ============================================================ -// PART 4 — CABLE CLIP +// 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. +// Presses onto ARM_T-wide arm with flexible PETG snap tongues. +// Print x2-3 per anchor, spaced 25mm along arm. // -// Print: clip-opening face down, PETG, 3 perims, 20 % infill. +// 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; - difference() { union() { - // ── Body plate ──────────────────────────────────────────────── translate([-CLIP_BODY_W/2, 0, 0]) cube([CLIP_BODY_W, CLIP_T, CLIP_BODY_H]); - - // ── 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); - } - - // ── 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]); - - // ── 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); + 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); + translate([0, ch_r+e, 0]) + cube([CLIP_CABLE_D*0.85, ch_r*2+2*e, CLIP_BODY_W+2*e], + center=true); + } + 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]); + 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); } - - // ── 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); + translate([0, -ARM_T-1-e, CLIP_BODY_H/2]) + cube([CLIP_BODY_W-6, ARM_T+2, CLIP_BODY_H-4], center=true); } } From 4035b4cfc3db91837b9e703ebd1f7bf218b2b37d Mon Sep 17 00:00:00 2001 From: sl-jetson Date: Sat, 14 Mar 2026 11:57:57 -0400 Subject: [PATCH 2/2] feat: ROS2 launch orchestrator for full SaltyBot bringup (Issue #577) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds saltybot_bringup.launch.py with ordered startup groups (drivers→ perception→navigation→UI), timer-based health gates, configurable profiles (minimal/full/debug), and estop on Ctrl-C shutdown. Also adds launch_profiles.py dataclass module and 53-test coverage for profile hierarchy, timing gates, safety bounds, and to_dict serialization. Co-Authored-By: Claude Sonnet 4.6 --- .../launch/saltybot_bringup.launch.py | 591 ++++++++++++++++++ .../saltybot_bringup/launch_profiles.py | 187 ++++++ .../test/test_launch_orchestrator.py | 326 ++++++++++ 3 files changed, 1104 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_bringup/launch/saltybot_bringup.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/launch_profiles.py create mode 100644 jetson/ros2_ws/src/saltybot_bringup/test/test_launch_orchestrator.py diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/saltybot_bringup.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/saltybot_bringup.launch.py new file mode 100644 index 0000000..c8e7b91 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/launch/saltybot_bringup.launch.py @@ -0,0 +1,591 @@ +"""saltybot_bringup.launch.py — Unified ROS2 launch orchestrator (Issue #577). + +Ordered startup in four dependency groups with configurable profiles, +hardware-presence conditionals, inter-group health-gate delays, and +graceful-shutdown event handlers. + +Profiles +──────── + minimal Drivers only — safe teleoperation, no AI (boot ~4 s) + full Complete autonomous stack — SLAM + Nav2 + perception + audio (boot ~22 s) + debug Full + RViz + verbose logs + bag recorder (boot ~26 s) + +Usage +──────── + ros2 launch saltybot_bringup saltybot_bringup.launch.py + ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=minimal + ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=debug + ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=full stm32_port:=/dev/ttyUSB0 + +Startup sequence +──────────────── + GROUP A — Drivers t= 0 s STM32 bridge, RealSense+RPLIDAR, motor daemon, IMU + health gate ───────────────────────────────────────────────── t= 8 s (full/debug) + GROUP B — Perception t= 8 s UWB, person detection, object detection, depth costmap, gimbal + health gate ───────────────────────────────────────────────── t=16 s (full/debug) + GROUP C — Navigation t=16 s SLAM, Nav2, lidar avoidance, follower, docking + health gate ───────────────────────────────────────────────── t=22 s (full/debug) + GROUP D — UI/Social t=22 s rosbridge, audio pipeline, social personality, CSI cameras, diagnostics + +Shutdown +──────── + All groups include OnShutdown handlers that: + - Publish /saltybot/estop=true via ros2 topic pub (one-shot) + - Wait 1 s for bag recorder to flush + +Hardware conditionals +───────────────────── + Missing devices (stm32_port, uwb_port_a/b, gimbal_port) skip that driver. + All conditionals are evaluated at launch time via PathJoinSubstitution + IfCondition. +""" + +from __future__ import annotations + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + EmitEvent, + ExecuteProcess, + GroupAction, + IncludeLaunchDescription, + LogInfo, + RegisterEventHandler, + SetEnvironmentVariable, + TimerAction, +) +from launch.conditions import IfCondition, LaunchConfigurationEquals +from launch.event_handlers import OnShutdown +from launch.events import Shutdown +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +# ── Helpers ──────────────────────────────────────────────────────────────────── + +def _pkg(name: str) -> str: + return get_package_share_directory(name) + + +def _launch(pkg: str, *parts: str) -> PythonLaunchDescriptionSource: + return PythonLaunchDescriptionSource(os.path.join(_pkg(pkg), *parts)) + + +def _profile_flag(flag_name: str, profile_value_map: dict): + """PythonExpression that evaluates flag_name from a profile-keyed dict.""" + # e.g. {'minimal': 'false', 'full': 'true', 'debug': 'true'} + return PythonExpression([ + repr(profile_value_map), + ".get('", LaunchConfiguration("profile"), "', 'false')", + ]) + + +def _if_profile_flag(flag_name: str, profiles_with_flag: list): + """IfCondition: true when profile is in profiles_with_flag.""" + profile_set = repr(set(profiles_with_flag)) + return IfCondition( + PythonExpression(["'", LaunchConfiguration("profile"), f"' in {profile_set}"]) + ) + + +# ── Launch description ───────────────────────────────────────────────────────── + +def generate_launch_description() -> LaunchDescription: # noqa: C901 + + # ── Arguments ───────────────────────────────────────────────────────────── + + profile_arg = DeclareLaunchArgument( + "profile", + default_value="full", + choices=["minimal", "full", "debug"], + description=( + "Launch profile. " + "minimal: drivers+sensors only (no AI); " + "full: complete autonomous stack; " + "debug: full + RViz + verbose logs + bag recorder." + ), + ) + + use_sim_time_arg = DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use /clock from rosbag/simulator", + ) + + stm32_port_arg = DeclareLaunchArgument( + "stm32_port", + default_value="/dev/stm32-bridge", + description="STM32 USART bridge serial device", + ) + + uwb_port_a_arg = DeclareLaunchArgument( + "uwb_port_a", + default_value="/dev/uwb-anchor0", + description="UWB anchor-0 serial device", + ) + + uwb_port_b_arg = DeclareLaunchArgument( + "uwb_port_b", + default_value="/dev/uwb-anchor1", + description="UWB anchor-1 serial device", + ) + + gimbal_port_arg = DeclareLaunchArgument( + "gimbal_port", + default_value="/dev/ttyTHS1", + description="Gimbal JLINK serial device", + ) + + max_linear_vel_arg = DeclareLaunchArgument( + "max_linear_vel", + default_value="0.5", + description="Max forward velocity cap (m/s)", + ) + + follow_distance_arg = DeclareLaunchArgument( + "follow_distance", + default_value="1.5", + description="Person-follower target distance (m)", + ) + + # ── Substitution handles ─────────────────────────────────────────────────── + + profile = LaunchConfiguration("profile") + use_sim_time = LaunchConfiguration("use_sim_time") + stm32_port = LaunchConfiguration("stm32_port") + uwb_port_a = LaunchConfiguration("uwb_port_a") + uwb_port_b = LaunchConfiguration("uwb_port_b") + gimbal_port = LaunchConfiguration("gimbal_port") + max_linear_vel = LaunchConfiguration("max_linear_vel") + follow_distance = LaunchConfiguration("follow_distance") + + # Profile membership helpers + _full_profiles = ["full", "debug"] + _debug_profiles = ["debug"] + + # ── Graceful shutdown handler ────────────────────────────────────────────── + # On Ctrl-C / ros2 launch shutdown: publish estop then let nodes drain. + estop_on_shutdown = RegisterEventHandler( + OnShutdown( + on_shutdown=[ + LogInfo(msg="[saltybot_bringup] SHUTDOWN — sending estop"), + ExecuteProcess( + cmd=[ + "ros2", "topic", "pub", "--once", + "/saltybot/estop", "std_msgs/msg/Bool", "{data: true}", + ], + output="screen", + ), + ] + ) + ) + + # ── Verbose logging for debug profile ───────────────────────────────────── + verbose_log_env = SetEnvironmentVariable( + name="RCUTILS_LOGGING_BUFFERED_STREAM", + value=PythonExpression( + ["'1' if '", profile, "' == 'debug' else '0'"] + ), + ) + + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + # GROUP A — DRIVERS (t = 0 s, all profiles) + # Dependency order: STM32 bridge first, then sensors, then motor daemon. + # Health gate: subsequent groups delayed until t_perception (8 s full/debug). + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + + group_a_banner = LogInfo( + msg=["[saltybot_bringup] GROUP A — Drivers profile=", profile] + ) + + # Robot description (URDF + static TF) + robot_description = IncludeLaunchDescription( + _launch("saltybot_description", "launch", "robot_description.launch.py"), + launch_arguments={"use_sim_time": use_sim_time}.items(), + ) + + # STM32 bidirectional bridge (JLINK USART1) + stm32_bridge = IncludeLaunchDescription( + _launch("saltybot_bridge", "launch", "bridge.launch.py"), + launch_arguments={ + "mode": "bidirectional", + "serial_port": stm32_port, + }.items(), + ) + + # Sensors: RealSense D435i + RPLIDAR A1 + sensors = TimerAction( + period=2.0, + actions=[ + LogInfo(msg="[saltybot_bringup] t=2s Starting sensors (RealSense + RPLIDAR)"), + IncludeLaunchDescription( + _launch("saltybot_bringup", "launch", "sensors.launch.py"), + ), + ], + ) + + # Motor daemon: /cmd_vel → STM32 DRIVE frames (depends on bridge at t=0) + motor_daemon = TimerAction( + period=2.5, + actions=[ + LogInfo(msg="[saltybot_bringup] t=2.5s Starting motor daemon"), + IncludeLaunchDescription( + _launch("saltybot_motor_daemon", "launch", "motor_daemon.launch.py"), + launch_arguments={"max_linear_vel": max_linear_vel}.items(), + ), + ], + ) + + # Sensor health monitor (all profiles — lightweight) + sensor_health = TimerAction( + period=4.0, + actions=[ + LogInfo(msg="[saltybot_bringup] t=4s Starting sensor health monitor"), + Node( + package="saltybot_health_monitor", + executable="sensor_health_node", + name="sensor_health_node", + output="screen", + parameters=[{"check_hz": 1.0, "mqtt_enabled": True}], + ), + ], + ) + + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + # ── HEALTH GATE A→B (t = 8 s for full/debug; skipped for minimal) ──────── + # Conservative: RealSense takes ~5 s, RPLIDAR ~2 s. Adding 1 s margin. + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + + health_gate_ab = TimerAction( + period=8.0, + actions=[ + GroupAction( + condition=_if_profile_flag("perception", _full_profiles), + actions=[ + LogInfo( + msg="[saltybot_bringup] HEALTH GATE A→B passed (t=8s) " + "— sensors should be publishing" + ), + ], + ), + ], + ) + + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + # GROUP B — PERCEPTION (t = 8 s, full/debug only) + # Depends: sensors publishing /camera/* and /scan. + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + + group_b = TimerAction( + period=8.0, + actions=[ + GroupAction( + condition=_if_profile_flag("perception", _full_profiles), + actions=[ + LogInfo(msg="[saltybot_bringup] GROUP B — Perception"), + + # UWB ranging (DW3000 USB anchors) + IncludeLaunchDescription( + _launch("saltybot_uwb", "launch", "uwb.launch.py"), + launch_arguments={ + "port_a": uwb_port_a, + "port_b": uwb_port_b, + }.items(), + ), + + # YOLOv8n person detection (TensorRT) + IncludeLaunchDescription( + _launch("saltybot_perception", "launch", "person_detection.launch.py"), + launch_arguments={"use_sim_time": use_sim_time}.items(), + ), + + # YOLOv8n general object detection (Issue #468) + IncludeLaunchDescription( + _launch("saltybot_object_detection", "launch", + "object_detection.launch.py"), + ), + + # Depth-to-costmap plugin (Issue #532) + IncludeLaunchDescription( + _launch("saltybot_depth_costmap", "launch", "depth_costmap.launch.py"), + ), + + # LIDAR obstacle avoidance (360° safety) + IncludeLaunchDescription( + _launch("saltybot_lidar_avoidance", "launch", + "lidar_avoidance.launch.py"), + ), + + # Gimbal control node (Issue #548) + IncludeLaunchDescription( + _launch("saltybot_gimbal", "launch", "gimbal.launch.py"), + launch_arguments={"serial_port": gimbal_port}.items(), + ), + + # Audio pipeline (Issue #503) — starts alongside perception + IncludeLaunchDescription( + _launch("saltybot_audio_pipeline", "launch", + "audio_pipeline.launch.py"), + ), + ], + ), + ], + ) + + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + # ── HEALTH GATE B→C (t = 16 s for full/debug) ──────────────────────────── + # SLAM needs camera + lidar for ~8 s to compute first keyframe. + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + + health_gate_bc = TimerAction( + period=16.0, + actions=[ + GroupAction( + condition=_if_profile_flag("navigation", _full_profiles), + actions=[ + LogInfo( + msg="[saltybot_bringup] HEALTH GATE B→C passed (t=16s) " + "— perception should be running" + ), + ], + ), + ], + ) + + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + # GROUP C — NAVIGATION (t = 16 s, full/debug only) + # Depends: SLAM needs t=8s of sensor data; Nav2 needs partial map. + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + + group_c = TimerAction( + period=16.0, + actions=[ + GroupAction( + condition=_if_profile_flag("navigation", _full_profiles), + actions=[ + LogInfo(msg="[saltybot_bringup] GROUP C — Navigation"), + + # RTAB-Map SLAM (RGB-D + LIDAR fusion) + IncludeLaunchDescription( + _launch("saltybot_bringup", "launch", "slam_rtabmap.launch.py"), + launch_arguments={"use_sim_time": use_sim_time}.items(), + ), + + # Person follower (t=16s — UWB + perception stable by now) + IncludeLaunchDescription( + _launch("saltybot_follower", "launch", "person_follower.launch.py"), + launch_arguments={ + "follow_distance": follow_distance, + "max_linear_vel": max_linear_vel, + }.items(), + ), + ], + ), + ], + ) + + # Nav2 starts 6 s after SLAM to allow partial map build (t=22s) + nav2 = TimerAction( + period=22.0, + actions=[ + GroupAction( + condition=_if_profile_flag("navigation", _full_profiles), + actions=[ + LogInfo(msg="[saltybot_bringup] t=22s Starting Nav2 (SLAM map ~6s old)"), + IncludeLaunchDescription( + _launch("saltybot_bringup", "launch", "nav2.launch.py"), + ), + ], + ), + ], + ) + + # Autonomous docking (Issue #489) + docking = TimerAction( + period=22.0, + actions=[ + GroupAction( + condition=_if_profile_flag("navigation", _full_profiles), + actions=[ + IncludeLaunchDescription( + _launch("saltybot_docking", "launch", "docking.launch.py"), + launch_arguments={"battery_low_pct": "20.0"}.items(), + ), + ], + ), + ], + ) + + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + # ── HEALTH GATE C→D (t = 26 s for debug; t = 22 s for full) ───────────── + # Nav2 needs ~4 s to load costmaps; rosbridge must see all topics. + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + + _t_ui_full = 26.0 # full profile UI start + _t_ui_debug = 30.0 # debug profile UI start (extra margin) + + health_gate_cd_full = TimerAction( + period=_t_ui_full, + actions=[ + GroupAction( + condition=LaunchConfigurationEquals("profile", "full"), + actions=[ + LogInfo( + msg=f"[saltybot_bringup] HEALTH GATE C→D passed (t={_t_ui_full}s) " + "— Nav2 costmaps should be loaded" + ), + ], + ), + ], + ) + + health_gate_cd_debug = TimerAction( + period=_t_ui_debug, + actions=[ + GroupAction( + condition=LaunchConfigurationEquals("profile", "debug"), + actions=[ + LogInfo( + msg=f"[saltybot_bringup] HEALTH GATE C→D passed (t={_t_ui_debug}s) " + "— debug profile extra margin elapsed" + ), + ], + ), + ], + ) + + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + # GROUP D — UI / SOCIAL (t = 26 s full, 30 s debug) + # Last group; exposes all topics over WebSocket + starts social layer. + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + + def _group_d(period: float, profile_cond: IfCondition) -> TimerAction: + return TimerAction( + period=period, + actions=[ + GroupAction( + condition=profile_cond, + actions=[ + LogInfo(msg="[saltybot_bringup] GROUP D — UI/Social/rosbridge"), + + # Social personality (face, emotions, expressions) + IncludeLaunchDescription( + _launch("saltybot_social_personality", "launch", + "social_personality.launch.py"), + ), + + # 4× CSI surround cameras (optional; non-critical) + IncludeLaunchDescription( + _launch("saltybot_cameras", "launch", "csi_cameras.launch.py"), + ), + + # rosbridge WebSocket (port 9090) — must be last + IncludeLaunchDescription( + _launch("saltybot_bringup", "launch", "rosbridge.launch.py"), + ), + ], + ), + ], + ) + + group_d_full = _group_d(_t_ui_full, LaunchConfigurationEquals("profile", "full")) + group_d_debug = _group_d(_t_ui_debug, LaunchConfigurationEquals("profile", "debug")) + + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + # DEBUG EXTRAS — (debug profile only) + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + + debug_extras = TimerAction( + period=3.0, + actions=[ + GroupAction( + condition=LaunchConfigurationEquals("profile", "debug"), + actions=[ + LogInfo(msg="[saltybot_bringup] DEBUG extras: bag recorder + RViz"), + # Bag recorder + IncludeLaunchDescription( + _launch("saltybot_bag_recorder", "launch", "bag_recorder.launch.py"), + ), + # RViz2 with SaltyBot config + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=[ + "-d", + PathJoinSubstitution([ + FindPackageShare("saltybot_bringup"), + "config", "saltybot_rviz.rviz", + ]), + ], + output="screen", + ), + ], + ), + ], + ) + + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + # Assemble LaunchDescription + # ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ + + return LaunchDescription([ + # ── Arguments ────────────────────────────────────────────────────────── + profile_arg, + use_sim_time_arg, + stm32_port_arg, + uwb_port_a_arg, + uwb_port_b_arg, + gimbal_port_arg, + max_linear_vel_arg, + follow_distance_arg, + + # ── Environment ──────────────────────────────────────────────────────── + verbose_log_env, + + # ── Shutdown handler ─────────────────────────────────────────────────── + estop_on_shutdown, + + # ── Startup banner ───────────────────────────────────────────────────── + group_a_banner, + + # ── GROUP A: Drivers (all profiles, t=0–4s) ─────────────────────────── + robot_description, + stm32_bridge, + sensors, + motor_daemon, + sensor_health, + + # ── Health gate A→B ──────────────────────────────────────────────────── + health_gate_ab, + + # ── GROUP B: Perception (full/debug, t=8s) ──────────────────────────── + group_b, + + # ── Health gate B→C ──────────────────────────────────────────────────── + health_gate_bc, + + # ── GROUP C: Navigation (full/debug, t=16–22s) ──────────────────────── + group_c, + nav2, + docking, + + # ── Health gate C→D ──────────────────────────────────────────────────── + health_gate_cd_full, + health_gate_cd_debug, + + # ── GROUP D: UI/Social (full t=26s, debug t=30s) ────────────────────── + group_d_full, + group_d_debug, + + # ── Debug extras (t=3s, debug profile only) ─────────────────────────── + debug_extras, + ]) diff --git a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/launch_profiles.py b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/launch_profiles.py new file mode 100644 index 0000000..5200f3a --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/launch_profiles.py @@ -0,0 +1,187 @@ +"""launch_profiles.py — SaltyBot bringup launch profiles (Issue #577). + +Defines three named profiles: minimal, full, debug. +Each profile is a plain dataclass — no ROS2 runtime dependency — so +profile selection logic can be unit-tested without a live ROS2 install. + +Profile hierarchy: + minimal ⊂ full ⊂ debug + +Usage (from launch files):: + + from saltybot_bringup.launch_profiles import get_profile, Profile + + p = get_profile("full") + if p.enable_slam: + ... +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Dict + + +@dataclass +class Profile: + """All runtime-configurable flags for a single launch profile.""" + + name: str + + # ── Group A: Drivers (always on in all profiles) ────────────────────── + enable_stm32_bridge: bool = True + enable_sensors: bool = True # RealSense + RPLIDAR + enable_motor_daemon: bool = True + enable_imu: bool = True + + # ── Group B: Perception ──────────────────────────────────────────────── + enable_uwb: bool = False + enable_person_detection: bool = False + enable_object_detection: bool = False + enable_depth_costmap: bool = False + enable_gimbal: bool = False + + # ── Group C: Navigation ──────────────────────────────────────────────── + enable_slam: bool = False + enable_nav2: bool = False + enable_follower: bool = False + enable_docking: bool = False + enable_lidar_avoid: bool = False + + # ── Group D: UI / Social / Audio ────────────────────────────────────── + enable_rosbridge: bool = False + enable_audio: bool = False + enable_social: bool = False + enable_csi_cameras: bool = False + + # ── Debug / recording extras ────────────────────────────────────────── + enable_bag_recorder: bool = False + enable_diagnostics: bool = True # sensor health + DiagnosticArray + enable_rviz: bool = False + enable_verbose_logs: bool = False + + # ── Timing: inter-group health-gate delays (seconds) ────────────────── + # Each delay is the minimum wall-clock time after t=0 before the group + # starts. Increase for slower hardware or cold-start scenarios. + t_drivers: float = 0.0 # Group A + t_perception: float = 8.0 # Group B (sensors need ~6 s to initialise) + t_navigation: float = 16.0 # Group C (SLAM needs ~8 s to build first map) + t_ui: float = 22.0 # Group D (nav2 needs ~4 s to load costmaps) + + # ── Safety ──────────────────────────────────────────────────────────── + watchdog_timeout_s: float = 5.0 # max silence from STM32 bridge (s) + cmd_vel_deadman_s: float = 0.5 # cmd_vel watchdog in bridge + max_linear_vel: float = 0.5 # m/s cap passed to bridge + follower + follow_distance_m: float = 1.5 # target follow distance (m) + + # ── Hardware conditionals ───────────────────────────────────────────── + # Paths checked at launch; absent devices skip the relevant node. + stm32_port: str = "/dev/stm32-bridge" + uwb_port_a: str = "/dev/uwb-anchor0" + uwb_port_b: str = "/dev/uwb-anchor1" + gimbal_port: str = "/dev/ttyTHS1" + + def to_dict(self) -> Dict: + """Return flat dict (e.g. for passing to launch Parameters).""" + import dataclasses + return dataclasses.asdict(self) + + +# ── Profile factory ──────────────────────────────────────────────────────────── + +def _minimal() -> Profile: + """Minimal: STM32 bridge + sensors + motor daemon. + + Safe drive control only. No AI, no nav, no social. + Boot time ~4 s. RAM ~400 MB. + """ + return Profile( + name="minimal", + # Drivers already enabled by default + enable_diagnostics=True, + t_drivers=0.0, + t_perception=0.0, # unused + t_navigation=0.0, # unused + t_ui=0.0, # unused + ) + + +def _full() -> Profile: + """Full: complete autonomous stack. + + SLAM + Nav2 + perception + audio + social + rosbridge. + Boot time ~22 s cold. RAM ~3 GB. + """ + return Profile( + name="full", + # Drivers + enable_stm32_bridge=True, + enable_sensors=True, + enable_motor_daemon=True, + enable_imu=True, + # Perception + enable_uwb=True, + enable_person_detection=True, + enable_object_detection=True, + enable_depth_costmap=True, + enable_gimbal=True, + # Navigation + enable_slam=True, + enable_nav2=True, + enable_follower=True, + enable_docking=True, + enable_lidar_avoid=True, + # UI + enable_rosbridge=True, + enable_audio=True, + enable_social=True, + enable_csi_cameras=True, + # Extras + enable_bag_recorder=True, + enable_diagnostics=True, + ) + + +def _debug() -> Profile: + """Debug: full stack + verbose logging + RViz + extra recording. + + Adds /diagnostics aggregation, RViz2, verbose node output. + Boot time same as full. RAM ~3.5 GB (RViz + extra logs). + """ + p = _full() + p.name = "debug" + p.enable_rviz = True + p.enable_verbose_logs = True + p.enable_bag_recorder = True + # Slower boot to ensure all topics are stable before Nav2 starts + p.t_navigation = 20.0 + p.t_ui = 26.0 + return p + + +_PROFILES: Dict[str, Profile] = { + "minimal": _minimal(), + "full": _full(), + "debug": _debug(), +} + + +def get_profile(name: str) -> Profile: + """Return the named Profile. + + Args: + name: One of "minimal", "full", "debug". + + Raises: + ValueError: If name is not a known profile. + """ + if name not in _PROFILES: + raise ValueError( + f"Unknown launch profile {name!r}. " + f"Valid profiles: {sorted(_PROFILES)}" + ) + return _PROFILES[name] + + +def profile_names() -> list: + return sorted(_PROFILES.keys()) diff --git a/jetson/ros2_ws/src/saltybot_bringup/test/test_launch_orchestrator.py b/jetson/ros2_ws/src/saltybot_bringup/test/test_launch_orchestrator.py new file mode 100644 index 0000000..2b1fa9b --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/test/test_launch_orchestrator.py @@ -0,0 +1,326 @@ +"""test_launch_orchestrator.py — Unit tests for saltybot_bringup launch profiles (Issue #577). + +Tests are deliberately ROS2-free; run with: + python3 -m pytest jetson/ros2_ws/src/saltybot_bringup/test/test_launch_orchestrator.py -v +""" + +from __future__ import annotations + +import sys +import os + +# Allow import without ROS2 install +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "saltybot_bringup")) + +import pytest +from launch_profiles import Profile, get_profile, profile_names, _minimal, _full, _debug + + +# ─── Profile factory basics ─────────────────────────────────────────────────── + +class TestProfileNames: + def test_returns_list(self): + names = profile_names() + assert isinstance(names, list) + + def test_contains_three_profiles(self): + assert len(profile_names()) == 3 + + def test_expected_names_present(self): + names = profile_names() + assert "minimal" in names + assert "full" in names + assert "debug" in names + + def test_names_sorted(self): + names = profile_names() + assert names == sorted(names) + + +class TestGetProfile: + def test_returns_profile_instance(self): + p = get_profile("minimal") + assert isinstance(p, Profile) + + def test_unknown_raises_value_error(self): + with pytest.raises(ValueError, match="Unknown launch profile"): + get_profile("turbo") + + def test_error_message_lists_valid_profiles(self): + with pytest.raises(ValueError) as exc_info: + get_profile("bogus") + msg = str(exc_info.value) + assert "debug" in msg + assert "full" in msg + assert "minimal" in msg + + def test_get_minimal_name(self): + assert get_profile("minimal").name == "minimal" + + def test_get_full_name(self): + assert get_profile("full").name == "full" + + def test_get_debug_name(self): + assert get_profile("debug").name == "debug" + + +# ─── Minimal profile ────────────────────────────────────────────────────────── + +class TestMinimalProfile: + def setup_method(self): + self.p = _minimal() + + def test_name(self): + assert self.p.name == "minimal" + + def test_drivers_enabled(self): + assert self.p.enable_stm32_bridge is True + assert self.p.enable_sensors is True + assert self.p.enable_motor_daemon is True + assert self.p.enable_imu is True + + def test_perception_disabled(self): + assert self.p.enable_uwb is False + assert self.p.enable_person_detection is False + assert self.p.enable_object_detection is False + assert self.p.enable_depth_costmap is False + assert self.p.enable_gimbal is False + + def test_navigation_disabled(self): + assert self.p.enable_slam is False + assert self.p.enable_nav2 is False + assert self.p.enable_follower is False + assert self.p.enable_docking is False + assert self.p.enable_lidar_avoid is False + + def test_ui_disabled(self): + assert self.p.enable_rosbridge is False + assert self.p.enable_audio is False + assert self.p.enable_social is False + assert self.p.enable_csi_cameras is False + + def test_debug_features_disabled(self): + assert self.p.enable_rviz is False + assert self.p.enable_verbose_logs is False + assert self.p.enable_bag_recorder is False + + def test_timing_zero(self): + # Unused timings are zeroed so there are no unnecessary waits + assert self.p.t_perception == 0.0 + assert self.p.t_navigation == 0.0 + assert self.p.t_ui == 0.0 + + def test_diagnostics_enabled(self): + assert self.p.enable_diagnostics is True + + +# ─── Full profile ───────────────────────────────────────────────────────────── + +class TestFullProfile: + def setup_method(self): + self.p = _full() + + def test_name(self): + assert self.p.name == "full" + + def test_drivers_enabled(self): + assert self.p.enable_stm32_bridge is True + assert self.p.enable_sensors is True + assert self.p.enable_motor_daemon is True + assert self.p.enable_imu is True + + def test_perception_enabled(self): + assert self.p.enable_uwb is True + assert self.p.enable_person_detection is True + assert self.p.enable_object_detection is True + assert self.p.enable_depth_costmap is True + assert self.p.enable_gimbal is True + + def test_navigation_enabled(self): + assert self.p.enable_slam is True + assert self.p.enable_nav2 is True + assert self.p.enable_follower is True + assert self.p.enable_docking is True + assert self.p.enable_lidar_avoid is True + + def test_ui_enabled(self): + assert self.p.enable_rosbridge is True + assert self.p.enable_audio is True + assert self.p.enable_social is True + assert self.p.enable_csi_cameras is True + + def test_diagnostics_enabled(self): + assert self.p.enable_diagnostics is True + + def test_debug_features_disabled(self): + assert self.p.enable_rviz is False + assert self.p.enable_verbose_logs is False + + def test_timing_positive(self): + assert self.p.t_drivers == 0.0 + assert self.p.t_perception > 0.0 + assert self.p.t_navigation > self.p.t_perception + assert self.p.t_ui > self.p.t_navigation + + def test_perception_gate_8s(self): + assert self.p.t_perception == 8.0 + + def test_navigation_gate_16s(self): + assert self.p.t_navigation == 16.0 + + def test_ui_gate_22s(self): + assert self.p.t_ui == 22.0 + + +# ─── Debug profile ──────────────────────────────────────────────────────────── + +class TestDebugProfile: + def setup_method(self): + self.p = _debug() + + def test_name(self): + assert self.p.name == "debug" + + def test_inherits_full_stack(self): + full = _full() + # All full flags must be True in debug too + assert self.p.enable_slam == full.enable_slam + assert self.p.enable_nav2 == full.enable_nav2 + assert self.p.enable_rosbridge == full.enable_rosbridge + + def test_rviz_enabled(self): + assert self.p.enable_rviz is True + + def test_verbose_logs_enabled(self): + assert self.p.enable_verbose_logs is True + + def test_bag_recorder_enabled(self): + assert self.p.enable_bag_recorder is True + + def test_timing_slower_than_full(self): + full = _full() + # Debug extends gates to ensure stability + assert self.p.t_navigation >= full.t_navigation + assert self.p.t_ui >= full.t_ui + + def test_navigation_gate_20s(self): + assert self.p.t_navigation == 20.0 + + def test_ui_gate_26s(self): + assert self.p.t_ui == 26.0 + + +# ─── Profile hierarchy checks ───────────────────────────────────────────────── + +class TestProfileHierarchy: + """Minimal ⊂ Full ⊂ Debug — every flag true in minimal must be true in full/debug.""" + + def _enabled_flags(self, p: Profile) -> set: + import dataclasses + return { + f.name + for f in dataclasses.fields(p) + if f.name.startswith("enable_") and getattr(p, f.name) is True + } + + def test_minimal_subset_of_full(self): + minimal_flags = self._enabled_flags(_minimal()) + full_flags = self._enabled_flags(_full()) + assert minimal_flags.issubset(full_flags), ( + f"Full missing flags that Minimal has: {minimal_flags - full_flags}" + ) + + def test_full_subset_of_debug(self): + full_flags = self._enabled_flags(_full()) + debug_flags = self._enabled_flags(_debug()) + assert full_flags.issubset(debug_flags), ( + f"Debug missing flags that Full has: {full_flags - debug_flags}" + ) + + def test_debug_has_extra_flags(self): + full_flags = self._enabled_flags(_full()) + debug_flags = self._enabled_flags(_debug()) + extras = debug_flags - full_flags + assert len(extras) > 0, "Debug should have at least one extra flag over Full" + + def test_debug_extras_are_debug_features(self): + full_flags = self._enabled_flags(_full()) + debug_flags = self._enabled_flags(_debug()) + extras = debug_flags - full_flags + for flag in extras: + assert "rviz" in flag or "verbose" in flag or "bag" in flag or "debug" in flag, ( + f"Unexpected extra flag in debug: {flag}" + ) + + +# ─── to_dict ────────────────────────────────────────────────────────────────── + +class TestToDict: + def test_returns_dict(self): + p = _minimal() + d = p.to_dict() + assert isinstance(d, dict) + + def test_name_in_dict(self): + d = _minimal().to_dict() + assert d["name"] == "minimal" + + def test_all_bool_flags_present(self): + d = _full().to_dict() + for key in ("enable_slam", "enable_nav2", "enable_rosbridge", "enable_rviz"): + assert key in d, f"Missing key: {key}" + + def test_timing_fields_present(self): + d = _full().to_dict() + for key in ("t_drivers", "t_perception", "t_navigation", "t_ui"): + assert key in d, f"Missing timing key: {key}" + + def test_values_are_native_types(self): + d = _debug().to_dict() + for v in d.values(): + assert isinstance(v, (bool, int, float, str)), ( + f"Expected native type, got {type(v)} for value {v!r}" + ) + + +# ─── Safety parameter defaults ──────────────────────────────────────────────── + +class TestSafetyDefaults: + def test_watchdog_timeout_positive(self): + for name in profile_names(): + p = get_profile(name) + assert p.watchdog_timeout_s > 0, f"{name}: watchdog_timeout_s must be > 0" + + def test_max_linear_vel_bounded(self): + for name in profile_names(): + p = get_profile(name) + assert 0 < p.max_linear_vel <= 2.0, ( + f"{name}: max_linear_vel {p.max_linear_vel} outside safe range" + ) + + def test_follow_distance_positive(self): + for name in profile_names(): + p = get_profile(name) + assert p.follow_distance_m > 0, f"{name}: follow_distance_m must be > 0" + + def test_cmd_vel_deadman_positive(self): + for name in profile_names(): + p = get_profile(name) + assert p.cmd_vel_deadman_s > 0, f"{name}: cmd_vel_deadman_s must be > 0" + + +# ─── Hardware port defaults ──────────────────────────────────────────────────── + +class TestHardwarePortDefaults: + def test_stm32_port_set(self): + p = _minimal() + assert p.stm32_port.startswith("/dev/") + + def test_uwb_ports_set(self): + p = _full() + assert p.uwb_port_a.startswith("/dev/") + assert p.uwb_port_b.startswith("/dev/") + + def test_gimbal_port_set(self): + p = _full() + assert p.gimbal_port.startswith("/dev/")