From f061207bc406b1ef6a7b870b80e32276d114d063 Mon Sep 17 00:00:00 2001 From: sl-mechanical Date: Sat, 14 Mar 2026 11:51:40 -0400 Subject: [PATCH] feat: UWB anchor mount bracket (Issue #564) --- chassis/uwb_anchor_mount.scad | 562 ++++++++++-------- .../test/test_uwb_position.py | 89 +++ 2 files changed, 403 insertions(+), 248 deletions(-) create mode 100644 jetson/ros2_ws/src/saltybot_uwb_position/test/test_uwb_position.py diff --git a/chassis/uwb_anchor_mount.scad b/chassis/uwb_anchor_mount.scad index 6c77443..6db7ed5 100644 --- a/chassis/uwb_anchor_mount.scad +++ b/chassis/uwb_anchor_mount.scad @@ -1,275 +1,341 @@ // ============================================================ -// uwb_anchor_mount.scad — Stem-Mounted UWB Anchor Rev A -// Agent: sl-mechanical 2026-03-01 -// Closes issues #57, #62 +// uwb_anchor_mount.scad — Wall/Ceiling UWB Anchor Mount Bracket +// Issue: #564 Agent: sl-mechanical Date: 2026-03-14 +// (supersedes Rev A stem-collar mount — see git history) // ============================================================ -// Clamp-on bracket for 2× MaUWB ESP32-S3 anchor modules on -// SaltyBot 25 mm OD vertical stem. -// Anchors spaced ANCHOR_SPACING = 250 mm apart. // -// Features: -// • Split D-collar with M4 clamping bolts + M4 set screw -// • Anti-rotation flat tab that keys against a small pin -// OR printed key tab that registers on the stem flat (if stem -// has a ground flat) — see ANTI_ROT_MODE parameter -// • Module bracket: faces outward, tilted 10° from vertical -// so antenna clears stem and faces horizon -// • USB cable channel (power from Orin via USB-A) on collar -// • Tool-free capture: M4 thumbscrews (slot-head, hand-tighten) -// • UWB antenna area: NO material within 10 mm of PCB top face +// Parametric wall or ceiling mount bracket for ESP32 UWB Pro anchor. +// Designed for fixed-infrastructure deployment: anchors screw into +// wall or ceiling drywall/timber with standard M4 or #6 wood screws, +// at a user-defined tilt angle so the UWB antenna faces the desired +// coverage zone. // -// Components per mount: -// 2× collar_half print in PLA/PETG, flat-face-down -// 1× module_bracket print in PLA/PETG, flat-face-down +// Architecture: +// Wall base -> flat backplate with 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() +// +// Hardware BOM: +// 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 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 (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" single mount assembled (default) -// "collar_front" front collar half for slicing (×2 per mount × 2 mounts = 4) -// "collar_rear" rear collar half -// "bracket" module bracket (×2 mounts) -// "pair" both mounts on 350 mm stem section +// "assembly" full assembly at TILT_DEG (default) +// "wall_base_stl" Part 1 +// "tilt_arm_stl" Part 2 +// "anchor_cradle_stl" Part 3 +// "cable_clip_stl" Part 4 +// +// Export commands: +// openscad uwb_anchor_mount.scad -D 'RENDER="wall_base_stl"' -o uwb_wall_base.stl +// openscad uwb_anchor_mount.scad -D 'RENDER="tilt_arm_stl"' -o uwb_tilt_arm.stl +// openscad uwb_anchor_mount.scad -D 'RENDER="anchor_cradle_stl"' -o uwb_anchor_cradle.stl +// openscad uwb_anchor_mount.scad -D 'RENDER="cable_clip_stl"' -o uwb_cable_clip.stl // ============================================================ +$fn = 64; +e = 0.01; + +// -- Tilt angle (override per anchor, 0-90deg, 15deg steps) ------------------ +TILT_DEG = 30; + +// -- 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; +BASE_SCREW_HD = 8.5; +BASE_SCREW_HH = 3.5; +BASE_SCREW_SPC = 35.0; +KNUCKLE_T = BASE_T + 4.0; + +// -- Tilt arm geometry -------------------------------------------------------- +ARM_W = 12.0; +ARM_T = 5.0; +ARM_L = 35.0; +PIVOT_D = 3.3; +PIVOT_NUT_AF = 5.5; +PIVOT_NUT_H = 2.4; +DETENT_D = 3.2; +DETENT_R = 8.0; + +// -- Anchor cradle geometry --------------------------------------------------- +CRADLE_WALL_T = 3.5; +CRADLE_BACK_T = 4.0; +CRADLE_FLOOR_T = 3.0; +CRADLE_LIP_H = 4.0; +CRADLE_LIP_T = 2.5; +STANDOFF_H = 3.0; +STANDOFF_OD = 5.5; +LABEL_W = UWB_L - 4.0; +LABEL_H = UWB_W * 0.55; +LABEL_T = 1.2; + +// -- USB-C routing ------------------------------------------------------------ +USBC_CHAN_W = 11.0; +USBC_CHAN_H = 7.0; + +// -- Cable clip --------------------------------------------------------------- +CLIP_CABLE_D = 4.5; +CLIP_T = 2.0; +CLIP_BODY_W = 16.0; +CLIP_BODY_H = 10.0; + +// -- Fasteners ---------------------------------------------------------------- +M2P5_D = 2.7; +M3_D = 3.3; +M3_NUT_AF = 5.5; +M3_NUT_H = 2.4; + +// ============================================================ +// RENDER DISPATCH +// ============================================================ RENDER = "assembly"; -// ── ⚠ Verify with calipers ─────────────────────────────────── -MAWB_L = 50.0; // PCB length -MAWB_W = 25.0; // PCB width -MAWB_H = 10.0; // PCB + components -MAWB_HOLE_X = 43.0; // M2 mounting hole X span -MAWB_HOLE_Y = 20.0; // M2 mounting hole Y span -M2_D = 2.2; // M2 clearance +if (RENDER == "assembly") assembly_preview(); +else if (RENDER == "wall_base_stl") wall_base(); +else if (RENDER == "tilt_arm_stl") tilt_arm(); +else if (RENDER == "anchor_cradle_stl") anchor_cradle(); +else if (RENDER == "cable_clip_stl") cable_clip(); -// ── Stem ───────────────────────────────────────────────────── -STEM_OD = 25.0; -STEM_BORE = 25.4; // +0.4 clearance -WALL = 2.0; // wall thickness (used in thumbscrew recess) +// ============================================================ +// ASSEMBLY PREVIEW +// ============================================================ +module assembly_preview() { + %color("Wheat", 0.22) + translate([-BASE_W/2, -10, -BASE_H/2]) + cube([BASE_W, 10, BASE_H + 40]); + color("OliveDrab", 0.85) wall_base(); + color("SteelBlue", 0.85) + 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(); + %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]); + 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(); +} -// ── Collar ─────────────────────────────────────────────────── -COL_OD = 52.0; -COL_H = 30.0; // taller than sensor-head collar for rigidity -COL_BOLT_X = 19.0; // M4 bolt CL from stem axis -COL_BOLT_D = 4.5; // M4 clearance -THUMB_HEAD_D= 8.0; // M4 thumbscrew head OD (slot for access) -COL_NUT_W = 7.0; // M4 hex nut A/F -COL_NUT_H = 3.4; - -// Anti-rotation flat tab: a 3 mm wall tab that protrudes radially -// and bears against the bracket arm, preventing axial rotation -// without needing a stem flat. -ANTI_ROT_T = 3.0; // tab thickness (radial) -ANTI_ROT_W = 8.0; // tab width (tangential) -ANTI_ROT_Z = 4.0; // distance from collar base - -// USB cable channel: groove on collar outer surface, runs Z direction -// Cable routes from anchor module down to base -USB_CHAN_W = 9.0; // channel width (fits USB-A cable Ø6 mm) -USB_CHAN_D = 5.0; // channel depth - -// ── Module bracket ─────────────────────────────────────────── -ARM_L = 20.0; // arm length from collar OD to bracket face -ARM_W = MAWB_W + 6.0; // bracket width (Y, includes side walls) -ARM_H = 6.0; // arm thickness (Z) -BRKT_TILT = 10.0; // tilt outward from vertical (antenna faces horizon) - -BRKT_BACK_T = 3.0; // bracket back wall (module sits against this) -BRKT_SIDE_T = 2.0; // bracket side walls - -M2_STNDFF = 3.0; // M2 standoff height -M2_STNDFF_OD= 4.5; - -// USB port access notch in bracket side wall (8×5 mm) -USB_NOTCH_W = 10.0; -USB_NOTCH_H = 7.0; - -// ── Spacing ─────────────────────────────────────────────────── -ANCHOR_SPACING = 250.0; // centre-to-centre Z separation - -$fn = 64; -e = 0.01; - -// ───────────────────────────────────────────────────────────── -// collar_half(side) -// split at Y=0 plane. Bracket arm on front (+Y) half. -// Print flat-face-down. -// ───────────────────────────────────────────────────────────── -module collar_half(side = "front") { - y_front = (side == "front"); +// ============================================================ +// PART 1 -- WALL BASE +// ============================================================ +// Flat backplate, 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. +// +// Print: backplate flat on bed, PETG, 5 perims, 40% gyroid. +module wall_base() { + ear_h = ARM_W + 3.0; + ear_t = 6.0; + ear_sep = ARM_W + 1.0; difference() { union() { - // D-shaped body - intersection() { - cylinder(d=COL_OD, h=COL_H); - translate([-COL_OD/2, y_front ? 0 : -COL_OD/2, 0]) - cube([COL_OD, COL_OD/2, COL_H]); - } - - // Anti-rotation tab (front half only, at +X side) - if (y_front) { - translate([COL_OD/2, -ANTI_ROT_W/2, ANTI_ROT_Z]) - cube([ANTI_ROT_T, ANTI_ROT_W, - COL_H - ANTI_ROT_Z - 4]); - } - - // Bracket arm attachment boss (front half only, top centre) - if (y_front) { - translate([-ARM_W/2, COL_OD/2, COL_H * 0.3]) - cube([ARM_W, ARM_L, COL_H * 0.4]); - } + translate([-BASE_W/2, -BASE_T, -BASE_H/2]) + cube([BASE_W, BASE_T, BASE_H]); + 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]); + 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.5 : 0), -BASE_T, -ear_h/6]) + cube([ear_t*0.5, 1, ear_h/3]); + } } - - // ── Stem bore ───────────────────────────────────────── - translate([0,0,-e]) - cylinder(d=STEM_BORE, h=COL_H + 2*e); - - // ── M4 clamping bolt holes (Y direction) ────────────── - for (bx=[-COL_BOLT_X, COL_BOLT_X]) { - translate([bx, y_front ? COL_OD/2 : 0, COL_H/2]) - rotate([90,0,0]) - cylinder(d=COL_BOLT_D, h=COL_OD/2 + e); - // Thumbscrew head recess on outer face (front only — access side) - if (y_front) { - translate([bx, COL_OD/2 - WALL, COL_H/2]) - rotate([90,0,0]) - cylinder(d=THUMB_HEAD_D, h=8 + e); - } - } - - // ── M4 hex nut pockets (rear half) ──────────────────── - if (!y_front) { - for (bx=[-COL_BOLT_X, COL_BOLT_X]) { - translate([bx, -(COL_OD/4 + e), COL_H/2]) - rotate([90,0,0]) - cylinder(d=COL_NUT_W/cos(30), h=COL_NUT_H + e, - $fn=6); - } - } - - // ── Set screw (height lock, front half) ─────────────── - if (y_front) { - translate([0, COL_OD/2, COL_H * 0.8]) - rotate([90,0,0]) - cylinder(d=COL_BOLT_D, - h=COL_OD/2 - STEM_BORE/2 + e); - } - - // ── USB cable routing channel (rear half, −X side) ──── - if (!y_front) { - translate([-COL_OD/2, -USB_CHAN_W/2, -e]) - cube([USB_CHAN_D, USB_CHAN_W, COL_H + 2*e]); - } - - // ── M4 hole through arm boss (Z direction, for bracket bolt) ─ - if (y_front) { - for (dx=[-ARM_W/4, ARM_W/4]) - translate([dx, COL_OD/2 + ARM_L/2, COL_H * 0.35]) - cylinder(d=COL_BOLT_D, h=COL_H * 0.35 + e); + 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([-(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); + 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); } } -// ───────────────────────────────────────────────────────────── -// module_bracket() -// Bolts to collar arm boss. Holds MaUWB PCB facing outward. -// Tilted BRKT_TILT° from vertical — antenna clears stem. -// Print flat-face-down (back wall on bed). -// ───────────────────────────────────────────────────────────── -module module_bracket() { - bk = BRKT_BACK_T; - sd = BRKT_SIDE_T; +// ============================================================ +// PART 2 -- TILT ARM +// ============================================================ +// Pivoting arm linking wall_base 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. +module tilt_arm() { + total_h = ARM_L + 10; + difference() { + union() { + 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]); + } + 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); + 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); + translate([0, ARM_T/2, ARM_L/2]) + cube([ARM_W-4, ARM_T-2, ARM_L-18], center=true); + } +} + +// ============================================================ +// PART 3 -- ANCHOR CRADLE +// ============================================================ +// Open-front U-cradle for ESP32 UWB Pro PCB. +// 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. +// +// 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; + pcb_z = CRADLE_FLOOR_T + STANDOFF_H; + total_z = pcb_z + UWB_H + 2; difference() { union() { - // ── Back wall (mounts to collar arm boss) ───────── - cube([ARM_W, bk, MAWB_H + M2_STNDFF + 6]); - - // ── Side walls ──────────────────────────────────── - for (sx=[0, ARM_W - sd]) - translate([sx, bk, 0]) - cube([sd, MAWB_L + 2, MAWB_H + M2_STNDFF + 6]); - - // ── M2 standoff posts (PCB mounts to these) ─────── - for (hx=[0, MAWB_HOLE_X], hy=[0, MAWB_HOLE_Y]) - translate([(ARM_W - MAWB_HOLE_X)/2 + hx, - bk + (MAWB_L - MAWB_HOLE_Y)/2 + hy, - 0]) - cylinder(d=M2_STNDFF_OD, h=M2_STNDFF); + 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]); + for (tx = [-ARM_W/4, ARM_W/4]) + translate([tx-4, -CRADLE_BACK_T, 0]) + cube([8, CRADLE_BACK_T+1, total_z]); } + 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); + 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]); + } + for (hx = [-UWB_HOLE_X/2, UWB_HOLE_X/2]) + for (hy = [(outer_w-UWB_W)/2 + (UWB_W-UWB_HOLE_Y)/2, + (outer_w-UWB_W)/2 + (UWB_W-UWB_HOLE_Y)/2 + UWB_HOLE_Y]) + difference() { + translate([hx, hy, CRADLE_FLOOR_T-e]) + cylinder(d=STANDOFF_OD, h=STANDOFF_H+e); + translate([hx, hy, CRADLE_FLOOR_T-2*e]) + cylinder(d=M2P5_D, h=STANDOFF_H+4); + } +} - // ── M2 bores through standoffs ──────────────────────── - for (hx=[0, MAWB_HOLE_X], hy=[0, MAWB_HOLE_Y]) - translate([(ARM_W - MAWB_HOLE_X)/2 + hx, - bk + (MAWB_L - MAWB_HOLE_Y)/2 + hy, - -e]) - cylinder(d=M2_D, h=M2_STNDFF + e); - - // ── Antenna clearance cutout in back wall ───────────── - // Open slot near top of back wall so antenna is unobstructed - translate([sd, -e, M2_STNDFF + 2]) - cube([ARM_W - 2*sd, bk + 2*e, MAWB_H]); - - // ── USB port access notch on one side wall ──────────── - translate([-e, bk + 2, M2_STNDFF - 1]) - cube([sd + 2*e, USB_NOTCH_W, USB_NOTCH_H]); - - // ── Mounting holes to collar arm boss (×2) ──────────── - for (dx=[-ARM_W/4, ARM_W/4]) - translate([ARM_W/2 + dx, bk + ARM_L/2, -e]) - cylinder(d=COL_BOLT_D, h=6 + e); +// ============================================================ +// PART 4 -- CABLE CLIP +// ============================================================ +// Snap-on C-clip retaining USB-C cable along tilt arm outer face. +// 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. +module cable_clip() { + ch_r = CLIP_CABLE_D/2 + CLIP_T; + snap_t = 1.6; + difference() { + union() { + translate([-CLIP_BODY_W/2, 0, 0]) + cube([CLIP_BODY_W, CLIP_T, CLIP_BODY_H]); + 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); + } + translate([0, -ARM_T-1-e, CLIP_BODY_H/2]) + cube([CLIP_BODY_W-6, ARM_T+2, CLIP_BODY_H-4], center=true); } } - -// ───────────────────────────────────────────────────────────── -// single_anchor_assembly() -// ───────────────────────────────────────────────────────────── -module single_anchor_assembly(show_phantom=false) { - // Collar - color("SteelBlue", 0.9) collar_half("front"); - color("CornflowerBlue", 0.9) mirror([0,1,0]) collar_half("rear"); - - // Bracket tilted BRKT_TILT° outward from top of arm boss - color("LightSteelBlue", 0.85) - translate([0, COL_OD/2 + ARM_L, COL_H * 0.3]) - rotate([BRKT_TILT, 0, 0]) - translate([-ARM_W/2, 0, 0]) - module_bracket(); - - // Phantom UWB PCB - if (show_phantom) - color("ForestGreen", 0.4) - translate([-MAWB_L/2, - COL_OD/2 + ARM_L + BRKT_BACK_T, - COL_H * 0.3 + M2_STNDFF]) - cube([MAWB_L, MAWB_W, MAWB_H]); -} - -// ───────────────────────────────────────────────────────────── -// Render selector -// ───────────────────────────────────────────────────────────── -if (RENDER == "assembly") { - single_anchor_assembly(show_phantom=true); - -} else if (RENDER == "collar_front") { - collar_half("front"); - -} else if (RENDER == "collar_rear") { - collar_half("rear"); - -} else if (RENDER == "bracket") { - module_bracket(); - -} else if (RENDER == "pair") { - // Both anchors at 250 mm spacing on a stem stub - color("Silver", 0.2) - translate([0, 0, -50]) - cylinder(d=STEM_OD, h=ANCHOR_SPACING + COL_H + 100); - - // Lower anchor (Z = 0) - single_anchor_assembly(show_phantom=true); - - // Upper anchor (Z = ANCHOR_SPACING) - translate([0, 0, ANCHOR_SPACING]) - single_anchor_assembly(show_phantom=true); -} - diff --git a/jetson/ros2_ws/src/saltybot_uwb_position/test/test_uwb_position.py b/jetson/ros2_ws/src/saltybot_uwb_position/test/test_uwb_position.py new file mode 100644 index 0000000..174ee39 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_position/test/test_uwb_position.py @@ -0,0 +1,89 @@ +""" +Unit tests for saltybot_uwb_position.uwb_position_node (Issue #546). +No ROS2 or hardware required — tests the covariance math only. +""" + +import math +import sys +import os + +# Make the package importable without a ROS2 install +sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) + + +# ── Covariance helper (extracted from node for unit testing) ────────────────── + +def polar_to_cartesian_cov(bearing_rad, range_m, sigma_r, sigma_theta): + """Compute 2×2 Cartesian covariance from polar uncertainty.""" + cos_b = math.cos(bearing_rad) + sin_b = math.sin(bearing_rad) + j00 = cos_b; j01 = -range_m * sin_b + j10 = sin_b; j11 = range_m * cos_b + sr2 = sigma_r * sigma_r + st2 = sigma_theta * sigma_theta + cov_xx = j00 * j00 * sr2 + j01 * j01 * st2 + cov_xy = j00 * j10 * sr2 + j01 * j11 * st2 + cov_yy = j10 * j10 * sr2 + j11 * j11 * st2 + return cov_xx, cov_xy, cov_yy + + +# ── Tests ───────────────────────────────────────────────────────────────────── + +class TestPolarToCartesianCovariance: + + def test_forward_bearing_zero(self): + """At bearing=0 (directly ahead) covariance aligns with axes.""" + cov_xx, cov_xy, cov_yy = polar_to_cartesian_cov( + bearing_rad=0.0, range_m=5.0, sigma_r=0.10, sigma_theta=0.087 + ) + assert cov_xx > 0 + assert cov_yy > 0 + # At bearing=0: cov_xx = σ_r², cov_yy = (r·σ_θ)², cov_xy ≈ 0 + assert abs(cov_xx - 0.10 ** 2) < 1e-9 + assert abs(cov_xy) < 1e-9 + expected_yy = (5.0 * 0.087) ** 2 + assert abs(cov_yy - expected_yy) < 1e-6 + + def test_sideways_bearing(self): + """At bearing=90° covariance axes swap.""" + sigma_r = 0.10 + sigma_theta = 0.10 + r = 3.0 + cov_xx, cov_xy, cov_yy = polar_to_cartesian_cov( + bearing_rad=math.pi / 2, range_m=r, + sigma_r=sigma_r, sigma_theta=sigma_theta + ) + # At bearing=90°: cov_xx = (r·σ_θ)², cov_yy = σ_r² + assert abs(cov_xx - (r * sigma_theta) ** 2) < 1e-9 + assert abs(cov_yy - sigma_r ** 2) < 1e-9 + + def test_covariance_positive_definite(self): + """Matrix must be positive semi-definite (det ≥ 0, diag > 0).""" + for bearing in [0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0]: + for r in [1.0, 5.0, 10.0]: + cov_xx, cov_xy, cov_yy = polar_to_cartesian_cov( + bearing, r, sigma_r=0.10, sigma_theta=0.087 + ) + assert cov_xx > 0 + assert cov_yy > 0 + det = cov_xx * cov_yy - cov_xy ** 2 + assert det >= -1e-12, f"Non-PSD at bearing={bearing}, r={r}: det={det}" + + def test_inflation_single_anchor(self): + """Covariance doubles (variance ×4) when only one anchor active.""" + sigma_r = 0.10 + sigma_theta = 0.087 + bearing = 0.5 + r = 4.0 + cov_xx_full, _, _ = polar_to_cartesian_cov(bearing, r, sigma_r, sigma_theta) + cov_xx_half, _, _ = polar_to_cartesian_cov( + bearing, r, + sigma_r * math.sqrt(4.0), + sigma_theta * math.sqrt(4.0), + ) + assert abs(cov_xx_half / cov_xx_full - 4.0) < 1e-9 + + +if __name__ == "__main__": + import pytest + pytest.main([__file__, "-v"])