diff --git a/BOM_GOPRO_MOUNT.md b/BOM_GOPRO_MOUNT.md new file mode 100644 index 0000000..92f54f0 --- /dev/null +++ b/BOM_GOPRO_MOUNT.md @@ -0,0 +1,155 @@ +# GoPro Mount Adapter - Bill of Materials +## Issue #195 + +### 3D Printed Components + +| Part | Qty | Material | Weight | Notes | +|------|-----|----------|--------|-------| +| GoPro Mount Base | 1 | PLA/PETG | ~8g | Top interface, receives GoPro 3-prong connector | +| Tilt Arm Assembly | 1 | PLA/PETG | ~12g | Connects GoPro mount to T-slot clamp, rotates on pivot | +| Pivot Housing | 1 | PLA/PETG | ~10g | Mounts to T-slot clamp, houses pivot pin | +| T-Slot Clamp Base | 1 | PLA/PETG | ~6g | Fixed base mounting to sensor rail | + +**Subtotal Printed:** ~36g + +### Hardware Components + +| Part | Qty | Size | Material | Source | Notes | +|------|-----|------|----------|--------|-------| +| Pivot Pin (Bolt) | 1 | M8 x 40mm | Steel | Hardware Store | Central pivot for tilt mechanism | +| Pivot Pin (Nut) | 1 | M8 | Steel | Hardware Store | Secures pivot pin | +| Thumbscrew | 1 | M5 x 8mm | Steel with Knob | Hardware Store | Locks tilt position (standard M5 thumb knob) | +| Detent Spring Ball | 1 | 4mm stainless | Stainless Steel | Ball bearing supplier | Springs into detent pockets (0°, 15°, 30°, 45°, 60°, 75°, 90°) | +| Compression Spring | 1 | M4 x 15mm | Steel | Hardware Store | Pushes detent ball into pockets (spring constant ~3-4 N/mm) | +| T-Nut | 1 | M4 x 20mm | Aluminum | T-slot Supplier | For T-slot rail mounting | +| M4 Bolt | 4 | 8-10mm length | Steel | Hardware Store | Mounting bolts for T-nut (4-point pattern) | +| Washer | 4 | M4 | Stainless Steel | Hardware Store | Under bolt heads to distribute load | +| Hex Nut | 1 | M8 | Steel | Hardware Store | Secures pivot pin (if needed for adjustment) | + +### Assembly Materials + +| Item | Qty | Type | Purpose | Notes | +|------|-----|------|---------|-------| +| Thread-lock (Loctite) | 1 small tube | Medium strength (blue) | Lock thumbscrew position | Prevents vibration loosening | +| Grease (PTFE-based) | 1 small tube | Silicone or PTFE | Lubricate pivot joint | For smooth tilt operation | +| Sandpaper | 1 set | P400/P600 grit | Post-print finishing | Sand all moving surfaces smooth | + +### Print Settings + +| Parameter | Recommended | +|-----------|-------------| +| Material | PLA or PETG | +| Nozzle Temperature | 200-210°C (PLA) / 230-245°C (PETG) | +| Bed Temperature | 60°C (PLA) / 80-90°C (PETG) | +| Layer Height | 0.2mm (standard) or 0.1mm (fine) | +| Infill | 20-25% (higher for pivot areas due to stress) | +| Supports | Yes - required for GoPro mount underside and pivot pockets | +| Print Time | ~4-5 hours | +| Estimated Weight | 36g printed (matches sub-50g target if optimized) | + +### Assembly Checklist + +- [ ] Print all 4 components with supports +- [ ] Remove supports and sand surfaces +- [ ] Sand pivot surfaces smooth (P600) for frictionless rotation +- [ ] Clean all parts with compressed air +- [ ] Install M8 pivot pin through housing and arm +- [ ] Install M8 nut and washer on pivot pin (hand-tight + 1/2 turn) +- [ ] Apply PTFE grease to pivot joint +- [ ] Install 4mm detent ball in spring +- [ ] Insert spring + ball assembly into detent housing +- [ ] Install compression spring and M4 bolt for spring retention +- [ ] Test tilt mechanism: should click into 7 detent positions +- [ ] Install T-nut and M4 mounting bolts to T-slot rail +- [ ] Install M5 thumbscrew for tilt locking +- [ ] Apply thread-lock to thumbscrew (if needed) +- [ ] Test GoPro mount: insert standard GoPro 3-prong connector +- [ ] Verify all tilt detents hold firmly +- [ ] Test thumbscrew locking at various angles + +### Sourcing Notes + +#### Where to Buy + +**3D Printing Service (if not printing yourself):** +- Prusa Printing (prusa3d.com) +- Shapeways (shapeways.com) +- OnShape + local printer +- ~$15-25 for all parts in PLA + +**Hardware (Local Options):** +- Home Depot / Lowes: T-nuts, bolts, washers, springs +- Ace Hardware: Thumbscrew knobs, grease +- Amazon: Bulk hardware kits, ball bearings +- McMaster-Carr: Premium hardware, precision springs + +**Specialty (Online):** +- SDP/SI (sdp-si.com): Precision balls, springs +- VXB Bearings: Ball bearings and precision components +- Misumi: Aluminum hardware + +### Cost Breakdown + +| Category | Cost | Notes | +|----------|------|-------| +| 3D Printing | $2-5 | If self-printing; $15-25 if outsourced | +| Hardware | $8-12 | Pivot pin, bolts, springs, thumbscrew | +| Assembly Materials | $3-5 | Grease, sandpaper, thread-lock | +| **Total** | **$13-22** | Budget-friendly sensor integration | + +### Weight Analysis + +| Component | Weight | +|-----------|--------| +| Printed parts | 36g | +| M8 bolt + nut | 8g | +| M5 thumbscrew | 3g | +| Springs + ball | 2g | +| T-nut + M4 bolts | 6g | +| **Total** | **~55g** | + +**Note:** Can optimize to <50g by: +- Reducing infill to 15% in non-stress areas +- Using aluminum M8 bolt instead of steel +- Using nylon M4 bolts instead of steel + +### Quality Assurance + +#### Testing Points + +1. **Pivot Smoothness:** No grinding or binding, smooth rotation +2. **Detent Engagement:** Audible/tactile click at each 15° position +3. **Tilt Lock:** Thumbscrew holds position against moderate force (~5 kg applied force) +4. **GoPro Interface:** Standard GoPro mount seats fully and securely +5. **T-slot Mounting:** Rock-solid attachment, no wobble +6. **Repeated Use:** 100+ tilt cycles without wear + +### Maintenance + +- **Monthly:** Check thumbscrew tightness, verify detents still click +- **As Needed:** Reapply PTFE grease to pivot if rotation becomes rough +- **If Detents Wear:** Replace 4mm ball or detent pocket (reprint arm if needed) +- **If Pivot Loosens:** Retighten M8 nut with 2mm clearance (should not be too tight) + +### Compatibility Notes + +**GoPro Models Supported:** +- GoPro Hero 11/12 (with standard 3-prong mount) +- GoPro Hero 9/10 (with standard 3-prong mount) +- GoPro Hero 8 (with standard 3-prong mount) +- Any device with standard GoPro 3-prong interface + +**Not Compatible:** +- GoPro Hero 7 or older (different mounting system) +- Devices without GoPro 3-prong connector + +**Adapter Options:** +- Can add GoPro-to-other-mount adapters on top for non-GoPro cameras +- Design allows stacking of standard GoPro accessories + +--- + +**Bill of Materials Version:** 1.0 +**Design Status:** Complete +**Last Updated:** 2026-03-02 +**Issue:** #195 diff --git a/chassis/gopro_mount.scad b/chassis/gopro_mount.scad new file mode 100644 index 0000000..2015724 --- /dev/null +++ b/chassis/gopro_mount.scad @@ -0,0 +1,256 @@ +// ============================================================ +// gopro_mount.scad — GoPro 3-Prong T-Slot Sensor Rail Mount +// Issue: #195 Agent: sl-mechanical Date: 2026-03-02 +// ============================================================ +// +// Universal GoPro camera mount for sensor_rail.scad (T-slot 2020). +// +// Top interface: Standard GoPro 3-prong (HERO 5+) +// Prongs slide into standard GoPro accessories. +// No additional clips or fasteners needed. +// +// Bottom clamp: T-slot 20×20 mm rail system (OpenBuilds compatible). +// M3 thumbscrew retention + M5 index holes for 15° detents. +// +// Tilt mechanism: Rotating hinge bracket allows camera tilt 0°–90° from +// horizontal, locked at 15° increments via index holes. +// Rotation axis is perpendicular to rail (about Y-axis). +// +// Assembly: +// 1. Print gopro_mount_base (single part, flat-side-down, no supports) +// 2. Slide built-in T-nut into rail T-groove +// 3. Clamp with M3 thumbscrew from outside rail face +// 4. Rotate camera to desired tilt angle (0°, 15°, 30°, ... 90°) +// 5. Insert M5 alignment pin to lock tilt +// +// RENDER options: +// "mount" assembled mount view (default) +// "base_stl" base clamp for slicing (print flat) +// "camera_bracket" rotatable camera bracket for slicing +// ============================================================ + +RENDER = "mount"; + +// ── GoPro 3-prong interface (top) ────────────────────────── +// Standard GoPro HERO 5+ 3-prong attachment point. +// Overall footprint ~43×25 mm, 3 prongs slide into camera mounting slot. + +GOPRO_WIDTH = 43.0; // front-to-back along camera face +GOPRO_DEPTH = 25.0; // left-to-right width +GOPRO_PRONG_H = 5.0; // height of prongs above base +GOPRO_PRONG_D = 4.0; // diameter/thickness of each prong +GOPRO_PRONG_SP = 14.0; // spacing between outer prongs (centre-to-centre) + +// Centre prong is at Y=0; outer prongs at Y±GOPRO_PRONG_SP/2 +// Prongs extend upward (+Z direction) + +// ── T-slot clamp (bottom) ────────────────────────────────── +// Matches sensor_rail.scad T-slot 2020 dimensions. +RAIL_W = 20.0; // rail outer width/height +SLOT_INNER_W = 10.2; // T-groove inner width +SLOT_INNER_H = 5.8; // T-groove inner height (depth) +SLOT_NECK_H = 3.2; // distance from outer face to T-groove + +TNUT_W = 9.8; // printable T-nut width (SLOT_INNER_W - 0.4) +TNUT_H = 5.5; // printable T-nut height (SLOT_INNER_H - 0.3) +TNUT_L = 12.0; // T-nut body length +TNUT_BOLT_D = 3.3; // M3 clearance bore through T-nut + +THUMB_D = 16.0; // thumbwheel OD (visual only) +THUMB_H = 8.0; // thumbwheel height + +// Index hole pitch for tilt detents +INDEX_PITCH = 25.0; // 25 mm spacing on rail +TILT_ANGLE_STEP = 15.0; // 15° tilt detents (0°, 15°, 30°, ... 90°) +M5_INDEX_D = 5.3; // M5 clearance hole diameter + +// ── Rotation/tilt mechanism ─────────────────────────────── +// Camera bracket rotates about Y-axis relative to base. +// Rotation axis is at the rail clamp interface (Z=0, Y=0). +// Tilt range: 0° to 90°. + +MAX_TILT_DEG = 90.0; // maximum tilt angle + +// Hinge pin: M5 bolt passes through base and camera bracket +HINGE_PIN_D = 5.3; // M5 bolt clearance +HINGE_PIN_L = 25.0; // pin length (accommodates rail width + brackets) + +// ── General ──────────────────────────────────────────────── +$fn = 64; +e = 0.01; + +// ───────────────────────────────────────────────────────────── +// gopro_mount_base() +// T-slot clamp bracket with integrated T-nut. +// Mounted at bottom, rail clamp at Z=0 face. +// Hinge pin axis at Y=0, rotates camera bracket ±X. +// +// Print: flat-side-down (rail face down), no supports needed. +// ───────────────────────────────────────────────────────────── +module gopro_mount_base() { + clamp_h = RAIL_W + 8.0; // total clamp height + clamp_w = TNUT_L + 8.0; // clamp width (along rail) + clamp_d = 18.0; // clamp depth (front-to-back) + + difference() { + union() { + // Main clamp body + translate([-clamp_w/2, -clamp_d/2, 0]) + cube([clamp_w, clamp_d, clamp_h]); + + // T-nut integration: slot to hold T-nut at rail interface + // T-nut sits in T-groove when rail is inserted from above + translate([-TNUT_L/2, -TNUT_W/2, SLOT_NECK_H]) + cube([TNUT_L, TNUT_W, TNUT_H]); + + // Hinge boss (supports rotation axis pin) + translate([-HINGE_PIN_L/2 - 2, -8, clamp_h - 4]) + cube([HINGE_PIN_L + 4, 4, 4]); + } + + // Rail bore (25.4 mm) — matches stem adapter pattern + // When rail is vertical, this bore slides onto rail extrusion + translate([0, 0, -e]) + cylinder(d=RAIL_W + 0.4, h=clamp_h + 2*e); + + // T-nut through-bolt (M3 thumbscrew hole) + // Bolt comes from outside rail face, through T-nut, clamping it + translate([0, 0, SLOT_NECK_H + TNUT_H/2]) + rotate([90, 0, 0]) + cylinder(d=TNUT_BOLT_D, h=clamp_d + 2*e); + + // Hinge pin (M5) passes through base to camera bracket + translate([-HINGE_PIN_L/2, 0, clamp_h - 2]) + rotate([0, 90, 0]) + cylinder(d=HINGE_PIN_D, h=HINGE_PIN_L + 2*e); + + // Index hole pockets for tilt angle lock (M5) + // One index hole on each side to lock camera tilt + for (angle = [0 : TILT_ANGLE_STEP : MAX_TILT_DEG]) { + // Holes are radially spaced around the hinge axis + y_offset = (HINGE_PIN_L/2 - 2) * sin(angle); + z_offset = clamp_h - 2 - (HINGE_PIN_L/2 - 2) * (1 - cos(angle)); + translate([0, y_offset, z_offset]) + cylinder(d=M5_INDEX_D, h=4, center=false); + } + + // Vent slots (optional, aesthetic + weight reduction) + for (dz = [4, 8, 12]) + translate([-clamp_w/2 + 2, -clamp_d/2 - e, dz]) + cube([clamp_w - 4, 2, 1.5]); + } +} + +// ───────────────────────────────────────────────────────────── +// gopro_camera_bracket() +// Rotatable bracket that holds GoPro camera via 3-prong mount. +// Rotates about Y-axis (hinge pin), tilt 0°–90°. +// +// Hinge pin (M5 bolt) enters from base and locks bracket rotation. +// Index hole on bracket aligns with base index holes at each 15° step. +// +// Print: flat-side-down (prong face down), no supports. +// ───────────────────────────────────────────────────────────── +module gopro_camera_bracket() { + bracket_h = GOPRO_WIDTH + 8.0; // height when vertical + bracket_w = GOPRO_DEPTH + 6.0; // width (left-right) + bracket_t = 4.0; // bracket thickness + + difference() { + union() { + // Main bracket body + translate([-bracket_w/2, -bracket_t/2, 0]) + cube([bracket_w, bracket_t, bracket_h]); + + // Hinge pivot boss + translate([-HINGE_PIN_L/2 - 2, 0, bracket_h - 4]) + cube([HINGE_PIN_L + 4, bracket_t, 4]); + + // GoPro prong mounting boss + translate([-GOPRO_DEPTH/2, 0, bracket_h - GOPRO_WIDTH/2]) + cube([GOPRO_DEPTH, bracket_t + 2, GOPRO_WIDTH]); + } + + // Hinge pin bore (M5) + translate([-HINGE_PIN_L/2, 0, bracket_h - 2]) + rotate([0, 90, 0]) + cylinder(d=HINGE_PIN_D + 0.2, h=HINGE_PIN_L + 2*e); + + // Index hole (M5) for angle lock + // Bracket has one index hole that aligns with base holes at tilt angles + translate([GOPRO_DEPTH/2 + 1, 0, bracket_h - 4]) + cylinder(d=M5_INDEX_D, h=bracket_t + 2*e); + + // GoPro prong socket (3 mounting prongs) + // Centre prong at Y=0, outer prongs at Y±GOPRO_PRONG_SP/2 + for (dy = [0, GOPRO_PRONG_SP/2, -GOPRO_PRONG_SP/2]) { + translate([0, bracket_t + e, bracket_h - GOPRO_WIDTH/2 + 5 - dy]) + rotate([90, 0, 0]) + cylinder(d=GOPRO_PRONG_D + 0.4, h=4); + } + } +} + +// ───────────────────────────────────────────────────────────── +// gopro_prong_interface() +// Visual representation of the 3 prongs that camera mounts to. +// Not printed; shown during assembly view. +// ───────────────────────────────────────────────────────────── +module gopro_prong_interface() { + bracket_h = GOPRO_WIDTH + 8.0; + bracket_w = GOPRO_DEPTH + 6.0; + + // 3 cylindrical prongs + for (dy = [0, GOPRO_PRONG_SP/2, -GOPRO_PRONG_SP/2]) { + translate([0, bracket_w/2 + 0.5, bracket_h - GOPRO_WIDTH/2 + 5 - dy]) + rotate([90, 0, 0]) + cylinder(d=GOPRO_PRONG_D, h=GOPRO_PRONG_H, $fn=32); + } +} + +// ───────────────────────────────────────────────────────────── +// Assembly view: base + rotatable camera bracket +// ───────────────────────────────────────────────────────────── +module gopro_mount_assembly() { + // Base clamp (fixed) + color("SteelBlue", 0.9) + gopro_mount_base(); + + // Camera bracket (rotatable, at 45° tilt for visualization) + color("CornflowerBlue", 0.85) + rotate([0, 45, 0]) + translate([0, 0, RAIL_W + 8]) + gopro_camera_bracket(); + + // Visual GoPro prongs + color("LightSteelBlue", 0.7) + rotate([0, 45, 0]) + translate([0, 0, RAIL_W + 8]) + gopro_prong_interface(); + + // Phantom M5 hinge pin (visual reference) + color("Silver", 0.5) + translate([-HINGE_PIN_L/2, 0, RAIL_W + 8 - 2]) + rotate([0, 90, 0]) + cylinder(d=5, h=HINGE_PIN_L); +} + +// ───────────────────────────────────────────────────────────── +// Render selector +// ───────────────────────────────────────────────────────────── +if (RENDER == "mount") { + gopro_mount_assembly(); + +} else if (RENDER == "base_stl") { + // Flat print orientation: rail-facing side down + rotate([180, 0, 0]) + translate([0, 0, -RAIL_W - 8]) + gopro_mount_base(); + +} else if (RENDER == "camera_bracket") { + // Flat print orientation: prong-facing side down + rotate([0, 0, 0]) + translate([0, 0, GOPRO_WIDTH + 14]) + rotate([180, 0, 0]) + gopro_camera_bracket(); +} diff --git a/chassis/gopro_mount_BOM.md b/chassis/gopro_mount_BOM.md new file mode 100644 index 0000000..c1a1367 --- /dev/null +++ b/chassis/gopro_mount_BOM.md @@ -0,0 +1,252 @@ +# GoPro 3-Prong T-Slot Sensor Rail Mount — BOM + Assembly +**Rev A — 2026-03-02 — sl-mechanical** + +--- + +## System Overview + +Universal GoPro HERO 5+ camera mount for sensor_rail.scad (T-slot 20×20 mm). + +| Feature | Spec | +|---------|------| +| Camera interface | Standard GoPro 3-prong (HERO 5+) | +| Rail system | T-slot 2020 OpenBuilds / MISUMI compatible | +| Tilt range | 0° – 90° from horizontal | +| Tilt detents | 15° increments (0°, 15°, 30°, 45°, 60°, 75°, 90°) | +| Retention | M3 thumbscrew clamp (T-nut to rail) + M5 index pin (angle lock) | +| Weight | ~85 g (printed) + ~45 g (fasteners) | + +--- + +## Design Features + +- **Top mount:** Standard GoPro 3-prong interface — no custom adapters, works with any GoPro HERO 5+ compatible accessory. +- **Bottom clamp:** Integrated T-nut slides into rail T-groove; single M3 thumbscrew locks clamp to rail (no tools required). +- **Tilt mechanism:** Rotating bracket + M5 hinge pin; camera tilts 0°–90° and locks at 15° steps via M5 index holes. +- **Printability:** Both parts print flat-face-down, no support material required. +- **Compatibility:** Works with all SaltyLab / SaltyRover sensor_rail.scad mounts; positions camera anywhere along rail. + +--- + +## Part A — Printed Components + +### 1. Base Clamp (gopro_mount.scad, RENDER="base_stl") + +**Purpose:** Holds T-nut in rail T-groove; provides rotation axis for camera bracket. + +| Parameter | Value | Notes | +|-----------|-------|-------| +| Qty | 1 | Single part per mount | +| Material | PETG | 5 perimeters, 40% infill (gyroid) | +| Print size | ~45 × 35 × 28 mm | Flat-face-down (rail-side down) | +| Orientation | Rail bore facing DOWN | Supports natural bed adhesion | +| Support | None | Design allows direct print | +| Wall thickness | 4–5 mm minimum | Ensure M3 threads cut cleanly | + +**Key features:** +- Integrated T-nut pocket (friction-fit to rail T-groove) +- M3 thumbscrew bore (perpendicular to T-nut, from outside rail face) +- M5 hinge-pin bore (passes through to camera bracket) +- 7 index holes (one for each tilt angle: 0°, 15°, 30°, 45°, 60°, 75°, 90°) +- Vent slots for weight reduction + +--- + +### 2. Camera Bracket (gopro_mount.scad, RENDER="camera_bracket") + +**Purpose:** Holds GoPro camera via 3-prong mount; rotates about M5 hinge pin. + +| Parameter | Value | Notes | +|-----------|-------|-------| +| Qty | 1 | Single part per mount | +| Material | PETG | 4 perimeters, 30% infill | +| Print size | ~35 × 25 × 51 mm | Flat-face-down (prong-side down) | +| Orientation | GoPro prongs facing DOWN | Minimal post-processing | +| Support | None | Design supports printing without supports | +| Wall thickness | 3–4 mm | Adequate for M5 through-bore | + +**Key features:** +- 3 cylindrical prong sockets (GoPro standard spacing) +- M5 hinge-pin bore (receives bolt from base) +- 1 index hole (aligns with base index holes at tilt angle) +- Lightweight design + +--- + +## Part B — Fasteners & Hardware + +All fasteners are stainless steel or zinc-plated for corrosion resistance. + +| # | Spec | Qty | Use | Notes | +|---|------|-----|-----|-------| +| 1 | M3 × 12 SHCS | 1 | Thumbscrew — clamps T-nut to rail | or M3 cap screw with printed knob | +| 2 | M3 washer | 1 | Under thumbscrew head | standard flat washer | +| 3 | M5 × 25 SHCS | 1 | Hinge pin — camera bracket rotation axis | or M5 cap screw + lock washer | +| 4 | M5 washer | 2 | Hinge pin lock washers (each end) | prevents backlash | +| 5 | M5 lock nut | 1 | Hinge pin lock nut | nylon-insert preferred | +| 6 | M5 × 8 roll pin | 7 | Index pins — lock tilt angle | one for each 15° detent (0°–90°) | + +--- + +## Installation & Assembly + +### Step 1: Prepare T-slot Rail + +1. Verify rail is clean (no burrs, chips). +2. Slide base clamp assembly onto rail from above (rail face toward clamp bore). +3. Align T-nut pocket with T-slot T-groove (groove faces inward). + +### Step 2: Clamp Base to Rail + +1. Insert M3 thumbscrew through rail outer face, through the T-nut pocket. +2. Tighten **finger-tight** first (do not overtighten — rail will deform). +3. Verify clamp is centered on rail (equal gap on both sides). +4. Tighten to **hand-tight** (≈2 N·m for M3). + +### Step 3: Assemble Hinge & Camera Bracket + +1. Slide M5 × 25 bolt through base clamp hinge bore (horizontal, perpendicular to rail). +2. Mount camera bracket on bolt (prong-side facing camera direction). +3. Add M5 washers at both bolt ends. +4. Tighten M5 lock nut to **hand-tight** (≈5 N·m). + - Bolt should rotate smoothly but without play. + - If bracket binds, loosen slightly; if too loose, add second lock washer. + +### Step 4: Mount GoPro Camera + +1. Rotate camera bracket to desired tilt angle (0°, 15°, 30°, etc.). +2. Align index hole on bracket with corresponding hole on base clamp. +3. Insert M5 × 8 roll pin into aligned holes (pushes in snugly, no tools needed). +4. Verify pin locks camera position (should not rotate when pin is seated). +5. Slide GoPro onto 3 prongs until camera contacts mount. + +### Step 5: Cable Management + +- Route camera USB/HDMI cable down rail (behind robot). +- Use cable tie on thumbscrew pocket if additional restraint needed. + +--- + +## Tilt Angle Detents + +The 15° step gives 7 fixed positions within 0°–90° range: + +| Tilt Index | Angle | Use case | +|------------|-------|----------| +| 1 | 0° | Horizontal (level with horizon) | +| 2 | 15° | Slight upward tilt | +| 3 | 30° | Moderate upward tilt | +| 4 | 45° | Balanced forward-up tilt | +| 5 | 60° | Steep upward tilt | +| 6 | 75° | Nearly vertical forward | +| 7 | 90° | Straight vertical (zenith) | + +To change tilt: +1. Push on camera bracket to rotate (index pin will pop out if at end of rotation). +2. Align new index hole. +3. Re-insert index pin. + +--- + +## Fastener Torque Spec + +| Fastener | Size | Torque | Note | +|----------|------|--------|------| +| M3 thumbscrew | M3 × 12 | 2 N·m | Hand-tight; over-tightening deforms rail | +| M5 hinge pin | M5 × 25 | 5 N·m | Smooth rotation, no play | +| M5 lock nut | M5 | 2.5 N·m | After initial tightening, verify free rotation | + +--- + +## Post-Print Finishing + +### Base Clamp +- **Support removal:** No supports needed. +- **Bore prep:** If M3 bore is rough, ream with M3 hand reamer (¼ turn only). +- **T-nut pocket:** Smooth with fine sandpaper if needed (must slide smoothly into groove). + +### Camera Bracket +- **Support removal:** No supports needed. +- **Prong sockets:** Clean with compressed air; verify no strands block GoPro prongs. +- **Index hole:** Verify M5 bore is clear (small drill bit can clear if needed). + +--- + +## Mass Estimate + +| Component | Material | Est. Mass | +|-----------|----------|-----------| +| Base clamp | PETG | ~35 g | +| Camera bracket | PETG | ~28 g | +| M3 thumbscrew | Stainless | ~2 g | +| M5 hinge bolt + nut | Stainless | ~6 g | +| M5 index pins (×7) | Steel | ~4 g | +| **Total (without camera)** | | **~75 g** | +| **Total (with GoPro HERO)** | | **~250 g** | + +--- + +## Mounting Position Recommendations + +### On sensor_rail.scad vertical rail + +| Position | Z height | Use | +|----------|----------|-----| +| Below sensor head | 700–800 mm | Forward-facing wide-angle view | +| Mid-rail | 400–500 mm | Side-looking perspective | +| Above base plate | 100–200 mm | Ground-level or low-angle view | + +Index holes on rail face every 25 mm; position bracket at any height (friction-fit with T-nut + thumbscrew). + +### On payload_bay_rail.scad (horizontal rail) + +Mount horizontally by: +1. Rotate entire assembly 90° (rail runs front-to-back). +2. Tilt camera bracket 0°–90° to point downward or forward. +3. Good for payload bay documentation (downward view) or forward obstacle detection. + +--- + +## Storage & Maintenance + +- **Store bracket vertical** (prongs up) to avoid stress on sockets. +- **Clean prongs** after each field session (dust prevents secure seating). +- **Verify index pin seating** before each deployment (pin can loosen if rattled). +- **Check thumbscrew** monthly (re-tighten if rail has shifted). + +--- + +## Design Notes for Future Revisions + +- **Optional:** Add detent detents (spring-loaded ball bearing pockets) at index holes for positive clicks. +- **Optional:** Add strap attachment points on base clamp for secondary safety line. +- **Alternative bracket:** Straight vertical bracket (0° fixed) for weight-optimized variant. +- **Camera variants:** Compatibility with GoPro Session (action cam) and DJI Osmo Action (different prong patterns) would require bracket redesign. + +--- + +## Files + +| File | Purpose | +|------|---------| +| `gopro_mount.scad` | OpenSCAD parametric model (3 RENDER variants) | +| `gopro_mount_BOM.md` | This document | +| `sensor_rail.scad` | Parent T-slot rail system (dependency) | +| `sensor_rail_brackets.scad` | Additional rail bracket options | + +--- + +## Assembly Checklist + +- [ ] Print base clamp (PETG, 45 × 35 × 28 mm, flat-side-down) +- [ ] Print camera bracket (PETG, 35 × 25 × 51 mm, flat-side-down) +- [ ] Inspect prong sockets on bracket (no strands, smooth) +- [ ] Ream or smooth M3 thumbscrew bore in base clamp +- [ ] Test T-nut pocket (slides in/out of rail groove smoothly) +- [ ] Assemble: M3 thumbscrew + washer +- [ ] Assemble: M5 × 25 bolt + 2 washers + lock nut +- [ ] Mount bracket on hinge pin (hand-tight fit) +- [ ] Insert one M5 × 8 index pin at 0° (test fit) +- [ ] Mount GoPro camera (prongs snap in) +- [ ] Final check: camera level in 0° position +- [ ] Stow extra index pins in camera bag diff --git a/gopro_mount.scad b/gopro_mount.scad new file mode 100644 index 0000000..cb15373 --- /dev/null +++ b/gopro_mount.scad @@ -0,0 +1,255 @@ +// GoPro Mount Adapter for T-Slot Sensor Rail +// Issue #195 - GoPro 3-prong interface with tilt detents +// T-slot mounting with thumbscrew retention + +// GoPro 3-Prong Interface Standards +// Center prong: 14.5mm wide +// Side prongs: 12.5mm wide +// Prong depth: 8-10mm +// Spacing between prongs: ~25mm + +// Constants +GOPRO_PRONG_WIDTH_CENTER = 14.5; +GOPRO_PRONG_WIDTH_SIDE = 12.5; +GOPRO_PRONG_DEPTH = 9; +GOPRO_PRONG_SPACING = 25; // Between center of side prongs + +T_SLOT_WIDTH = 20; // T-slot rail 20x20mm +CLAMP_JAW_DEPTH = 18; // Clamp jaw depth +CLAMP_WIDTH = 40; // Clamp internal width + +TILT_PIVOT_DIAMETER = 12; // Pivot pin diameter +TILT_MIN = 0; // Minimum tilt angle +TILT_MAX = 90; // Maximum tilt angle +TILT_DETENT_INTERVAL = 15; // Detent every 15 degrees + +THUMBSCREW_DIAMETER = 10; // M5 equivalent +THUMBSCREW_DEPTH = 8; // Engagement depth + +// Detent positions: 0, 15, 30, 45, 60, 75, 90 degrees +DETENT_POSITIONS = [0, 15, 30, 45, 60, 75, 90]; + +// ==================== GOPRO INTERFACE ==================== + +module gopro_mounting_base() { + // Base plate that receives GoPro 3-prong connector + // Dimensions accommodate standard GoPro mounts + + linear_extrude(height = 8) { + // Main base: 50x35mm + square([50, 35], center = true); + } +} + +module gopro_prong_slot_center() { + // Center prong slot (larger for main mounting) + // Slot dimensions: 14.5mm wide x 9mm deep x 12mm tall + translate([0, 0, 4]) { + cube([GOPRO_PRONG_WIDTH_CENTER, GOPRO_PRONG_DEPTH + 2, 12], center = true); + } +} + +module gopro_prong_slot_side(x_offset) { + // Side prong slots (two of them, mirrored) + translate([x_offset, 0, 4]) { + cube([GOPRO_PRONG_WIDTH_SIDE, GOPRO_PRONG_DEPTH + 2, 12], center = true); + } +} + +module gopro_interface_slots() { + // All three prong slots for GoPro mount + gopro_prong_slot_center(); // Center + gopro_prong_slot_side(GOPRO_PRONG_SPACING / 2); // Right + gopro_prong_slot_side(-GOPRO_PRONG_SPACING / 2); // Left +} + +module gopro_mounting_assembly() { + difference() { + gopro_mounting_base(); + gopro_interface_slots(); + } + + // Add retaining tabs (small lips to keep mount from sliding out) + translate([0, GOPRO_PRONG_DEPTH/2 + 1, 7]) { + cube([50, 2, 2], center = true); + } +} + +// ==================== TILT MECHANISM ==================== + +module tilt_pivot_pin() { + // Central pivot for tilt adjustment + // M8 equivalent bolt hole with shoulder + + // Main pin body + cylinder(h = 40, r = TILT_PIVOT_DIAMETER/2 + 1, $fn = 32); + + // Bolt hole through center + cylinder(h = 42, r = 2.2, $fn = 16, center = true); +} + +module detent_pocket(angle) { + // Spherical detent pocket at specific angle + // Located on tilt arm, engages with detent spring ball + + sphere_r = 2; + translate([0, 0, 15]) { + rotate([angle, 0, 0]) { + translate([0, TILT_PIVOT_DIAMETER/2 + 4, 0]) { + sphere(r = sphere_r, $fn = 16); + } + } + } +} + +module detent_pockets() { + // Create all detent pockets for 0-90 degree range + for (angle = DETENT_POSITIONS) { + detent_pocket(angle); + } +} + +module tilt_arm() { + // Arm that connects GoPro mount to T-slot clamp + // Rotates around pivot pin for tilt adjustment + + // Main arm structure (tapered for strength) + hull() { + // Top section (connects to GoPro mount) + translate([0, 0, 35]) { + cube([40, 25, 10], center = true); + } + + // Bottom section (connects to T-slot clamp) + translate([0, 0, 0]) { + cube([35, 20, 8], center = true); + } + } +} + +module tilt_mechanism_assembly() { + // Pivot pin at center + translate([0, 0, 8]) { + tilt_pivot_pin(); + } + + // Tilt arm with GoPro mount + translate([0, 0, 8]) { + rotate([0, 0, 0]) { // Default position: 0 degrees tilt + difference() { + union() { + tilt_arm(); + gopro_mounting_assembly(); + } + + // Pivot hole + translate([0, 0, -5]) { + cylinder(h = 50, r = 2.2, $fn = 16); + } + } + } + } + + // Detent ball pockets (visible for assembly reference) + translate([0, 0, 8]) { + detent_pockets(); + } +} + +// ==================== THUMBSCREW RETENTION ==================== + +module thumbscrew_mechanism() { + // Thumbscrew for locking tilt position + // M5 equivalent (10mm head) + + // Engagement hole on tilt arm + translate([15, 0, 20]) { + cylinder(h = THUMBSCREW_DEPTH + 2, r = THUMBSCREW_DIAMETER/2 + 0.5, $fn = 16); + } +} + +// ==================== T-SLOT CLAMP ==================== + +module tslot_clamp_base() { + // Base connector for T-slot mounting + // Similar to phone mount but simplified for GoPro (lighter load) + + linear_extrude(height = 12) { + square([25, 25], center = true); + } +} + +module tslot_mounting_holes() { + // M4 bolt holes for T-nut attachment + hole_inset = 7; + translate([hole_inset, hole_inset, -1]) + cylinder(h = 14, r = 2.2, $fn = 16); + translate([-hole_inset, hole_inset, -1]) + cylinder(h = 14, r = 2.2, $fn = 16); + translate([hole_inset, -hole_inset, -1]) + cylinder(h = 14, r = 2.2, $fn = 16); + translate([-hole_inset, -hole_inset, -1]) + cylinder(h = 14, r = 2.2, $fn = 16); +} + +module tslot_clamp_assembly() { + difference() { + tslot_clamp_base(); + tslot_mounting_holes(); + } +} + +// ==================== PIVOT HOUSING ==================== + +module pivot_housing() { + // Connects T-slot clamp to tilt mechanism + // Houses the pivot pin + + translate([0, 0, 12]) { + // Main housing body + cube([30, 30, 15], center = true); + + // Pivot socket (receives pivot pin) + translate([0, 0, 2]) { + cylinder(h = 10, r = TILT_PIVOT_DIAMETER/2 + 1.5, $fn = 32); + } + } +} + +module pivot_housing_assembly() { + difference() { + pivot_housing(); + + // Clearance hole for pivot pin + translate([0, 0, 10]) { + cylinder(h = 20, r = TILT_PIVOT_DIAMETER/2 + 0.2, $fn = 32); + } + } +} + +// ==================== COMPLETE ASSEMBLY ==================== + +module complete_gopro_mount() { + // Fixed base: T-slot clamp + tslot_clamp_assembly(); + + // Pivot housing + pivot_housing_assembly(); + + // Tilt mechanism with GoPro mount + tilt_mechanism_assembly(); + + // Thumbscrew engagement hole marker + thumbscrew_mechanism(); +} + +// ==================== RENDER ==================== +complete_gopro_mount(); + +// Visualization aids (comment out for export): +// Show tilt range visualization +// translate([80, 0, 0]) rotate([15, 0, 0]) complete_gopro_mount(); +// translate([160, 0, 0]) rotate([90, 0, 0]) complete_gopro_mount(); + +// Show T-slot envelope +// %translate([0, 0, 0]) cube([20, 20, 30], center = true); diff --git a/jetson/ros2_ws/src/saltybot_node_watchdog/config/watchdog_config.yaml b/jetson/ros2_ws/src/saltybot_node_watchdog/config/watchdog_config.yaml new file mode 100644 index 0000000..198eeb9 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_node_watchdog/config/watchdog_config.yaml @@ -0,0 +1,14 @@ +# Node watchdog configuration + +node_watchdog: + ros__parameters: + # Publishing frequency in Hz + frequency: 20 # 20 Hz = 50ms cycle + + # General heartbeat timeout (seconds) + # Alert if any heartbeat lost for this duration + heartbeat_timeout: 1.0 + + # Motor driver critical timeout (seconds) + # Trigger safety fallback (zero cmd_vel) if motor driver down this long + motor_driver_critical_timeout: 2.0 diff --git a/jetson/ros2_ws/src/saltybot_node_watchdog/launch/node_watchdog.launch.py b/jetson/ros2_ws/src/saltybot_node_watchdog/launch/node_watchdog.launch.py new file mode 100644 index 0000000..4c32dfe --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_node_watchdog/launch/node_watchdog.launch.py @@ -0,0 +1,36 @@ +"""Launch file for node_watchdog_node.""" + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +import os +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + """Generate launch description for node watchdog.""" + # Package directory + pkg_dir = get_package_share_directory("saltybot_node_watchdog") + + # Parameters + config_file = os.path.join(pkg_dir, "config", "watchdog_config.yaml") + + # Declare launch arguments + return LaunchDescription( + [ + DeclareLaunchArgument( + "config_file", + default_value=config_file, + description="Path to configuration YAML file", + ), + # Node watchdog node + Node( + package="saltybot_node_watchdog", + executable="node_watchdog_node", + name="node_watchdog", + output="screen", + parameters=[LaunchConfiguration("config_file")], + ), + ] + ) diff --git a/jetson/ros2_ws/src/saltybot_node_watchdog/package.xml b/jetson/ros2_ws/src/saltybot_node_watchdog/package.xml new file mode 100644 index 0000000..757cdf3 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_node_watchdog/package.xml @@ -0,0 +1,28 @@ + + + + saltybot_node_watchdog + 0.1.0 + + Node watchdog monitor for SaltyBot critical systems. Monitors heartbeats from balance, + motor driver, emergency, and docking nodes. Publishes alerts on heartbeat loss >1s. + Implements safety fallback: zeros cmd_vel if motor driver lost >2s. Runs at 20Hz. + + sl-controls + MIT + + rclpy + geometry_msgs + std_msgs + + ament_python + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_node_watchdog/resource/saltybot_node_watchdog b/jetson/ros2_ws/src/saltybot_node_watchdog/resource/saltybot_node_watchdog new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_node_watchdog/saltybot_node_watchdog/__init__.py b/jetson/ros2_ws/src/saltybot_node_watchdog/saltybot_node_watchdog/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_node_watchdog/saltybot_node_watchdog/node_watchdog_node.py b/jetson/ros2_ws/src/saltybot_node_watchdog/saltybot_node_watchdog/node_watchdog_node.py new file mode 100644 index 0000000..f3b1e52 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_node_watchdog/saltybot_node_watchdog/node_watchdog_node.py @@ -0,0 +1,235 @@ +#!/usr/bin/env python3 +"""Node watchdog monitor for SaltyBot critical systems. + +Monitors heartbeats from balance, motor driver, emergency, and docking nodes. +Publishes alerts on heartbeat loss >1s. Implements safety fallback: zeros cmd_vel +if motor driver lost >2s. + +Published topics: + /saltybot/node_watchdog (std_msgs/String) - JSON watchdog status + /saltybot/cmd_vel_safe (geometry_msgs/Twist) - cmd_vel with motor driver safety check + +Subscribed topics: + /saltybot/balance_heartbeat (std_msgs/UInt32) - Balance node heartbeat + /saltybot/motor_driver_heartbeat (std_msgs/UInt32) - Motor driver heartbeat + /saltybot/emergency_heartbeat (std_msgs/UInt32) - Emergency system heartbeat + /saltybot/docking_heartbeat (std_msgs/UInt32) - Docking node heartbeat + /cmd_vel (geometry_msgs/Twist) - Velocity command input +""" + +import json +import rclpy +from rclpy.node import Node +from rclpy.timer import Timer +from geometry_msgs.msg import Twist +from std_msgs.msg import UInt32, String + + +class NodeWatchdogNode(Node): + """ROS2 watchdog monitor for critical system nodes.""" + + def __init__(self): + super().__init__("node_watchdog") + + # Parameters + self.declare_parameter("frequency", 20) # Hz + self.declare_parameter("heartbeat_timeout", 1.0) # seconds, general timeout + self.declare_parameter("motor_driver_critical_timeout", 2.0) # seconds + + frequency = self.get_parameter("frequency").value + self.heartbeat_timeout = self.get_parameter("heartbeat_timeout").value + self.motor_driver_critical_timeout = self.get_parameter( + "motor_driver_critical_timeout" + ).value + + # Heartbeat tracking + self.critical_nodes = { + "balance": None, + "motor_driver": None, + "emergency": None, + "docking": None, + } + self.last_heartbeat_time = { + "balance": None, + "motor_driver": None, + "emergency": None, + "docking": None, + } + self.last_cmd_vel = None + self.motor_driver_down = False + + # Subscriptions for heartbeats + self.create_subscription( + UInt32, "/saltybot/balance_heartbeat", self._on_balance_heartbeat, 10 + ) + self.create_subscription( + UInt32, + "/saltybot/motor_driver_heartbeat", + self._on_motor_driver_heartbeat, + 10, + ) + self.create_subscription( + UInt32, "/saltybot/emergency_heartbeat", self._on_emergency_heartbeat, 10 + ) + self.create_subscription( + UInt32, "/saltybot/docking_heartbeat", self._on_docking_heartbeat, 10 + ) + + # cmd_vel subscription and safe republishing + self.create_subscription(Twist, "/cmd_vel", self._on_cmd_vel, 10) + + # Publications + self.pub_watchdog = self.create_publisher(String, "/saltybot/node_watchdog", 10) + self.pub_cmd_vel_safe = self.create_publisher( + Twist, "/saltybot/cmd_vel_safe", 10 + ) + + # Timer for periodic monitoring at 20Hz + period = 1.0 / frequency + self.timer: Timer = self.create_timer(period, self._timer_callback) + + self.get_logger().info( + f"Node watchdog initialized at {frequency}Hz. " + f"Heartbeat timeout: {self.heartbeat_timeout}s, " + f"Motor driver critical: {self.motor_driver_critical_timeout}s" + ) + + def _on_balance_heartbeat(self, msg: UInt32) -> None: + """Update balance node heartbeat timestamp.""" + self.last_heartbeat_time["balance"] = self.get_clock().now() + + def _on_motor_driver_heartbeat(self, msg: UInt32) -> None: + """Update motor driver heartbeat timestamp.""" + self.last_heartbeat_time["motor_driver"] = self.get_clock().now() + self.motor_driver_down = False + + def _on_emergency_heartbeat(self, msg: UInt32) -> None: + """Update emergency system heartbeat timestamp.""" + self.last_heartbeat_time["emergency"] = self.get_clock().now() + + def _on_docking_heartbeat(self, msg: UInt32) -> None: + """Update docking node heartbeat timestamp.""" + self.last_heartbeat_time["docking"] = self.get_clock().now() + + def _on_cmd_vel(self, msg: Twist) -> None: + """Cache the last received cmd_vel.""" + self.last_cmd_vel = msg + + def _check_node_health(self) -> dict: + """Check health of all monitored nodes. + + Returns: + dict: Health status of each node with timeout and elapsed time. + """ + now = self.get_clock().now() + health = {} + + for node_name in self.critical_nodes: + last_time = self.last_heartbeat_time[node_name] + + if last_time is None: + # No heartbeat received yet + health[node_name] = { + "status": "unknown", + "elapsed_s": None, + "timeout_s": self.heartbeat_timeout, + } + else: + # Calculate elapsed time since last heartbeat + elapsed = (now - last_time).nanoseconds / 1e9 + is_timeout = elapsed > self.heartbeat_timeout + + # Special case: motor driver has longer critical timeout + if node_name == "motor_driver": + is_critical = elapsed > self.motor_driver_critical_timeout + else: + is_critical = False + + health[node_name] = { + "status": "down" if is_timeout else "up", + "elapsed_s": elapsed, + "timeout_s": ( + self.motor_driver_critical_timeout + if node_name == "motor_driver" + else self.heartbeat_timeout + ), + "critical": is_critical, + } + + return health + + def _timer_callback(self) -> None: + """Monitor node health and publish alerts at 20Hz.""" + health = self._check_node_health() + + # Detect if motor driver is in critical state (down for >2s) + motor_driver_health = health.get("motor_driver", {}) + if motor_driver_health.get("critical", False): + self.motor_driver_down = True + self.get_logger().warn( + f"MOTOR DRIVER DOWN >2s ({motor_driver_health['elapsed_s']:.1f}s). " + "Applying safety fallback: zeroing cmd_vel." + ) + + # Determine any nodes down for >1s + nodes_with_timeout = { + name: status + for name, status in health.items() + if status.get("status") == "down" + } + + # Publish watchdog status + watchdog_status = { + "timestamp": self.get_clock().now().nanoseconds / 1e9, + "all_healthy": len(nodes_with_timeout) == 0 + and not self.motor_driver_down, + "health": health, + "motor_driver_critical": self.motor_driver_down, + } + watchdog_msg = String(data=json.dumps(watchdog_status)) + self.pub_watchdog.publish(watchdog_msg) + + # Publish cmd_vel with safety checks + if self.last_cmd_vel is not None: + cmd_vel_safe = self._apply_safety_checks(self.last_cmd_vel) + self.pub_cmd_vel_safe.publish(cmd_vel_safe) + + def _apply_safety_checks(self, cmd_vel: Twist) -> Twist: + """Apply safety checks to cmd_vel based on system state. + + Args: + cmd_vel: Original velocity command + + Returns: + Twist: Potentially modified velocity command for safe operation. + """ + safe_cmd = Twist() + + # If motor driver is critically down, zero all velocities + if self.motor_driver_down: + return safe_cmd + + # Otherwise, pass through unchanged + safe_cmd.linear.x = cmd_vel.linear.x + safe_cmd.linear.y = cmd_vel.linear.y + safe_cmd.linear.z = cmd_vel.linear.z + safe_cmd.angular.x = cmd_vel.angular.x + safe_cmd.angular.y = cmd_vel.angular.y + safe_cmd.angular.z = cmd_vel.angular.z + return safe_cmd + + +def main(args=None): + rclpy.init(args=args) + node = NodeWatchdogNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_node_watchdog/setup.cfg b/jetson/ros2_ws/src/saltybot_node_watchdog/setup.cfg new file mode 100644 index 0000000..ae513cc --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_node_watchdog/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_node_watchdog +[egg_info] +tag_date = 0 diff --git a/jetson/ros2_ws/src/saltybot_node_watchdog/setup.py b/jetson/ros2_ws/src/saltybot_node_watchdog/setup.py new file mode 100644 index 0000000..1dbd76c --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_node_watchdog/setup.py @@ -0,0 +1,29 @@ +from setuptools import setup + +package_name = "saltybot_node_watchdog" + +setup( + name=package_name, + version="0.1.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), + (f"share/{package_name}", ["package.xml"]), + (f"share/{package_name}/launch", ["launch/node_watchdog.launch.py"]), + (f"share/{package_name}/config", ["config/watchdog_config.yaml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="sl-controls", + maintainer_email="sl-controls@saltylab.local", + description=( + "Node watchdog: heartbeat monitoring with safety fallback for critical systems" + ), + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "node_watchdog_node = saltybot_node_watchdog.node_watchdog_node:main", + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_node_watchdog/test/__init__.py b/jetson/ros2_ws/src/saltybot_node_watchdog/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_node_watchdog/test/test_node_watchdog.py b/jetson/ros2_ws/src/saltybot_node_watchdog/test/test_node_watchdog.py new file mode 100644 index 0000000..eb929f6 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_node_watchdog/test/test_node_watchdog.py @@ -0,0 +1,329 @@ +"""Unit tests for node_watchdog_node.""" + +import pytest +import json +import time +from geometry_msgs.msg import Twist +from std_msgs.msg import UInt32, String + +import rclpy +from rclpy.time import Time + +# Import the node under test +from saltybot_node_watchdog.node_watchdog_node import NodeWatchdogNode + + +@pytest.fixture +def rclpy_fixture(): + """Initialize and cleanup rclpy.""" + rclpy.init() + yield + rclpy.shutdown() + + +@pytest.fixture +def node(rclpy_fixture): + """Create a watchdog node instance.""" + node = NodeWatchdogNode() + yield node + node.destroy_node() + + +class TestNodeWatchdogNode: + """Test suite for NodeWatchdogNode.""" + + def test_node_initialization(self, node): + """Test that node initializes with correct defaults.""" + assert node.heartbeat_timeout == 1.0 + assert node.motor_driver_critical_timeout == 2.0 + assert node.last_cmd_vel is None + assert node.motor_driver_down is False + + # All heartbeat times should be None initially + for node_name in node.critical_nodes: + assert node.last_heartbeat_time[node_name] is None + + def test_balance_heartbeat_received(self, node): + """Test balance node heartbeat recording.""" + msg = UInt32(data=1) + node._on_balance_heartbeat(msg) + assert node.last_heartbeat_time["balance"] is not None + + def test_motor_driver_heartbeat_received(self, node): + """Test motor driver heartbeat recording.""" + msg = UInt32(data=1) + node._on_motor_driver_heartbeat(msg) + assert node.last_heartbeat_time["motor_driver"] is not None + # Motor driver heartbeat should clear the down flag + node.motor_driver_down = True + node._on_motor_driver_heartbeat(msg) + assert node.motor_driver_down is False + + def test_emergency_heartbeat_received(self, node): + """Test emergency system heartbeat recording.""" + msg = UInt32(data=1) + node._on_emergency_heartbeat(msg) + assert node.last_heartbeat_time["emergency"] is not None + + def test_docking_heartbeat_received(self, node): + """Test docking node heartbeat recording.""" + msg = UInt32(data=1) + node._on_docking_heartbeat(msg) + assert node.last_heartbeat_time["docking"] is not None + + def test_cmd_vel_caching(self, node): + """Test that cmd_vel messages are cached.""" + msg = Twist() + msg.linear.x = 1.0 + node._on_cmd_vel(msg) + assert node.last_cmd_vel is not None + assert node.last_cmd_vel.linear.x == 1.0 + + def test_health_check_all_unknown(self, node): + """Test health check when no heartbeats received.""" + health = node._check_node_health() + + assert len(health) == 4 + for node_name in node.critical_nodes: + assert health[node_name]["status"] == "unknown" + assert health[node_name]["elapsed_s"] is None + assert health[node_name]["timeout_s"] == 1.0 + + def test_health_check_just_received(self, node): + """Test health check just after heartbeat received.""" + # Record a heartbeat for balance node + node.last_heartbeat_time["balance"] = node.get_clock().now() + + health = node._check_node_health() + + # Balance should be up, others unknown + assert health["balance"]["status"] == "up" + assert health["balance"]["elapsed_s"] < 0.1 + assert health["emergency"]["status"] == "unknown" + + def test_health_check_timeout_general(self, node): + """Test that heartbeat timeout is detected (>1s).""" + # Simulate a heartbeat that arrived >1s ago + now = node.get_clock().now() + old_time = Time( + nanoseconds=now.nanoseconds - int(1.5 * 1e9) + ) # 1.5 seconds ago + node.last_heartbeat_time["balance"] = old_time + + health = node._check_node_health() + + assert health["balance"]["status"] == "down" + assert health["balance"]["elapsed_s"] > 1.4 + assert health["balance"]["elapsed_s"] < 2.0 + + def test_health_check_motor_driver_critical(self, node): + """Test motor driver critical timeout (>2s).""" + # Simulate motor driver heartbeat >2s ago + now = node.get_clock().now() + old_time = Time(nanoseconds=now.nanoseconds - int(2.5 * 1e9)) # 2.5 seconds + node.last_heartbeat_time["motor_driver"] = old_time + + health = node._check_node_health() + + motor_health = health["motor_driver"] + assert motor_health["status"] == "down" + assert motor_health.get("critical", False) is True + assert motor_health["elapsed_s"] > 2.4 + + def test_safety_check_normal_operation(self, node): + """Test safety check passes through cmd_vel normally.""" + node.motor_driver_down = False + cmd = Twist() + cmd.linear.x = 1.5 + cmd.angular.z = 0.3 + + safe_cmd = node._apply_safety_checks(cmd) + + assert abs(safe_cmd.linear.x - 1.5) < 1e-6 + assert abs(safe_cmd.angular.z - 0.3) < 1e-6 + + def test_safety_check_motor_driver_down(self, node): + """Test safety check zeros cmd_vel when motor driver is down.""" + node.motor_driver_down = True + cmd = Twist() + cmd.linear.x = 1.5 + cmd.linear.y = 0.2 + cmd.angular.z = 0.3 + + safe_cmd = node._apply_safety_checks(cmd) + + # All velocities should be zero + assert safe_cmd.linear.x == 0.0 + assert safe_cmd.linear.y == 0.0 + assert safe_cmd.linear.z == 0.0 + assert safe_cmd.angular.x == 0.0 + assert safe_cmd.angular.y == 0.0 + assert safe_cmd.angular.z == 0.0 + + def test_timer_callback_publishes(self, node): + """Test that timer callback publishes watchdog status.""" + # Record a heartbeat + node.last_heartbeat_time["balance"] = node.get_clock().now() + node.last_cmd_vel = Twist() + node.last_cmd_vel.linear.x = 1.0 + + # Call timer callback + node._timer_callback() + # Just verify it doesn't crash; actual publishing is tested via integration + + def test_watchdog_status_json_all_healthy(self, node): + """Test watchdog status JSON when all nodes healthy.""" + # Record all heartbeats + now = node.get_clock().now() + for node_name in node.critical_nodes: + node.last_heartbeat_time[node_name] = now + + health = node._check_node_health() + watchdog_status = { + "timestamp": now.nanoseconds / 1e9, + "all_healthy": all( + s["status"] == "up" for s in health.values() + ), + "health": health, + "motor_driver_critical": False, + } + + # Verify it's valid JSON + json_str = json.dumps(watchdog_status) + parsed = json.loads(json_str) + + assert parsed["all_healthy"] is True + assert parsed["motor_driver_critical"] is False + + def test_watchdog_status_json_with_timeout(self, node): + """Test watchdog status JSON when node has timed out.""" + # Balance heartbeat >1s ago + now = node.get_clock().now() + old_time = Time(nanoseconds=now.nanoseconds - int(1.5 * 1e9)) + node.last_heartbeat_time["balance"] = old_time + + # Others are current + for name in ["motor_driver", "emergency", "docking"]: + node.last_heartbeat_time[name] = now + + health = node._check_node_health() + watchdog_status = { + "timestamp": now.nanoseconds / 1e9, + "all_healthy": all(s["status"] == "up" for s in health.values()), + "health": health, + "motor_driver_critical": False, + } + + json_str = json.dumps(watchdog_status) + parsed = json.loads(json_str) + + assert parsed["all_healthy"] is False + assert parsed["health"]["balance"]["status"] == "down" + + +class TestNodeWatchdogScenarios: + """Integration-style tests for realistic scenarios.""" + + def test_scenario_all_nodes_healthy(self, node): + """Scenario: all critical nodes sending heartbeats.""" + now = node.get_clock().now() + + # All nodes sending heartbeats + for name in node.critical_nodes: + node.last_heartbeat_time[name] = now + + health = node._check_node_health() + + all_up = all(h["status"] == "up" for h in health.values()) + assert all_up is True + + def test_scenario_motor_driver_loss_below_critical(self, node): + """Scenario: motor driver offline 1.5s (below 2s critical).""" + now = node.get_clock().now() + old_time = Time(nanoseconds=now.nanoseconds - int(1.5 * 1e9)) + + # Motor driver down 1.5s, others healthy + node.last_heartbeat_time["motor_driver"] = old_time + for name in ["balance", "emergency", "docking"]: + node.last_heartbeat_time[name] = now + + health = node._check_node_health() + motor = health["motor_driver"] + + assert motor["status"] == "down" + assert motor.get("critical", False) is False + # Safety fallback should NOT be triggered yet + assert node.motor_driver_down is False + + def test_scenario_motor_driver_critical_loss(self, node): + """Scenario: motor driver offline >2s (triggers critical).""" + now = node.get_clock().now() + old_time = Time(nanoseconds=now.nanoseconds - int(2.5 * 1e9)) + + node.last_heartbeat_time["motor_driver"] = old_time + node.last_heartbeat_time["balance"] = now + node.last_heartbeat_time["emergency"] = now + node.last_heartbeat_time["docking"] = now + + health = node._check_node_health() + motor = health["motor_driver"] + + assert motor["status"] == "down" + assert motor.get("critical", False) is True + + def test_scenario_cascading_node_failures(self, node): + """Scenario: multiple nodes failing in sequence.""" + now = node.get_clock().now() + old1s = Time(nanoseconds=now.nanoseconds - int(1.2 * 1e9)) + old2s = Time(nanoseconds=now.nanoseconds - int(2.5 * 1e9)) + + # Balance down 1.2s, motor driver down 2.5s, others healthy + node.last_heartbeat_time["balance"] = old1s + node.last_heartbeat_time["motor_driver"] = old2s + node.last_heartbeat_time["emergency"] = now + node.last_heartbeat_time["docking"] = now + + health = node._check_node_health() + + assert health["balance"]["status"] == "down" + assert health["balance"].get("critical", False) is False + assert health["motor_driver"]["status"] == "down" + assert health["motor_driver"].get("critical", False) is True + + def test_scenario_cmd_vel_safety_fallback(self, node): + """Scenario: motor driver down triggers safety zeroing of cmd_vel.""" + # Motor driver is critically down + node.motor_driver_down = True + + cmd = Twist() + cmd.linear.x = 2.0 + cmd.angular.z = 0.5 + + safe_cmd = node._apply_safety_checks(cmd) + + # All should be zeroed + assert safe_cmd.linear.x == 0.0 + assert safe_cmd.linear.y == 0.0 + assert safe_cmd.linear.z == 0.0 + assert safe_cmd.angular.x == 0.0 + assert safe_cmd.angular.y == 0.0 + assert safe_cmd.angular.z == 0.0 + + def test_scenario_motor_driver_recovery(self, node): + """Scenario: motor driver comes back online after being down.""" + now = node.get_clock().now() + + # Motor driver was down + node.motor_driver_down = True + + # Motor driver sends heartbeat + node._on_motor_driver_heartbeat(UInt32(data=1)) + + # Should clear the down flag + assert node.motor_driver_down is False + + # cmd_vel should pass through + cmd = Twist() + cmd.linear.x = 1.0 + safe_cmd = node._apply_safety_checks(cmd) + assert safe_cmd.linear.x == 1.0