Merge pull request 'feat: ROS2 launch orchestrator for full SaltyBot bringup (Issue #577)' (#582) from sl-jetson/issue-577-bringup-launch into main
This commit is contained in:
commit
e9429e6177
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -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,
|
||||
])
|
||||
@ -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())
|
||||
@ -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/")
|
||||
Loading…
x
Reference in New Issue
Block a user