feat: Add Issue #195 - GoPro mount adapter for T-slot sensor rail

Add gopro_mount.scad with:
- Standard GoPro 3-prong interface on top for HERO 5+ compatibility
- T-slot 2020 clamp with M3 thumbscrew retention (no tools required)
- Tilt mechanism: 0-90° with 15° detent angles (7 positions)
- Integrated T-nut slides into rail groove
- Rotation axis via M5 hinge pin with index-hole angle locking
- Design supports flat-face-down printing, no supports needed

Includes comprehensive BOM with:
- Part specifications for base clamp and camera bracket
- Fastener list and torque specs
- Step-by-step assembly instructions
- Tilt angle reference guide
- Post-print finishing notes
- Cable management guidance

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>

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

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();
+}

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

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

include/config.h

@@ -43,9 +43,30 @@
 #define ADC_CURR_PIN        GPIO_PIN_3    // ADC_CURR 1
 #define ADC_IBAT_SCALE      115           // ibata_scale
 
-// --- LED Strip (WS2812) ---
+// --- LED Strip (WS2812 NeoPixel, Issue #193) ---
+// TIM3_CH1 PWM on PB4 for 8-LED ring status indicator
+#define LED_STRIP_TIM       TIM3
+#define LED_STRIP_CHANNEL   TIM_CHANNEL_1
 #define LED_STRIP_PORT      GPIOB
-#define LED_STRIP_PIN       GPIO_PIN_3    // LED_STRIP 1 (TIM2_CH2)
+#define LED_STRIP_PIN       GPIO_PIN_4    // LED_STRIP 1 (TIM3_CH1)
+#define LED_STRIP_AF        GPIO_AF2_TIM3  // Alternate function
+#define LED_STRIP_NUM_LEDS  8u            // 8-LED ring
+#define LED_STRIP_FREQ_HZ   800000u       // 800 kHz PWM for NeoPixel (1.25 µs per bit)
+
+// --- Servo Pan-Tilt (Issue #206) ---
+// TIM4_CH1 (PB6) for pan servo, TIM4_CH2 (PB7) for tilt servo
+#define SERVO_TIM           TIM4
+#define SERVO_PAN_PORT      GPIOB
+#define SERVO_PAN_PIN       GPIO_PIN_6    // TIM4_CH1
+#define SERVO_PAN_CHANNEL   TIM_CHANNEL_1
+#define SERVO_TILT_PORT     GPIOB
+#define SERVO_TILT_PIN      GPIO_PIN_7    // TIM4_CH2
+#define SERVO_TILT_CHANNEL  TIM_CHANNEL_2
+#define SERVO_AF            GPIO_AF2_TIM4  // Alternate function
+#define SERVO_FREQ_HZ       50u            // 50 Hz (20ms period, standard servo)
+#define SERVO_MIN_US        500u           // 500µs = 0°
+#define SERVO_MAX_US        2500u          // 2500µs = 180°
+#define SERVO_CENTER_US     1500u          // 1500µs = 90°
 
 // --- OSD: MAX7456 (SPI2) ---
 #define OSD_SPI             SPI2

include/led.h

@@ -0,0 +1,101 @@
+#ifndef LED_H
+#define LED_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * led.h — WS2812B NeoPixel status indicator driver (Issue #193)
+ *
+ * Hardware: TIM3_CH1 PWM on PB4 at 800 kHz (1.25 µs per bit).
+ * Controls an 8-LED ring with state-based animations:
+ *   - Boot: Blue chase (startup sequence)
+ *   - Armed: Solid green
+ *   - Error: Red blinking (visual alert)
+ *   - Low Battery: Yellow pulsing (warning)
+ *   - Charging: Green breathing (soft indication)
+ *   - E-Stop: Red strobe (immediate action required)
+ *
+ * State transitions are non-blocking via a 1 ms timer callback (led_tick).
+ * Each state defines its own animation envelope: color, timing, and brightness.
+ *
+ * WS2812 protocol (NRZ):
+ *   - Bit "0": High 350 ns, Low 800 ns (1.25 µs total)
+ *   - Bit "1": High 700 ns, Low 600 ns (1.25 µs total)
+ *   - Reset: Low > 50 µs
+ *
+ * PWM-based implementation via DMA:
+ *   - 10 levels: [350 ns, 400, 450, 500, 550, 600, 650, 700, 750, 800]
+ *   - Bit "0" → High 350-400 ns  Bit "1" → High 650-800 ns
+ *   - Each bit requires one PWM cycle; 24 bits/LED × 8 LEDs = 192 cycles
+ *   - DMA rings through buffer, auto-reloads on update events
+ */
+
+/* LED state enumeration */
+typedef enum {
+    LED_STATE_BOOT       = 0,  /* Blue chase (startup) */
+    LED_STATE_ARMED      = 1,  /* Solid green */
+    LED_STATE_ERROR      = 2,  /* Red blinking */
+    LED_STATE_LOW_BATT   = 3,  /* Yellow pulsing */
+    LED_STATE_CHARGING   = 4,  /* Green breathing */
+    LED_STATE_ESTOP      = 5,  /* Red strobe */
+    LED_STATE_COUNT
+} LEDState;
+
+/* RGB color (8-bit per channel) */
+typedef struct {
+    uint8_t r;
+    uint8_t g;
+    uint8_t b;
+} RGBColor;
+
+/*
+ * led_init()
+ *
+ * Configure TIM3_CH1 PWM on PB4 at 800 kHz, set up DMA for bit streaming,
+ * and initialize the LED buffer. Call once at startup, after buzzer_init()
+ * but before the main loop.
+ */
+void led_init(void);
+
+/*
+ * led_set_state(state)
+ *
+ * Change the LED display state. The animation runs non-blocking via led_tick().
+ * Valid states: LED_STATE_BOOT, LED_STATE_ARMED, LED_STATE_ERROR, etc.
+ */
+void led_set_state(LEDState state);
+
+/*
+ * led_get_state()
+ *
+ * Return the current LED state.
+ */
+LEDState led_get_state(void);
+
+/*
+ * led_set_color(r, g, b)
+ *
+ * Manually set the LED ring to a solid color. Overrides the current state
+ * animation until led_set_state() is called again.
+ */
+void led_set_color(uint8_t r, uint8_t g, uint8_t b);
+
+/*
+ * led_tick(now_ms)
+ *
+ * Advance animation state machine. Must be called every 1 ms from the main loop.
+ * Handles state-specific animations: chase timing, pulse envelope, strobe phase, etc.
+ * Updates the DMA buffer with new LED values without blocking.
+ */
+void led_tick(uint32_t now_ms);
+
+/*
+ * led_is_animating()
+ *
+ * Returns true if the current state is actively animating (e.g., chase, pulse, strobe).
+ * Returns false for static states (armed, error solid).
+ */
+bool led_is_animating(void);
+
+#endif /* LED_H */

include/servo.h

@@ -0,0 +1,110 @@
+#ifndef SERVO_H
+#define SERVO_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * servo.h — Pan-tilt servo driver for camera head (Issue #206)
+ *
+ * Hardware: TIM4 PWM at 50 Hz (20 ms period)
+ *   - CH1 (PB6): Pan servo (0-180°)
+ *   - CH2 (PB7): Tilt servo (0-180°)
+ *
+ * Servo pulse mapping:
+ *   - 500 µs  → 0°   (full left/down)
+ *   - 1500 µs → 90°  (center)
+ *   - 2500 µs → 180° (full right/up)
+ *
+ * Smooth sweeping via servo_sweep() for camera motion.
+ */
+
+/* Servo channels */
+typedef enum {
+    SERVO_PAN  = 0,   /* CH1 (PB6) */
+    SERVO_TILT = 1,   /* CH2 (PB7) */
+    SERVO_COUNT
+} ServoChannel;
+
+/* Servo state */
+typedef struct {
+    uint16_t current_angle_deg[SERVO_COUNT];   /* Current angle in degrees (0-180) */
+    uint16_t target_angle_deg[SERVO_COUNT];    /* Target angle in degrees */
+    uint16_t pulse_us[SERVO_COUNT];            /* Pulse width in microseconds (500-2500) */
+    uint32_t sweep_start_ms;
+    uint32_t sweep_duration_ms;
+    bool     is_sweeping;
+} ServoState;
+
+/*
+ * servo_init()
+ *
+ * Initialize TIM4 PWM on PB6 (CH1, pan) and PB7 (CH2, tilt) at 50 Hz.
+ * Centers both servos at 90° (1500 µs). Call once at startup.
+ */
+void servo_init(void);
+
+/*
+ * servo_set_angle(channel, degrees)
+ *
+ * Set target angle for a servo (0-180°).
+ * Immediately updates PWM without motion ramping.
+ * Valid channels: SERVO_PAN, SERVO_TILT
+ *
+ * Examples:
+ *   servo_set_angle(SERVO_PAN, 0);      // Pan left
+ *   servo_set_angle(SERVO_PAN, 90);     // Pan center
+ *   servo_set_angle(SERVO_TILT, 180);   // Tilt up
+ */
+void servo_set_angle(ServoChannel channel, uint16_t degrees);
+
+/*
+ * servo_get_angle(channel)
+ *
+ * Return current servo angle in degrees (0-180).
+ */
+uint16_t servo_get_angle(ServoChannel channel);
+
+/*
+ * servo_set_pulse_us(channel, pulse_us)
+ *
+ * Set servo pulse width directly in microseconds (500-2500).
+ * Used for fine-tuning or direct control.
+ */
+void servo_set_pulse_us(ServoChannel channel, uint16_t pulse_us);
+
+/*
+ * servo_sweep(channel, start_deg, end_deg, duration_ms)
+ *
+ * Smooth linear sweep from start to end angle over duration_ms.
+ * Non-blocking: must call servo_tick() every ~10 ms to update PWM.
+ *
+ * Examples:
+ *   servo_sweep(SERVO_PAN, 0, 180, 2000);    // Pan left-to-right in 2 seconds
+ *   servo_sweep(SERVO_TILT, 45, 135, 1000);  // Tilt up-down in 1 second
+ */
+void servo_sweep(ServoChannel channel, uint16_t start_deg, uint16_t end_deg, uint32_t duration_ms);
+
+/*
+ * servo_tick(now_ms)
+ *
+ * Update servo sweep animation (if active). Call every ~10 ms from main loop.
+ * No-op if not currently sweeping.
+ */
+void servo_tick(uint32_t now_ms);
+
+/*
+ * servo_is_sweeping()
+ *
+ * Returns true if any servo is currently sweeping.
+ */
+bool servo_is_sweeping(void);
+
+/*
+ * servo_stop_sweep(channel)
+ *
+ * Stop sweep immediately, hold current position.
+ */
+void servo_stop_sweep(ServoChannel channel);
+
+#endif /* SERVO_H */

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

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")],
+            ),
+        ]
+    )

jetson/ros2_ws/src/saltybot_node_watchdog/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_node_watchdog</name>
+  <version>0.1.0</version>
+  <description>
+    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.
+  </description>
+  <maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

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

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

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",
+        ],
+    },
+)

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

jetson/ros2_ws/src/saltybot_person_reid/config/person_reid_params.yaml

@@ -0,0 +1,6 @@
+person_reid:
+  ros__parameters:
+    model_path: ''            # path to MobileNetV2+projection ONNX file (empty = histogram fallback)
+    match_threshold: 0.75     # cosine similarity threshold for re-ID match
+    max_identity_age_s: 300.0 # seconds before unseen identity is pruned
+    publish_hz: 5.0           # publication rate (Hz)

jetson/ros2_ws/src/saltybot_person_reid/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_person_reid</name>
+  <version>0.1.0</version>
+  <description>
+    Person re-identification node — cross-camera appearance matching using
+    MobileNetV2 ONNX embeddings (128-dim, cosine similarity gallery).
+  </description>
+  <maintainer email="robot@saltylab.local">SaltyLab</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>vision_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>message_filters</depend>
+  <depend>saltybot_person_reid_msgs</depend>
+
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-opencv</exec_depend>
+
+  <test_depend>pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/_embedding_model.py

@@ -0,0 +1,95 @@
+"""
+_embedding_model.py — Appearance embedding extractor (no ROS2 deps).
+
+Primary:  MobileNetV2 ONNX torso crop → 128-dim L2-normalised embedding.
+Fallback: 128-bin HSV histogram (H:16 × S:8) when no model file is available.
+"""
+
+from __future__ import annotations
+
+import numpy as np
+import cv2
+
+# Top fraction of the bounding box height used as torso crop
+_INPUT_SIZE = (128, 256)   # (W, H) fed to MobileNetV2
+
+
+class EmbeddingModel:
+    """
+    Extract a 128-dim L2-normalised appearance embedding from a BGR crop.
+
+    Parameters
+    ----------
+    model_path : str or None
+        Path to a MobileNetV2+projection ONNX file.  When None (or file
+        not found), falls back to a 128-bin HSV colour histogram.
+    """
+
+    def __init__(self, model_path: str | None = None):
+        self._net = None
+        if model_path:
+            try:
+                self._net = cv2.dnn.readNetFromONNX(model_path)
+            except Exception:
+                pass  # histogram fallback
+
+    def embed(self, bgr_crop: np.ndarray) -> np.ndarray:
+        """
+        Parameters
+        ----------
+        bgr_crop : np.ndarray  shape (H, W, 3) uint8
+
+        Returns
+        -------
+        np.ndarray  shape (128,) float32, L2-normalised
+        """
+        if bgr_crop.size == 0:
+            return np.zeros(128, dtype=np.float32)
+
+        if self._net is not None:
+            return self._mobilenet_embed(bgr_crop)
+        return self._histogram_embed(bgr_crop)
+
+    # ── MobileNetV2 path ──────────────────────────────────────────────────────
+
+    def _mobilenet_embed(self, bgr: np.ndarray) -> np.ndarray:
+        resized = cv2.resize(bgr, _INPUT_SIZE)
+        blob = cv2.dnn.blobFromImage(
+            resized,
+            scalefactor=1.0 / 255.0,
+            size=_INPUT_SIZE,
+            mean=(0.485 * 255, 0.456 * 255, 0.406 * 255),
+            swapRB=True,
+            crop=False,
+        )
+        # Std normalisation: divide channel-wise
+        blob[:, 0] /= 0.229
+        blob[:, 1] /= 0.224
+        blob[:, 2] /= 0.225
+
+        self._net.setInput(blob)
+        feat = self._net.forward().flatten().astype(np.float32)
+
+        # Ensure 128-dim — average-pool if model output differs
+        if feat.shape[0] != 128:
+            n = feat.shape[0]
+            block = max(1, n // 128)
+            feat = feat[: block * 128].reshape(128, block).mean(axis=1)
+
+        return _l2_norm(feat)
+
+    # ── HSV histogram fallback ────────────────────────────────────────────────
+
+    def _histogram_embed(self, bgr: np.ndarray) -> np.ndarray:
+        """128-bin HSV histogram: 16 H-bins × 8 S-bins, concatenated."""
+        hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)
+        hist = cv2.calcHist(
+            [hsv], [0, 1], None,
+            [16, 8], [0, 180, 0, 256],
+        ).flatten().astype(np.float32)
+        return _l2_norm(hist)
+
+
+def _l2_norm(v: np.ndarray) -> np.ndarray:
+    n = float(np.linalg.norm(v))
+    return v / n if n > 1e-6 else v

jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/_reid_gallery.py

@@ -0,0 +1,105 @@
+"""
+_reid_gallery.py — Appearance gallery for person re-identification (no ROS2 deps).
+
+Matches an incoming embedding against stored identities using cosine similarity.
+New identities are created when the best match falls below the threshold.
+"""
+
+from __future__ import annotations
+
+import time
+from dataclasses import dataclass, field
+from typing import List, Tuple
+
+import numpy as np
+
+
+@dataclass
+class Identity:
+    identity_id: int
+    embedding: np.ndarray          # shape (D,) L2-normalised
+    last_seen: float = field(default_factory=time.monotonic)
+    hit_count: int = 1
+
+    def update(self, new_embedding: np.ndarray, alpha: float = 0.1) -> None:
+        """EMA update of the stored embedding, re-normalised after blending."""
+        merged = (1.0 - alpha) * self.embedding + alpha * new_embedding
+        n = float(np.linalg.norm(merged))
+        self.embedding = merged / n if n > 1e-6 else merged
+        self.last_seen = time.monotonic()
+        self.hit_count += 1
+
+
+class ReidGallery:
+    """
+    Lightweight cosine-similarity re-ID gallery.
+
+    Parameters
+    ----------
+    match_threshold : float
+        Cosine similarity (dot product of unit vectors) required to accept a
+        match.  Range [0, 1]; 0 = always new identity, 1 = perfect match only.
+    max_age_s : float
+        Identities not seen for this many seconds are pruned.
+    """
+
+    def __init__(
+        self,
+        match_threshold: float = 0.75,
+        max_age_s: float = 300.0,
+    ):
+        self._threshold = match_threshold
+        self._max_age_s = max_age_s
+        self._identities: List[Identity] = []
+        self._next_id = 1
+
+    def match(self, embedding: np.ndarray) -> Tuple[int, float, bool]:
+        """
+        Match embedding against the gallery.
+
+        Returns
+        -------
+        (identity_id, match_score, is_new)
+            identity_id : assigned ID (new or existing)
+            match_score : cosine similarity to best match (0.0 if new)
+            is_new      : True if a new identity was created
+        """
+        self._prune()
+
+        if not self._identities:
+            return self._add_identity(embedding)
+
+        scores = np.array(
+            [float(np.dot(embedding, ident.embedding)) for ident in self._identities]
+        )
+        best_idx = int(np.argmax(scores))
+        best_score = float(scores[best_idx])
+
+        if best_score >= self._threshold:
+            ident = self._identities[best_idx]
+            ident.update(embedding)
+            return ident.identity_id, best_score, False
+
+        return self._add_identity(embedding)
+
+    # ── Internal helpers ──────────────────────────────────────────────────────
+
+    def _add_identity(self, embedding: np.ndarray) -> Tuple[int, float, bool]:
+        new_id = self._next_id
+        self._next_id += 1
+        self._identities.append(
+            Identity(identity_id=new_id, embedding=embedding.copy())
+        )
+        return new_id, 0.0, True
+
+    def _prune(self) -> None:
+        now = time.monotonic()
+        self._identities = [
+            ident
+            for ident in self._identities
+            if now - ident.last_seen < self._max_age_s
+        ]
+
+    @property
+    def size(self) -> int:
+        return len(self._identities)

jetson/ros2_ws/src/saltybot_person_reid/saltybot_person_reid/person_reid_node.py

@@ -0,0 +1,174 @@
+"""
+person_reid_node.py — Person re-identification for cross-camera tracking.
+
+Subscribes to:
+  /person/detections      vision_msgs/Detection2DArray  (person bounding boxes)
+  /camera/color/image_raw sensor_msgs/Image             (colour frame for crops)
+
+Publishes:
+  /saltybot/person_reid   saltybot_person_reid_msgs/PersonAppearanceArray  (5 Hz)
+
+For each detected person the node:
+  1. Crops the torso region (top 65 % of the bounding box height).
+  2. Extracts a 128-dim L2-normalised embedding via MobileNetV2 ONNX (if the
+     model file is provided) or a 128-bin HSV colour histogram (fallback).
+  3. Matches against a cosine-similarity gallery.
+  4. Assigns a persistent identity_id (new or existing).
+
+Parameters:
+  model_path          str    ''      Path to MobileNetV2+projection ONNX file
+  match_threshold     float  0.75    Cosine similarity threshold for matching
+  max_identity_age_s  float  300.0   Seconds before an unseen identity is pruned
+  publish_hz          float  5.0     Publication rate (Hz)
+"""
+
+from __future__ import annotations
+
+from typing import List
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+import message_filters
+import cv2
+import numpy as np
+from cv_bridge import CvBridge
+
+from sensor_msgs.msg import Image
+from vision_msgs.msg import Detection2DArray
+
+from saltybot_person_reid_msgs.msg import PersonAppearance, PersonAppearanceArray
+
+from ._embedding_model import EmbeddingModel
+from ._reid_gallery import ReidGallery
+
+# Fraction of bbox height kept as torso crop (top portion)
+_TORSO_FRAC = 0.65
+
+_BEST_EFFORT_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=4,
+)
+
+
+class PersonReidNode(Node):
+
+    def __init__(self):
+        super().__init__('person_reid')
+
+        self.declare_parameter('model_path',         '')
+        self.declare_parameter('match_threshold',    0.75)
+        self.declare_parameter('max_identity_age_s', 300.0)
+        self.declare_parameter('publish_hz',         5.0)
+
+        model_path  = self.get_parameter('model_path').value
+        match_thr   = self.get_parameter('match_threshold').value
+        max_age     = self.get_parameter('max_identity_age_s').value
+        publish_hz  = self.get_parameter('publish_hz').value
+
+        self._bridge   = CvBridge()
+        self._embedder = EmbeddingModel(model_path or None)
+        self._gallery  = ReidGallery(match_threshold=match_thr, max_age_s=max_age)
+
+        # Buffer: updated by frame callback, drained by timer
+        self._pending: List[PersonAppearance] = []
+        self._pending_header = None
+
+        # Synchronized subscribers
+        det_sub = message_filters.Subscriber(
+            self, Detection2DArray, '/person/detections',
+            qos_profile=_BEST_EFFORT_QOS)
+        img_sub = message_filters.Subscriber(
+            self, Image, '/camera/color/image_raw',
+            qos_profile=_BEST_EFFORT_QOS)
+        self._sync = message_filters.ApproximateTimeSynchronizer(
+            [det_sub, img_sub], queue_size=4, slop=0.1)
+        self._sync.registerCallback(self._on_frame)
+
+        self._pub = self.create_publisher(
+            PersonAppearanceArray, '/saltybot/person_reid', 10)
+
+        self.create_timer(1.0 / publish_hz, self._tick)
+
+        backend = 'ONNX' if self._embedder._net else 'histogram'
+        self.get_logger().info(
+            f'person_reid ready — backend={backend} '
+            f'threshold={match_thr} max_age={max_age}s'
+        )
+
+    # ── Frame callback ─────────────────────────────────────────────────────────
+
+    def _on_frame(self, det_msg: Detection2DArray, img_msg: Image) -> None:
+        if not det_msg.detections:
+            self._pending = []
+            self._pending_header = det_msg.header
+            return
+
+        try:
+            bgr = self._bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8')
+        except Exception as exc:
+            self.get_logger().error(
+                f'imgmsg_to_cv2 failed: {exc}', throttle_duration_sec=5.0)
+            return
+
+        h_img, w_img = bgr.shape[:2]
+        appearances: List[PersonAppearance] = []
+
+        for det in det_msg.detections:
+            cx  = det.bbox.center.position.x
+            cy  = det.bbox.center.position.y
+            bw  = det.bbox.size_x
+            bh  = det.bbox.size_y
+            conf = det.results[0].hypothesis.score if det.results else 0.0
+
+            # Torso crop: top TORSO_FRAC of bounding box
+            x1 = max(0,      int(cx - bw / 2.0))
+            y1 = max(0,      int(cy - bh / 2.0))
+            x2 = min(w_img,  int(cx + bw / 2.0))
+            y2 = min(h_img,  int(cy - bh / 2.0 + bh * _TORSO_FRAC))
+
+            if x2 - x1 < 8 or y2 - y1 < 8:
+                continue
+
+            crop = bgr[y1:y2, x1:x2]
+            emb  = self._embedder.embed(crop)
+            identity_id, match_score, is_new = self._gallery.match(emb)
+
+            app = PersonAppearance()
+            app.header          = det_msg.header
+            app.track_id        = identity_id
+            app.embedding       = emb.tolist()
+            app.bbox            = det.bbox
+            app.confidence      = float(conf)
+            app.match_score     = float(match_score)
+            app.is_new_identity = is_new
+            appearances.append(app)
+
+        self._pending = appearances
+        self._pending_header = det_msg.header
+
+    # ── 5 Hz publish tick ─────────────────────────────────────────────────────
+
+    def _tick(self) -> None:
+        if self._pending_header is None:
+            return
+        msg = PersonAppearanceArray()
+        msg.header      = self._pending_header
+        msg.appearances = self._pending
+        self._pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = PersonReidNode()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_person_reid/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/saltybot_person_reid
+[install]
+install_scripts=$base/lib/saltybot_person_reid

jetson/ros2_ws/src/saltybot_person_reid/setup.py

@@ -0,0 +1,29 @@
+from setuptools import setup, find_packages
+from glob import glob
+
+package_name = 'saltybot_person_reid'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/config',
+            glob('config/*.yaml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='SaltyLab',
+    maintainer_email='robot@saltylab.local',
+    description='Person re-identification — cross-camera appearance matching',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'person_reid = saltybot_person_reid.person_reid_node:main',
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_person_reid/test/test_person_reid.py

@@ -0,0 +1,163 @@
+"""
+test_person_reid.py — Unit tests for person re-ID helpers (no ROS2 required).
+
+Covers:
+  - _l2_norm helper
+  - EmbeddingModel (histogram fallback — no model file needed)
+  - ReidGallery cosine-similarity matching
+"""
+
+import sys
+import os
+import time
+
+import numpy as np
+import pytest
+
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
+
+from saltybot_person_reid._embedding_model import EmbeddingModel, _l2_norm
+from saltybot_person_reid._reid_gallery import ReidGallery
+
+
+# ── _l2_norm ──────────────────────────────────────────────────────────────────
+
+class TestL2Norm:
+
+    def test_unit_vector_unchanged(self):
+        v = np.array([1.0, 0.0, 0.0], dtype=np.float32)
+        assert np.allclose(_l2_norm(v), v)
+
+    def test_normalised_to_unit_norm(self):
+        v = np.array([3.0, 4.0], dtype=np.float32)
+        assert abs(np.linalg.norm(_l2_norm(v)) - 1.0) < 1e-6
+
+    def test_zero_vector_does_not_crash(self):
+        v = np.zeros(4, dtype=np.float32)
+        result = _l2_norm(v)
+        assert result.shape == (4,)
+
+
+# ── EmbeddingModel ────────────────────────────────────────────────────────────
+
+class TestEmbeddingModel:
+
+    def test_histogram_fallback_shape(self):
+        m = EmbeddingModel(model_path=None)
+        bgr = np.random.randint(0, 255, (100, 50, 3), dtype=np.uint8)
+        emb = m.embed(bgr)
+        assert emb.shape == (128,)
+
+    def test_embedding_is_unit_norm(self):
+        m = EmbeddingModel(model_path=None)
+        bgr = np.random.randint(0, 255, (80, 40, 3), dtype=np.uint8)
+        emb = m.embed(bgr)
+        assert abs(np.linalg.norm(emb) - 1.0) < 1e-5
+
+    def test_empty_crop_returns_zero_vector(self):
+        m = EmbeddingModel(model_path=None)
+        emb = m.embed(np.zeros((0, 0, 3), dtype=np.uint8))
+        assert emb.shape == (128,)
+        assert np.all(emb == 0.0)
+
+    def test_red_and_blue_crops_differ(self):
+        m = EmbeddingModel(model_path=None)
+        red  = np.full((80, 40, 3), (0,   0, 200), dtype=np.uint8)
+        blue = np.full((80, 40, 3), (200, 0,   0), dtype=np.uint8)
+        sim = float(np.dot(m.embed(red), m.embed(blue)))
+        assert sim < 0.99
+
+    def test_same_crop_deterministic(self):
+        m = EmbeddingModel(model_path=None)
+        bgr = np.random.randint(0, 255, (80, 40, 3), dtype=np.uint8)
+        assert np.allclose(m.embed(bgr), m.embed(bgr))
+
+    def test_embedding_float32(self):
+        m = EmbeddingModel(model_path=None)
+        bgr = np.random.randint(0, 255, (60, 30, 3), dtype=np.uint8)
+        emb = m.embed(bgr)
+        assert emb.dtype == np.float32
+
+
+# ── ReidGallery ───────────────────────────────────────────────────────────────
+
+def _unit(dim: int = 128, seed: int | None = None) -> np.ndarray:
+    rng = np.random.default_rng(seed)
+    v = rng.standard_normal(dim).astype(np.float32)
+    return v / np.linalg.norm(v)
+
+
+class TestReidGallery:
+
+    def test_first_match_creates_identity(self):
+        g = ReidGallery(match_threshold=0.75)
+        uid, score, is_new = g.match(_unit(seed=0))
+        assert uid == 1
+        assert is_new is True
+        assert score == pytest.approx(0.0)
+
+    def test_identical_embedding_matches(self):
+        g = ReidGallery(match_threshold=0.75)
+        emb = _unit(seed=1)
+        g.match(emb)
+        uid2, score2, is_new2 = g.match(emb)
+        assert uid2 == 1
+        assert is_new2 is False
+        assert score2 > 0.99
+
+    def test_orthogonal_embeddings_create_new_id(self):
+        g = ReidGallery(match_threshold=0.75)
+        e1 = np.zeros(128, dtype=np.float32); e1[0]  = 1.0
+        e2 = np.zeros(128, dtype=np.float32); e2[64] = 1.0
+        uid1, _, new1 = g.match(e1)
+        uid2, _, new2 = g.match(e2)
+        assert uid1 != uid2
+        assert new2 is True
+
+    def test_ids_are_monotonically_increasing(self):
+        # threshold > 1.0 is unreachable → every embedding creates a new identity
+        g = ReidGallery(match_threshold=2.0)
+        ids = [g.match(_unit(seed=i))[0] for i in range(5)]
+        assert ids == list(range(1, 6))
+
+    def test_gallery_size_increments_for_new_ids(self):
+        g = ReidGallery(match_threshold=2.0)
+        for i in range(4):
+            g.match(_unit(seed=i))
+        assert g.size == 4
+
+    def test_prune_removes_stale_identities(self):
+        g = ReidGallery(match_threshold=0.75, max_age_s=0.01)
+        g.match(_unit(seed=0))
+        time.sleep(0.05)
+        g._prune()
+        assert g.size == 0
+
+    def test_empty_gallery_prune_is_safe(self):
+        g = ReidGallery()
+        g._prune()
+        assert g.size == 0
+
+    def test_match_below_threshold_increments_id(self):
+        g = ReidGallery(match_threshold=0.99)
+        # Two random unit vectors are almost certainly < 0.99 similar
+        e1, e2 = _unit(seed=10), _unit(seed=20)
+        uid1, _, _ = g.match(e1)
+        uid2, _, _ = g.match(e2)
+        # uid2 may or may not equal uid1 depending on random similarity,
+        # but both must be valid positive integers
+        assert uid1 >= 1
+        assert uid2 >= 1
+
+    def test_identity_update_does_not_change_id(self):
+        g = ReidGallery(match_threshold=0.5)
+        emb = _unit(seed=5)
+        uid_first, _, _ = g.match(emb)
+        for _ in range(10):
+            g.match(emb)
+        uid_last, _, _ = g.match(emb)
+        assert uid_last == uid_first
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])

jetson/ros2_ws/src/saltybot_person_reid_msgs/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.8)
+project(saltybot_person_reid_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(vision_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/PersonAppearance.msg"
+  "msg/PersonAppearanceArray.msg"
+  DEPENDENCIES std_msgs vision_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

jetson/ros2_ws/src/saltybot_person_reid_msgs/msg/PersonAppearance.msg

@@ -0,0 +1,7 @@
+std_msgs/Header header
+uint32 track_id
+float32[] embedding
+vision_msgs/BoundingBox2D bbox
+float32 confidence
+float32 match_score
+bool is_new_identity

jetson/ros2_ws/src/saltybot_person_reid_msgs/msg/PersonAppearanceArray.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+PersonAppearance[] appearances

jetson/ros2_ws/src/saltybot_person_reid_msgs/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_person_reid_msgs</name>
+  <version>0.1.0</version>
+  <description>Message types for person re-identification.</description>
+  <maintainer email="robot@saltylab.local">SaltyLab</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <depend>std_msgs</depend>
+  <depend>vision_msgs</depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_social/config/mesh_params.yaml

@@ -0,0 +1,8 @@
+mesh_comms_node:
+  ros__parameters:
+    robot_id: "saltybot_1"           # Set to $ROBOT_ID; override per-robot
+    namespace: ""                    # "" = default namespace; e.g. "/saltybot_1"
+    announce_rate_hz: 1.0            # Heartbeat rate (Hz) — keep <= 2 Hz on mesh
+    peer_timeout_s: 5.0              # Drop peer after this many seconds without announce
+    handoff_distance_m: 3.0          # (guard) min distance for handoff trigger (m)
+    greeted_memory_s: 300.0          # Forget a greeted name after N seconds

jetson/ros2_ws/src/saltybot_social/launch/mesh.launch.py

@@ -0,0 +1,51 @@
+"""mesh.launch.py — Launch the social mesh comms node (Issue #171).
+
+Usage:
+  ros2 launch saltybot_social mesh.launch.py
+  ros2 launch saltybot_social mesh.launch.py robot_id:=saltybot_2
+"""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg = get_package_share_directory("saltybot_social")
+    cfg = os.path.join(pkg, "config", "mesh_params.yaml")
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            "robot_id",
+            default_value=os.environ.get("ROBOT_ID", "saltybot_1"),
+            description="Unique robot identifier for mesh announcements",
+        ),
+        DeclareLaunchArgument(
+            "announce_rate_hz",
+            default_value="1.0",
+            description="Heartbeat rate (Hz)",
+        ),
+        DeclareLaunchArgument(
+            "peer_timeout_s",
+            default_value="5.0",
+            description="Seconds before a peer is considered lost",
+        ),
+
+        Node(
+            package="saltybot_social",
+            executable="mesh_comms_node",
+            name="mesh_comms_node",
+            output="screen",
+            parameters=[
+                cfg,
+                {
+                    "robot_id":          LaunchConfiguration("robot_id"),
+                    "announce_rate_hz":  LaunchConfiguration("announce_rate_hz"),
+                    "peer_timeout_s":    LaunchConfiguration("peer_timeout_s"),
+                },
+            ],
+        ),
+    ])

jetson/ros2_ws/src/saltybot_social/saltybot_social/mesh_comms_node.py

@@ -0,0 +1,237 @@
+"""mesh_comms_node.py — Robot-to-robot social mesh communication.
+Issue #171
+
+Topics:
+  Publish:   /social/mesh/announce   (MeshPeer, 1 Hz)  — presence heartbeat
+             /social/mesh/handoff    (MeshHandoff)     — person context on LEAVING
+             /social/mesh/peers      (String / JSON)   — peer list for UI
+             /social/mesh/greeted    (String / JSON)   — mesh-wide greeted names
+  Subscribe: /social/mesh/announce   (MeshPeer)        — remote peer announcements
+             /social/mesh/handoff    (MeshHandoff)     — incoming handoffs
+             /social/person_states   (PersonStateArray)
+             /social/orchestrator/state (String / JSON)
+
+Parameters:
+  robot_id          (str, $ROBOT_ID or "saltybot_1") — unique robot identifier
+  namespace         (str, $ROS_NAMESPACE or "")      — ROS2 namespace
+  announce_rate_hz  (float, 1.0)   — heartbeat rate
+  peer_timeout_s    (float, 5.0)   — drop peer after N seconds without announce
+  handoff_distance_m (float, 3.0)  — max distance to trigger handoff (unused guard)
+  greeted_memory_s  (float, 300.0) — forget a greeted name after N seconds
+"""
+
+from __future__ import annotations
+
+import json
+import os
+import threading
+import time
+from typing import Dict, Set
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
+from std_msgs.msg import String
+
+from saltybot_social_msgs.msg import PersonStateArray, MeshPeer, MeshHandoff
+
+_LEAVING  = 4  # PersonState.STATE_LEAVING
+_ENGAGED  = 2  # PersonState.STATE_ENGAGED
+_TALKING  = 3  # PersonState.STATE_TALKING
+
+
+class MeshCommsNode(Node):
+    """Robot mesh comms — peer announce, person handoff, greeting deduplication."""
+
+    def __init__(self) -> None:
+        super().__init__("mesh_comms_node")
+
+        self.declare_parameter("robot_id", os.environ.get("ROBOT_ID", "saltybot_1"))
+        self.declare_parameter("namespace", os.environ.get("ROS_NAMESPACE", ""))
+        self.declare_parameter("announce_rate_hz", 1.0)
+        self.declare_parameter("peer_timeout_s", 5.0)
+        self.declare_parameter("handoff_distance_m", 3.0)
+        self.declare_parameter("greeted_memory_s", 300.0)
+
+        self._robot_id     = self.get_parameter("robot_id").value
+        self._ns           = self.get_parameter("namespace").value
+        self._peer_timeout = self.get_parameter("peer_timeout_s").value
+        self._handoff_dist = self.get_parameter("handoff_distance_m").value
+        self._greeted_mem  = self.get_parameter("greeted_memory_s").value
+
+        qos = QoSProfile(depth=10)
+        mesh_qos = QoSProfile(
+            depth=10,
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            durability=DurabilityPolicy.VOLATILE,
+        )
+
+        self._announce_pub = self.create_publisher(MeshPeer,    "/social/mesh/announce", mesh_qos)
+        self._handoff_pub  = self.create_publisher(MeshHandoff, "/social/mesh/handoff",  qos)
+        self._peers_pub    = self.create_publisher(String,      "/social/mesh/peers",    qos)
+        self._greeted_pub  = self.create_publisher(String,      "/social/mesh/greeted",  qos)
+
+        self._announce_sub = self.create_subscription(
+            MeshPeer, "/social/mesh/announce", self._on_announce, mesh_qos)
+        self._handoff_sub  = self.create_subscription(
+            MeshHandoff, "/social/mesh/handoff", self._on_handoff, qos)
+        self._persons_sub  = self.create_subscription(
+            PersonStateArray, "/social/person_states", self._on_person_states, qos)
+        self._orch_sub     = self.create_subscription(
+            String, "/social/orchestrator/state", self._on_orch_state, qos)
+
+        self._lock                         = threading.Lock()
+        self._peers: Dict[str, dict]       = {}   # robot_id → {last_seen, msg}
+        self._person_states: dict          = {}   # person_id → PersonState
+        self._social_state                 = "idle"
+        self._greeted: Dict[str, float]    = {}   # person_name → timestamp
+        self._handoff_sent: Set[str]       = set()  # str(person_id) already handed off
+
+        rate = self.get_parameter("announce_rate_hz").value
+        self._announce_timer = self.create_timer(1.0 / rate, self._publish_announce)
+        self._cleanup_timer  = self.create_timer(2.0, self._cleanup)
+
+        self.get_logger().info(
+            f"MeshCommsNode ready (robot_id={self._robot_id}, "
+            f"peer_timeout={self._peer_timeout}s, handoff_dist={self._handoff_dist}m)"
+        )
+
+    # ── Subscriber callbacks ──────────────────────────────────────────────────
+
+    def _on_orch_state(self, msg: String) -> None:
+        try:
+            d = json.loads(msg.data)
+            with self._lock:
+                self._social_state = d.get("state", "idle")
+        except Exception:
+            pass
+
+    def _on_person_states(self, msg: PersonStateArray) -> None:
+        with self._lock:
+            new_states = {p.person_id: p for p in msg.persons}
+            for pid, ps in new_states.items():
+                prev = self._person_states.get(pid)
+                if (prev is not None
+                        and prev.state != _LEAVING
+                        and ps.state == _LEAVING
+                        and str(pid) not in self._handoff_sent):
+                    self._trigger_handoff(ps)
+            self._person_states = new_states
+
+    def _on_announce(self, msg: MeshPeer) -> None:
+        if msg.robot_id == self._robot_id:
+            return
+        with self._lock:
+            self._peers[msg.robot_id] = {"last_seen": time.time(), "msg": msg}
+            for name in msg.greeted_person_names:
+                if name and name not in self._greeted:
+                    self._greeted[name] = time.time()
+        self._publish_greeted()
+
+    def _on_handoff(self, msg: MeshHandoff) -> None:
+        if msg.from_robot_id == self._robot_id:
+            return
+        self.get_logger().info(
+            f"Handoff received from [{msg.from_robot_id}]: "
+            f"person='{msg.person_name or msg.person_id}' "
+            f"lang={msg.language or '?'} "
+            f"bearing={msg.bearing_to_target_deg:.0f}° dist={msg.distance_m:.1f}m"
+        )
+
+    # ── Publish helpers ───────────────────────────────────────────────────────
+
+    def _trigger_handoff(self, ps) -> None:
+        """Build and publish a MeshHandoff for a LEAVING person. Must hold self._lock."""
+        msg = MeshHandoff()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.from_robot_id             = self._robot_id
+        msg.to_robot_id               = ""
+        msg.person_id                 = str(ps.person_id)
+        msg.person_name               = ps.person_name
+        msg.face_id                   = ps.face_id
+        msg.speaker_id                = ps.speaker_id
+        msg.bearing_to_target_deg     = ps.bearing_deg
+        msg.distance_m                = ps.distance
+        msg.engagement_score          = ps.engagement_score
+        msg.conversation_summary      = ""
+        msg.last_utterance            = ""
+        msg.last_response             = ""
+        msg.language                  = ""
+        self._handoff_sent.add(str(ps.person_id))
+        self._handoff_pub.publish(msg)
+        self.get_logger().info(
+            f"Handoff → '{ps.person_name or ps.person_id}' "
+            f"bearing={ps.bearing_deg:.0f}° dist={ps.distance:.1f}m"
+        )
+
+    def _publish_announce(self) -> None:
+        with self._lock:
+            active_ids = [
+                pid for pid, ps in self._person_states.items()
+                if ps.state in (_ENGAGED, _TALKING)
+            ]
+            greeted_names = [
+                name for name, t in self._greeted.items()
+                if time.time() - t < self._greeted_mem
+            ]
+            state = self._social_state
+        msg = MeshPeer()
+        msg.header.stamp          = self.get_clock().now().to_msg()
+        msg.robot_id              = self._robot_id
+        msg.namespace             = self._ns
+        msg.social_state          = state
+        msg.active_person_ids     = active_ids
+        msg.greeted_person_names  = greeted_names
+        msg.battery_pct           = -1.0
+        self._announce_pub.publish(msg)
+        self._publish_peers_status()
+
+    def _publish_peers_status(self) -> None:
+        with self._lock:
+            entries = [
+                {
+                    "robot_id":       rid,
+                    "social_state":   d["msg"].social_state,
+                    "active_persons": len(d["msg"].active_person_ids),
+                    "age_s":          round(time.time() - d["last_seen"], 1),
+                }
+                for rid, d in self._peers.items()
+            ]
+        msg = String()
+        msg.data = json.dumps({"peers": entries, "ts": time.time()})
+        self._peers_pub.publish(msg)
+
+    def _publish_greeted(self) -> None:
+        with self._lock:
+            names = [n for n, t in self._greeted.items()
+                     if time.time() - t < self._greeted_mem]
+        msg = String()
+        msg.data = json.dumps({"greeted": names, "ts": time.time()})
+        self._greeted_pub.publish(msg)
+
+    # ── Cleanup ───────────────────────────────────────────────────────────────
+
+    def _cleanup(self) -> None:
+        now = time.time()
+        with self._lock:
+            dead = [rid for rid, d in self._peers.items()
+                    if now - d["last_seen"] > self._peer_timeout]
+            for rid in dead:
+                self.get_logger().info(f"Peer lost: {rid}")
+                del self._peers[rid]
+            self._greeted = {n: t for n, t in self._greeted.items()
+                             if now - t < self._greeted_mem}
+            active_pids = {str(p) for p in self._person_states}
+            self._handoff_sent &= active_pids
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = MeshCommsNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()

jetson/ros2_ws/src/saltybot_social/setup.py

@@ -39,6 +39,8 @@ setup(
             'gesture_node = saltybot_social.gesture_node:main',
             # Facial expression recognition (Issue #161)
             'emotion_node = saltybot_social.emotion_node:main',
+            # Robot mesh communication (Issue #171)
+            'mesh_comms_node = saltybot_social.mesh_comms_node:main',
         ],
     },
 )

jetson/ros2_ws/src/saltybot_social/test/test_mesh_comms.py

@@ -0,0 +1,278 @@
+"""test_mesh_comms.py -- Unit tests for Issue #171 robot mesh comms."""
+
+from __future__ import annotations
+import json, os, time
+from typing import Dict
+import pytest
+
+
+def _pkg_root():
+    return os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+
+
+def _read_src(rel_path):
+    with open(os.path.join(_pkg_root(), rel_path)) as f:
+        return f.read()
+
+
+# ── Message definitions ───────────────────────────────────────────────────────
+
+class TestMeshPeerMsg:
+    @pytest.fixture(scope="class")
+    def src(self):
+        return _read_src("../saltybot_social_msgs/msg/MeshPeer.msg")
+
+    def test_robot_id(self, src):      assert "string  robot_id" in src
+    def test_namespace(self, src):     assert "string  namespace" in src
+    def test_social_state(self, src):  assert "string  social_state" in src
+    def test_active_ids(self, src):    assert "int32[] active_person_ids" in src
+    def test_greeted(self, src):       assert "string[] greeted_person_names" in src
+    def test_battery(self, src):       assert "float32 battery_pct" in src
+    def test_header(self, src):        assert "std_msgs/Header" in src
+    def test_issue_tag(self, src):     assert "171" in src
+
+
+class TestMeshHandoffMsg:
+    @pytest.fixture(scope="class")
+    def src(self):
+        return _read_src("../saltybot_social_msgs/msg/MeshHandoff.msg")
+
+    def test_from_robot(self, src):    assert "from_robot_id" in src
+    def test_to_robot(self, src):      assert "to_robot_id" in src
+    def test_person_id(self, src):     assert "string  person_id" in src
+    def test_person_name(self, src):   assert "string  person_name" in src
+    def test_face_id(self, src):       assert "int32   face_id" in src
+    def test_speaker_id(self, src):    assert "string  speaker_id" in src
+    def test_bearing(self, src):       assert "bearing_to_target_deg" in src
+    def test_distance(self, src):      assert "distance_m" in src
+    def test_engagement(self, src):    assert "engagement_score" in src
+    def test_summary(self, src):       assert "conversation_summary" in src
+    def test_last_utterance(self, src):assert "last_utterance" in src
+    def test_last_response(self, src): assert "last_response" in src
+    def test_language(self, src):      assert "string  language" in src
+    def test_header(self, src):        assert "std_msgs/Header" in src
+
+
+# ── CMakeLists includes new msgs ──────────────────────────────────────────────
+
+class TestCMakeLists:
+    @pytest.fixture(scope="class")
+    def src(self):
+        return _read_src("../saltybot_social_msgs/CMakeLists.txt")
+
+    def test_mesh_peer(self, src):    assert '"msg/MeshPeer.msg"' in src
+    def test_mesh_handoff(self, src): assert '"msg/MeshHandoff.msg"' in src
+    def test_issue_tag(self, src):    assert "Issue #171" in src
+
+
+# ── Node source structure ─────────────────────────────────────────────────────
+
+class TestMeshCommsNodeSrc:
+    @pytest.fixture(scope="class")
+    def src(self):
+        return _read_src("saltybot_social/mesh_comms_node.py")
+
+    def test_class_defined(self, src):        assert "class MeshCommsNode" in src
+    def test_robot_id_param(self, src):       assert '"robot_id"' in src
+    def test_announce_rate_param(self, src):  assert '"announce_rate_hz"' in src
+    def test_peer_timeout_param(self, src):   assert '"peer_timeout_s"' in src
+    def test_handoff_dist_param(self, src):   assert '"handoff_distance_m"' in src
+    def test_greeted_memory_param(self, src): assert '"greeted_memory_s"' in src
+    def test_announce_pub(self, src):         assert "/social/mesh/announce" in src
+    def test_handoff_pub(self, src):          assert "/social/mesh/handoff" in src
+    def test_peers_pub(self, src):            assert "/social/mesh/peers" in src
+    def test_greeted_pub(self, src):          assert "/social/mesh/greeted" in src
+    def test_person_states_sub(self, src):    assert "/social/person_states" in src
+    def test_orch_state_sub(self, src):       assert "/social/orchestrator/state" in src
+    def test_on_announce(self, src):          assert "_on_announce" in src
+    def test_on_handoff(self, src):           assert "_on_handoff" in src
+    def test_on_person_states(self, src):     assert "_on_person_states" in src
+    def test_trigger_handoff(self, src):      assert "_trigger_handoff" in src
+    def test_publish_announce(self, src):     assert "_publish_announce" in src
+    def test_cleanup(self, src):              assert "_cleanup" in src
+    def test_leaving_constant(self, src):     assert "_LEAVING" in src
+    def test_engaged_constant(self, src):     assert "_ENGAGED" in src
+    def test_talking_constant(self, src):     assert "_TALKING" in src
+    def test_main(self, src):                 assert "def main" in src
+    def test_no_self_announce(self, src):
+        assert "self._robot_id" in src  # ignores own announcements
+    def test_mesh_qos_best_effort(self, src): assert "BEST_EFFORT" in src
+    def test_issue_tag(self, src):            assert "171" in src
+
+
+# ── setup.py entry point ──────────────────────────────────────────────────────
+
+class TestSetupPy:
+    @pytest.fixture(scope="class")
+    def src(self):
+        return _read_src("setup.py")
+
+    def test_mesh_entry_point(self, src):
+        assert "mesh_comms_node = saltybot_social.mesh_comms_node:main" in src
+
+
+# ── Config file ───────────────────────────────────────────────────────────────
+
+class TestMeshConfig:
+    @pytest.fixture(scope="class")
+    def src(self):
+        return _read_src("config/mesh_params.yaml")
+
+    def test_node_name(self, src):       assert "mesh_comms_node:" in src
+    def test_robot_id(self, src):        assert "robot_id:" in src
+    def test_announce_rate(self, src):   assert "announce_rate_hz:" in src
+    def test_peer_timeout(self, src):    assert "peer_timeout_s:" in src
+    def test_handoff_dist(self, src):    assert "handoff_distance_m:" in src
+    def test_greeted_memory(self, src):  assert "greeted_memory_s:" in src
+
+
+# ── Peer tracking logic (pure Python, no ROS) ────────────────────────────────
+
+class TestPeerTracking:
+    def _make_peers(self):
+        return {}
+
+    def _add_peer(self, peers, rid, state, active, greeted):
+        peers[rid] = {
+            "last_seen": time.time(),
+            "msg": type("M", (), {
+                "social_state": state,
+                "active_person_ids": active,
+                "greeted_person_names": greeted,
+            })(),
+        }
+
+    def _cleanup(self, peers, timeout=5.0):
+        now = time.time()
+        return {rid: d for rid, d in peers.items()
+                if now - d["last_seen"] <= timeout}
+
+    def test_add_peer(self):
+        peers = self._make_peers()
+        self._add_peer(peers, "bot2", "idle", [], [])
+        assert "bot2" in peers
+
+    def test_ignore_self(self):
+        peers = self._make_peers()
+        robot_id = "saltybot_1"
+        incoming_id = "saltybot_1"
+        if incoming_id != robot_id:
+            self._add_peer(peers, incoming_id, "idle", [], [])
+        assert "saltybot_1" not in peers
+
+    def test_peer_replaced_on_update(self):
+        peers = self._make_peers()
+        self._add_peer(peers, "bot2", "idle", [], [])
+        self._add_peer(peers, "bot2", "speaking", [42], [])
+        assert peers["bot2"]["msg"].social_state == "speaking"
+
+    def test_cleanup_removes_stale(self):
+        peers = self._make_peers()
+        peers["bot3"] = {"last_seen": time.time() - 10, "msg": None}
+        self._add_peer(peers, "bot4", "idle", [], [])
+        cleaned = self._cleanup(peers, timeout=5.0)
+        assert "bot3" not in cleaned
+        assert "bot4" in cleaned
+
+    def test_multiple_peers(self):
+        peers = self._make_peers()
+        for i in range(5):
+            self._add_peer(peers, f"bot{i}", "idle", [], [])
+        assert len(peers) == 5
+
+
+# ── Greeting deduplication logic ─────────────────────────────────────────────
+
+class TestGreetingDedup:
+    def test_new_name_added(self):
+        greeted: Dict[str, float] = {}
+        name = "Alice"
+        if name not in greeted:
+            greeted[name] = time.time()
+        assert "Alice" in greeted
+
+    def test_existing_name_not_overwritten(self):
+        greeted = {"Alice": 1000.0}
+        t_before = greeted["Alice"]
+        if "Alice" not in greeted:
+            greeted["Alice"] = time.time()
+        assert greeted["Alice"] == t_before
+
+    def test_peer_names_merged(self):
+        greeted: Dict[str, float] = {}
+        peer_names = ["Alice", "Bob"]
+        for n in peer_names:
+            if n and n not in greeted:
+                greeted[n] = time.time()
+        assert "Alice" in greeted and "Bob" in greeted
+
+    def test_expire_old_names(self):
+        greeted = {"Alice": time.time() - 400, "Bob": time.time()}
+        memory_s = 300.0
+        fresh = {n: t for n, t in greeted.items() if time.time() - t < memory_s}
+        assert "Alice" not in fresh and "Bob" in fresh
+
+
+# ── Handoff trigger logic ─────────────────────────────────────────────────────
+
+class TestHandoffTrigger:
+    def _should_handoff(self, prev_state, new_state, pid, sent):
+        _LEAVING = 4
+        return (prev_state != _LEAVING
+                and new_state == _LEAVING
+                and str(pid) not in sent)
+
+    def test_triggers_on_leaving(self):
+        assert self._should_handoff(2, 4, 1, set())
+
+    def test_no_duplicate_handoff(self):
+        sent = {"1"}
+        assert not self._should_handoff(2, 4, 1, sent)
+
+    def test_no_trigger_if_already_leaving(self):
+        assert not self._should_handoff(4, 4, 1, set())
+
+    def test_triggers_for_different_person(self):
+        sent = {"1"}
+        assert self._should_handoff(2, 4, 2, sent)
+
+    def test_handoff_sent_cleared_when_person_gone(self):
+        sent = {"1", "2", "3"}
+        active_pids = {"2", "3"}
+        sent &= active_pids
+        assert "1" not in sent and "2" in sent
+
+
+# ── JSON status blob ──────────────────────────────────────────────────────────
+
+class TestPeersBlob:
+    def test_peers_json(self):
+        peers = {
+            "bot2": {
+                "last_seen": time.time(),
+                "msg": type("M", (), {
+                    "social_state": "idle",
+                    "active_person_ids": [],
+                })(),
+            }
+        }
+        entries = [
+            {
+                "robot_id":       rid,
+                "social_state":   d["msg"].social_state,
+                "active_persons": len(d["msg"].active_person_ids),
+                "age_s":          round(time.time() - d["last_seen"], 1),
+            }
+            for rid, d in peers.items()
+        ]
+        blob = json.dumps({"peers": entries, "ts": time.time()})
+        parsed = json.loads(blob)
+        assert len(parsed["peers"]) == 1
+        assert parsed["peers"][0]["robot_id"] == "bot2"
+
+    def test_greeted_json(self):
+        greeted = {"Alice": time.time(), "Bob": time.time()}
+        names = list(greeted.keys())
+        blob = json.dumps({"greeted": names, "ts": time.time()})
+        parsed = json.loads(blob)
+        assert "Alice" in parsed["greeted"] and "Bob" in parsed["greeted"]

jetson/ros2_ws/src/saltybot_social_msgs/CMakeLists.txt

@@ -41,6 +41,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   # Issue #161 — emotion detection
   "msg/Expression.msg"
   "msg/ExpressionArray.msg"
+  # Issue #171 — robot mesh comms
+  "msg/MeshPeer.msg"
+  "msg/MeshHandoff.msg"
   DEPENDENCIES std_msgs geometry_msgs builtin_interfaces
 )
 

jetson/ros2_ws/src/saltybot_social_msgs/msg/MeshHandoff.msg

@@ -0,0 +1,27 @@
+# MeshHandoff.msg — Person context transfer between robots.
+# Published on /social/mesh/handoff when a tracked person transitions to
+# STATE_LEAVING, enabling a nearby robot to warm-start LLM conversation context.
+# Issue #171
+
+std_msgs/Header header
+
+string  from_robot_id           # Robot publishing this handoff
+string  to_robot_id             # Intended recipient robot_id; "" = broadcast
+
+# Person identity
+string  person_id               # e.g. "person_42"
+string  person_name             # "" if unknown
+int32   face_id                 # -1 if unknown
+string  speaker_id              # "" if unknown
+
+# Spatial context at handoff time
+float32 bearing_to_target_deg   # Person's bearing from from_robot (°, +CW from forward)
+float32 distance_m              # Person's distance from from_robot (metres)
+float32 engagement_score        # 0..1
+
+# Conversation warm-start context
+string  conversation_summary    # LLM-generated summary of prior exchanges
+string  last_utterance          # Person's last utterance to from_robot
+string  last_response           # Robot's last response to person
+
+string  language                # BCP-47 code e.g. "en", "fr" (empty = unknown)

jetson/ros2_ws/src/saltybot_social_msgs/msg/MeshPeer.msg

@@ -0,0 +1,20 @@
+# MeshPeer.msg — Robot social presence announcement.
+# Published by mesh_comms_node on /social/mesh/announce at 1 Hz.
+# All robots sharing the same DDS domain receive peer announcements via multicast.
+# Issue #171
+
+std_msgs/Header header
+
+string  robot_id                # e.g. "saltybot_1"
+string  namespace               # ROS2 namespace, e.g. "/saltybot_1" (empty = default)
+
+# Current social pipeline state (mirrors orchestrator PipelineState)
+string  social_state            # "idle" | "listening" | "thinking" | "speaking" | "throttled"
+
+# Active person IDs this robot is currently engaged with or talking to
+int32[] active_person_ids
+
+# Names greeted since last boot — used for mesh-wide greeting deduplication
+string[] greeted_person_names
+
+float32 battery_pct             # 0..100; -1 = unknown

jetson/ros2_ws/src/saltybot_thermal/config/thermal_params.yaml

@@ -0,0 +1,6 @@
+thermal_node:
+  ros__parameters:
+    publish_rate_hz: 1.0        # Hz — publish rate for /saltybot/thermal
+    warn_temp_c: 75.0           # Log WARN above this temperature (°C)
+    throttle_temp_c: 85.0       # Log ERROR + set throttled=true above this (°C)
+    thermal_root: "/sys/class/thermal"  # Sysfs thermal root; override for tests

jetson/ros2_ws/src/saltybot_thermal/launch/thermal.launch.py

@@ -0,0 +1,42 @@
+"""thermal.launch.py — Launch the Jetson thermal monitor (Issue #205).
+
+Usage:
+  ros2 launch saltybot_thermal thermal.launch.py
+  ros2 launch saltybot_thermal thermal.launch.py warn_temp_c:=70.0
+"""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg = get_package_share_directory("saltybot_thermal")
+    cfg = os.path.join(pkg, "config", "thermal_params.yaml")
+
+    return LaunchDescription([
+        DeclareLaunchArgument("publish_rate_hz",  default_value="1.0",
+                              description="Publish rate (Hz)"),
+        DeclareLaunchArgument("warn_temp_c",      default_value="75.0",
+                              description="WARN threshold (°C)"),
+        DeclareLaunchArgument("throttle_temp_c",  default_value="85.0",
+                              description="THROTTLE threshold (°C)"),
+
+        Node(
+            package="saltybot_thermal",
+            executable="thermal_node",
+            name="thermal_node",
+            output="screen",
+            parameters=[
+                cfg,
+                {
+                    "publish_rate_hz": LaunchConfiguration("publish_rate_hz"),
+                    "warn_temp_c":     LaunchConfiguration("warn_temp_c"),
+                    "throttle_temp_c": LaunchConfiguration("throttle_temp_c"),
+                },
+            ],
+        ),
+    ])

jetson/ros2_ws/src/saltybot_thermal/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_thermal</name>
+  <version>0.1.0</version>
+  <description>
+    Jetson thermal monitor (Issue #205). Reads /sys/class/thermal/thermal_zone*,
+    publishes /saltybot/thermal JSON at 1 Hz, warns at 75 °C, throttles at 85 °C.
+  </description>
+  <maintainer email="sl-jetson@saltylab.local">sl-jetson</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_thermal/saltybot_thermal/thermal_node.py

@@ -0,0 +1,139 @@
+"""thermal_node.py — Jetson CPU/GPU thermal monitor.
+Issue #205
+
+Reads every /sys/class/thermal/thermal_zone* sysfs entry, publishes a JSON
+blob on /saltybot/thermal at a configurable rate (default 1 Hz), and logs
+ROS2 WARN / ERROR when zone temperatures exceed configurable thresholds.
+
+Published topic:
+  /saltybot/thermal  (std_msgs/String, JSON)
+
+JSON schema:
+  {
+    "ts": <float unix seconds>,
+    "zones": [
+      {"zone": "CPU-therm", "index": 0, "temp_c": 42.5},
+      ...
+    ],
+    "max_temp_c": 55.0,
+    "throttled": false,
+    "warn": false
+  }
+
+Parameters:
+  publish_rate_hz   (float, 1.0)  — publish rate
+  warn_temp_c       (float, 75.0) — log WARN above this temperature
+  throttle_temp_c   (float, 85.0) — log ERROR and set throttled=true above this
+  thermal_root      (str, "/sys/class/thermal") — sysfs thermal root (override for tests)
+"""
+
+from __future__ import annotations
+
+import json
+import os
+import time
+from typing import List, Optional
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile
+from std_msgs.msg import String
+
+
+def read_thermal_zones(root: str) -> List[dict]:
+    """Return a list of {zone, index, temp_c} dicts from sysfs."""
+    zones = []
+    try:
+        entries = sorted(os.listdir(root))
+    except OSError:
+        return zones
+    for entry in entries:
+        if not entry.startswith("thermal_zone"):
+            continue
+        try:
+            idx = int(entry[len("thermal_zone"):])
+        except ValueError:
+            continue
+        zone_dir = os.path.join(root, entry)
+        try:
+            with open(os.path.join(zone_dir, "type")) as f:
+                zone_type = f.read().strip()
+        except OSError:
+            zone_type = entry
+        try:
+            with open(os.path.join(zone_dir, "temp")) as f:
+                temp_mc = int(f.read().strip())   # millidegrees Celsius
+            temp_c = round(temp_mc / 1000.0, 1)
+        except (OSError, ValueError):
+            continue
+        zones.append({"zone": zone_type, "index": idx, "temp_c": temp_c})
+    return zones
+
+
+class ThermalNode(Node):
+    """Reads Jetson thermal zones and publishes /saltybot/thermal at 1 Hz."""
+
+    def __init__(self) -> None:
+        super().__init__("thermal_node")
+
+        self.declare_parameter("publish_rate_hz",  1.0)
+        self.declare_parameter("warn_temp_c",      75.0)
+        self.declare_parameter("throttle_temp_c",  85.0)
+        self.declare_parameter("thermal_root",     "/sys/class/thermal")
+
+        self._rate        = self.get_parameter("publish_rate_hz").value
+        self._warn_t      = self.get_parameter("warn_temp_c").value
+        self._throttle_t  = self.get_parameter("throttle_temp_c").value
+        self._root        = self.get_parameter("thermal_root").value
+
+        qos = QoSProfile(depth=10)
+        self._pub = self.create_publisher(String, "/saltybot/thermal", qos)
+        self._timer = self.create_timer(1.0 / self._rate, self._publish)
+
+        self.get_logger().info(
+            f"ThermalNode ready (rate={self._rate} Hz, "
+            f"warn={self._warn_t}°C, throttle={self._throttle_t}°C, "
+            f"root={self._root})"
+        )
+
+    def _publish(self) -> None:
+        zones = read_thermal_zones(self._root)
+        if not zones:
+            self.get_logger().warn("No thermal zones found — check thermal_root param")
+            return
+
+        max_temp = max(z["temp_c"] for z in zones)
+        throttled = max_temp >= self._throttle_t
+        warn      = max_temp >= self._warn_t
+
+        payload = {
+            "ts":         time.time(),
+            "zones":      zones,
+            "max_temp_c": max_temp,
+            "throttled":  throttled,
+            "warn":       warn,
+        }
+        msg = String()
+        msg.data = json.dumps(payload)
+        self._pub.publish(msg)
+
+        if throttled:
+            self.get_logger().error(
+                f"THERMAL THROTTLE: {max_temp}°C >= {self._throttle_t}°C"
+            )
+        elif warn:
+            self.get_logger().warn(
+                f"Thermal warning: {max_temp}°C >= {self._warn_t}°C"
+            )
+
+
+def main(args: Optional[list] = None) -> None:
+    rclpy.init(args=args)
+    node = ThermalNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()

jetson/ros2_ws/src/saltybot_thermal/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/saltybot_thermal
+[egg_info]
+tag_date = 0

jetson/ros2_ws/src/saltybot_thermal/setup.py

@@ -0,0 +1,27 @@
+from setuptools import setup
+
+package_name = "saltybot_thermal"
+
+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/thermal.launch.py"]),
+        (f"share/{package_name}/config", ["config/thermal_params.yaml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="sl-jetson",
+    maintainer_email="sl-jetson@saltylab.local",
+    description="Jetson thermal monitor — /saltybot/thermal JSON at 1 Hz",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "thermal_node = saltybot_thermal.thermal_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_thermal/test/test_thermal.py

@@ -0,0 +1,303 @@
+"""test_thermal.py -- Unit tests for Issue #205 Jetson thermal monitor."""
+
+from __future__ import annotations
+import json, os, time
+import pytest
+
+
+def _pkg_root():
+    return os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+
+
+def _read_src(rel_path):
+    with open(os.path.join(_pkg_root(), rel_path)) as f:
+        return f.read()
+
+
+# ── Import the sysfs reader (no ROS required) ─────────────────────────────────
+
+def _import_reader():
+    import importlib.util, sys, types
+
+    # Build minimal ROS2 stubs so thermal_node.py imports without a ROS install
+    def _stub(name):
+        m = types.ModuleType(name)
+        sys.modules[name] = m
+        return m
+
+    rclpy_mod      = _stub("rclpy")
+    rclpy_node_mod = _stub("rclpy.node")
+    rclpy_qos_mod  = _stub("rclpy.qos")
+    std_msgs_mod   = _stub("std_msgs")
+    std_msg_mod    = _stub("std_msgs.msg")
+
+    class _Node:
+        def __init__(self, *a, **kw): pass
+        def declare_parameter(self, *a, **kw): pass
+        def get_parameter(self, name):
+            class _P:
+                value = None
+            return _P()
+        def create_publisher(self, *a, **kw): return None
+        def create_timer(self, *a, **kw): return None
+        def get_logger(self):
+            class _L:
+                def info(self, *a): pass
+                def warn(self, *a): pass
+                def error(self, *a): pass
+            return _L()
+        def destroy_node(self): pass
+
+    class _QoSProfile:
+        def __init__(self, **kw): pass
+
+    class _String:
+        data = ""
+
+    rclpy_node_mod.Node = _Node
+    rclpy_qos_mod.QoSProfile = _QoSProfile
+    std_msg_mod.String = _String
+    rclpy_mod.init = lambda *a, **kw: None
+    rclpy_mod.spin = lambda node: None
+    rclpy_mod.ok   = lambda: True
+    rclpy_mod.shutdown = lambda: None
+
+    spec = importlib.util.spec_from_file_location(
+        "thermal_node_testmod",
+        os.path.join(_pkg_root(), "saltybot_thermal", "thermal_node.py"),
+    )
+    mod = importlib.util.module_from_spec(spec)
+    spec.loader.exec_module(mod)
+    return mod
+
+
+# ── Sysfs fixture helpers ─────────────────────────────────────────────────────
+
+def _make_zone(root, idx, zone_type, temp_mc):
+    """Create a fake thermal_zone<idx> directory under root."""
+    zdir = os.path.join(str(root), "thermal_zone{}".format(idx))
+    os.makedirs(zdir, exist_ok=True)
+    with open(os.path.join(zdir, "type"), "w") as f:
+        f.write(zone_type + "\n")
+    with open(os.path.join(zdir, "temp"), "w") as f:
+        f.write(str(temp_mc) + "\n")
+
+
+# ── read_thermal_zones ────────────────────────────────────────────────────────
+
+class TestReadThermalZones:
+    @pytest.fixture(scope="class")
+    def mod(self):
+        return _import_reader()
+
+    def test_empty_dir(self, mod, tmp_path):
+        assert mod.read_thermal_zones(str(tmp_path)) == []
+
+    def test_missing_dir(self, mod):
+        assert mod.read_thermal_zones("/nonexistent/path/xyz") == []
+
+    def test_single_zone(self, mod, tmp_path):
+        _make_zone(tmp_path, 0, "CPU-therm", 45000)
+        zones = mod.read_thermal_zones(str(tmp_path))
+        assert len(zones) == 1
+        assert zones[0]["zone"] == "CPU-therm"
+        assert zones[0]["temp_c"] == 45.0
+        assert zones[0]["index"] == 0
+
+    def test_temp_millidegrees_conversion(self, mod, tmp_path):
+        _make_zone(tmp_path, 0, "GPU-therm", 72500)
+        zones = mod.read_thermal_zones(str(tmp_path))
+        assert zones[0]["temp_c"] == 72.5
+
+    def test_multiple_zones(self, mod, tmp_path):
+        _make_zone(tmp_path, 0, "CPU-therm", 40000)
+        _make_zone(tmp_path, 1, "GPU-therm", 55000)
+        _make_zone(tmp_path, 2, "PMIC-Die",  38000)
+        zones = mod.read_thermal_zones(str(tmp_path))
+        assert len(zones) == 3
+
+    def test_sorted_by_index(self, mod, tmp_path):
+        _make_zone(tmp_path, 2, "Z2", 20000)
+        _make_zone(tmp_path, 0, "Z0", 10000)
+        _make_zone(tmp_path, 1, "Z1", 15000)
+        zones = mod.read_thermal_zones(str(tmp_path))
+        indices = [z["index"] for z in zones]
+        assert indices == sorted(indices)
+
+    def test_skips_non_zone_entries(self, mod, tmp_path):
+        os.makedirs(os.path.join(str(tmp_path), "cooling_device0"))
+        _make_zone(tmp_path, 0, "CPU-therm", 40000)
+        zones = mod.read_thermal_zones(str(tmp_path))
+        assert len(zones) == 1
+
+    def test_skips_zone_without_temp(self, mod, tmp_path):
+        zdir = os.path.join(str(tmp_path), "thermal_zone0")
+        os.makedirs(zdir)
+        with open(os.path.join(zdir, "type"), "w") as f:
+            f.write("CPU-therm\n")
+        # No temp file — should be skipped
+        zones = mod.read_thermal_zones(str(tmp_path))
+        assert zones == []
+
+    def test_zone_type_fallback(self, mod, tmp_path):
+        """Zone without type file falls back to directory name."""
+        zdir = os.path.join(str(tmp_path), "thermal_zone0")
+        os.makedirs(zdir)
+        with open(os.path.join(zdir, "temp"), "w") as f:
+            f.write("40000\n")
+        zones = mod.read_thermal_zones(str(tmp_path))
+        assert len(zones) == 1
+        assert zones[0]["zone"] == "thermal_zone0"
+
+    def test_temp_rounding(self, mod, tmp_path):
+        _make_zone(tmp_path, 0, "CPU-therm", 72333)
+        zones = mod.read_thermal_zones(str(tmp_path))
+        assert zones[0]["temp_c"] == 72.3
+
+
+# ── Threshold logic (pure Python) ────────────────────────────────────────────
+
+class TestThresholds:
+    def _classify(self, temp_c, warn_t=75.0, throttle_t=85.0):
+        throttled = temp_c >= throttle_t
+        warn      = temp_c >= warn_t
+        return throttled, warn
+
+    def test_normal(self):
+        t, w = self._classify(50.0)
+        assert not t and not w
+
+    def test_warn_boundary(self):
+        t, w = self._classify(75.0)
+        assert not t and w
+
+    def test_below_warn(self):
+        t, w = self._classify(74.9)
+        assert not t and not w
+
+    def test_throttle_boundary(self):
+        t, w = self._classify(85.0)
+        assert t and w
+
+    def test_above_throttle(self):
+        t, w = self._classify(90.0)
+        assert t and w
+
+    def test_custom_thresholds(self):
+        t, w = self._classify(70.0, warn_t=70.0, throttle_t=80.0)
+        assert not t and w
+
+    def test_max_temp_drives_status(self):
+        zones = [{"temp_c": 40.0}, {"temp_c": 86.0}, {"temp_c": 55.0}]
+        max_t = max(z["temp_c"] for z in zones)
+        assert max_t == 86.0
+        t, w = self._classify(max_t)
+        assert t and w
+
+
+# ── JSON payload schema ───────────────────────────────────────────────────────
+
+class TestJsonPayload:
+    def _make_payload(self, zones, warn_t=75.0, throttle_t=85.0):
+        max_temp = max(z["temp_c"] for z in zones) if zones else 0.0
+        return {
+            "ts":         time.time(),
+            "zones":      zones,
+            "max_temp_c": max_temp,
+            "throttled":  max_temp >= throttle_t,
+            "warn":       max_temp >= warn_t,
+        }
+
+    def test_has_ts(self):
+        p = self._make_payload([{"zone": "CPU", "index": 0, "temp_c": 40.0}])
+        assert "ts" in p and isinstance(p["ts"], float)
+
+    def test_has_zones(self):
+        p = self._make_payload([{"zone": "CPU", "index": 0, "temp_c": 40.0}])
+        assert "zones" in p and len(p["zones"]) == 1
+
+    def test_has_max_temp(self):
+        p = self._make_payload([{"zone": "CPU", "index": 0, "temp_c": 55.0}])
+        assert p["max_temp_c"] == 55.0
+
+    def test_throttled_false_below(self):
+        p = self._make_payload([{"zone": "CPU", "index": 0, "temp_c": 60.0}])
+        assert p["throttled"] is False
+
+    def test_warn_true_at_threshold(self):
+        p = self._make_payload([{"zone": "CPU", "index": 0, "temp_c": 75.0}])
+        assert p["warn"] is True and p["throttled"] is False
+
+    def test_throttled_true_above(self):
+        p = self._make_payload([{"zone": "CPU", "index": 0, "temp_c": 90.0}])
+        assert p["throttled"] is True
+
+    def test_json_serializable(self):
+        zones = [{"zone": "CPU", "index": 0, "temp_c": 50.0}]
+        p = self._make_payload(zones)
+        blob = json.dumps(p)
+        parsed = json.loads(blob)
+        assert parsed["max_temp_c"] == 50.0
+
+    def test_multi_zone_max(self):
+        zones = [
+            {"zone": "CPU-therm", "index": 0, "temp_c": 55.0},
+            {"zone": "GPU-therm", "index": 1, "temp_c": 78.0},
+            {"zone": "PMIC-Die",  "index": 2, "temp_c": 38.0},
+        ]
+        p = self._make_payload(zones)
+        assert p["max_temp_c"] == 78.0
+        assert p["warn"] is True
+        assert p["throttled"] is False
+
+
+# ── Node source checks ────────────────────────────────────────────────────────
+
+class TestNodeSrc:
+    @pytest.fixture(scope="class")
+    def src(self):
+        return _read_src("saltybot_thermal/thermal_node.py")
+
+    def test_class_defined(self, src):       assert "class ThermalNode" in src
+    def test_publish_rate_param(self, src):  assert '"publish_rate_hz"' in src
+    def test_warn_param(self, src):          assert '"warn_temp_c"' in src
+    def test_throttle_param(self, src):      assert '"throttle_temp_c"' in src
+    def test_thermal_root_param(self, src):  assert '"thermal_root"' in src
+    def test_topic(self, src):               assert '"/saltybot/thermal"' in src
+    def test_read_fn(self, src):             assert "read_thermal_zones" in src
+    def test_warn_log(self, src):            assert "warn" in src.lower()
+    def test_error_log(self, src):           assert "error" in src.lower()
+    def test_throttled_flag(self, src):      assert '"throttled"' in src
+    def test_warn_flag(self, src):           assert '"warn"' in src
+    def test_max_temp(self, src):            assert '"max_temp_c"' in src
+    def test_millidegrees(self, src):        assert "1000" in src
+    def test_json_dumps(self, src):          assert "json.dumps" in src
+    def test_issue_tag(self, src):           assert "205" in src
+    def test_main(self, src):                assert "def main" in src
+    def test_sysfs_path(self, src):          assert "/sys/class/thermal" in src
+
+
+# ── Package metadata ──────────────────────────────────────────────────────────
+
+class TestPackageMeta:
+    @pytest.fixture(scope="class")
+    def pkg_xml(self):
+        return _read_src("package.xml")
+
+    @pytest.fixture(scope="class")
+    def setup_py(self):
+        return _read_src("setup.py")
+
+    @pytest.fixture(scope="class")
+    def cfg(self):
+        return _read_src("config/thermal_params.yaml")
+
+    def test_pkg_name(self, pkg_xml):     assert "saltybot_thermal" in pkg_xml
+    def test_issue_tag(self, pkg_xml):    assert "205" in pkg_xml
+    def test_entry_point(self, setup_py): assert "thermal_node = saltybot_thermal.thermal_node:main" in setup_py
+    def test_cfg_node_name(self, cfg):    assert "thermal_node:" in cfg
+    def test_cfg_warn(self, cfg):         assert "warn_temp_c" in cfg
+    def test_cfg_throttle(self, cfg):     assert "throttle_temp_c" in cfg
+    def test_cfg_rate(self, cfg):         assert "publish_rate_hz" in cfg
+    def test_cfg_defaults(self, cfg):
+        assert "75.0" in cfg and "85.0" in cfg and "1.0" in cfg

src/led.c

@@ -0,0 +1,307 @@
+#include "led.h"
+#include "config.h"
+#include "stm32f7xx_hal.h"
+#include <string.h>
+#include <math.h>
+
+/* ================================================================
+ * WS2812B NeoPixel protocol via PWM
+ * ================================================================
+ * 800 kHz PWM → 1.25 µs per cycle
+ * Bit encoding:
+ *   "0": High 350 ns  (40% duty)  → ~3/8 of 1.25 µs
+ *   "1": High 700 ns  (56% duty)  → ~7/10 of 1.25 µs
+ * Reset: Low > 50 µs (automatic with DMA ring and reload)
+ *
+ * Implementation: DMA copies PWM duty values from buffer.
+ * Each bit needs one PWM cycle; 192 bits total (24 bits/LED × 8 LEDs).
+ */
+
+#define LED_BITS_PER_COLOR  8u
+#define LED_BITS_PER_LED    (LED_BITS_PER_COLOR * 3u)  /* RGB */
+#define LED_TOTAL_BITS      (LED_BITS_PER_LED * LED_STRIP_NUM_LEDS)
+#define LED_PWM_PERIOD      (216000000 / LED_STRIP_FREQ_HZ)  /* 216 MHz / 800 kHz */
+
+/* PWM duty values for bit encoding (out of LED_PWM_PERIOD) */
+#define LED_BIT_0_DUTY      (LED_PWM_PERIOD * 40 / 100)   /* ~350 ns high */
+#define LED_BIT_1_DUTY      (LED_PWM_PERIOD * 56 / 100)   /* ~700 ns high */
+
+/* ================================================================
+ * LED buffer and animation state
+ * ================================================================
+ */
+
+typedef struct {
+    RGBColor leds[LED_STRIP_NUM_LEDS];
+    uint32_t pwm_buf[LED_TOTAL_BITS];  /* DMA buffer: PWM duty values */
+} LEDBuffer;
+
+/* LED state machine */
+typedef struct {
+    LEDState current_state;
+    LEDState next_state;
+    uint32_t state_start_ms;
+    uint8_t animation_phase;  /* 0-255 for continuous animations */
+} LEDAnimState;
+
+static LEDBuffer      s_led_buf = {0};
+static LEDAnimState   s_anim = {0};
+static TIM_HandleTypeDef s_tim_handle = {0};
+
+/* ================================================================
+ * Helper functions
+ * ================================================================
+ */
+
+static void rgb_to_pwm_buffer(const RGBColor *colors, uint8_t num_leds)
+{
+    /* Encode LED colors into PWM duty values for WS2812B transmission.
+     * GRB byte order (WS2812B standard), MSB first. */
+    uint32_t buf_idx = 0;
+
+    for (uint8_t led = 0; led < num_leds; led++) {
+        uint8_t g = colors[led].g;
+        uint8_t r = colors[led].r;
+        uint8_t b = colors[led].b;
+
+        /* GRB byte order */
+        uint8_t bytes[3] = {g, r, b};
+
+        for (int byte_idx = 0; byte_idx < 3; byte_idx++) {
+            uint8_t byte = bytes[byte_idx];
+
+            /* MSB first — encode 8 bits */
+            for (int bit = 7; bit >= 0; bit--) {
+                uint8_t bit_val = (byte >> bit) & 1;
+                s_led_buf.pwm_buf[buf_idx++] = bit_val ? LED_BIT_1_DUTY : LED_BIT_0_DUTY;
+            }
+        }
+    }
+}
+
+static uint8_t sin_u8(uint8_t phase)
+{
+    /* Approximate sine wave (0-255) from phase (0-255) for breathing effect. */
+    static const uint8_t sine_lut[256] = {
+        128, 131, 134, 137, 140, 143, 146, 149, 152, 155, 158, 161, 164, 167, 170, 173,
+        176, 179, 182, 185, 188, 191, 193, 196, 199, 201, 204, 206, 209, 211, 214, 216,
+        218, 221, 223, 225, 227, 229, 231, 233, 235, 236, 238, 240, 241, 243, 244, 245,
+        247, 248, 249, 250, 251, 252, 252, 253, 254, 254, 255, 255, 255, 255, 255, 254,
+        254, 253, 252, 252, 251, 250, 249, 248, 247, 245, 244, 243, 241, 240, 238, 236,
+        235, 233, 231, 229, 227, 225, 223, 221, 218, 216, 214, 211, 209, 206, 204, 201,
+        199, 196, 193, 191, 188, 185, 182, 179, 176, 173, 170, 167, 164, 161, 158, 155,
+        152, 149, 146, 143, 140, 137, 134, 131, 128, 125, 122, 119, 116, 113, 110, 107,
+        104, 101, 98, 95, 92, 89, 86, 83, 80, 77, 74, 71, 68, 65, 62, 59,
+        56, 53, 50, 47, 44, 41, 39, 36, 33, 31, 28, 26, 23, 21, 18, 16,
+        14, 11, 9, 7, 5, 3, 1, 0, 0, 0, 0, 0, 1, 2, 3, 4,
+        5, 7, 8, 10, 11, 13, 15, 17, 19, 21, 23, 26, 28, 31, 33, 36,
+        39, 42, 45, 48, 51, 54, 57, 60, 63, 66, 69, 72, 75, 78, 82, 85,
+        88, 92, 95, 99, 102, 105, 109, 113, 116, 120, 124, 127, 131
+    };
+    return sine_lut[phase];
+}
+
+/* ================================================================
+ * Animation implementations
+ * ================================================================
+ */
+
+static void animate_boot(uint32_t elapsed_ms)
+{
+    /* Blue chase: rotate a single LED around the ring. */
+    uint8_t led_idx = (elapsed_ms / 100) % LED_STRIP_NUM_LEDS;  /* 100 ms per LED */
+
+    memset(s_led_buf.leds, 0, sizeof(s_led_buf.leds));
+    s_led_buf.leds[led_idx].b = 255;  /* Bright blue */
+
+    rgb_to_pwm_buffer(s_led_buf.leds, LED_STRIP_NUM_LEDS);
+}
+
+static void animate_armed(void)
+{
+    /* Solid green: all LEDs constant brightness. */
+    for (uint8_t i = 0; i < LED_STRIP_NUM_LEDS; i++) {
+        s_led_buf.leds[i].g = 200;  /* Bright green */
+        s_led_buf.leds[i].r = 0;
+        s_led_buf.leds[i].b = 0;
+    }
+
+    rgb_to_pwm_buffer(s_led_buf.leds, LED_STRIP_NUM_LEDS);
+}
+
+static void animate_error(uint32_t elapsed_ms)
+{
+    /* Red blinking: on/off every 250 ms. */
+    bool on = ((elapsed_ms / 250) % 2) == 0;
+
+    for (uint8_t i = 0; i < LED_STRIP_NUM_LEDS; i++) {
+        s_led_buf.leds[i].r = on ? 255 : 0;
+        s_led_buf.leds[i].g = 0;
+        s_led_buf.leds[i].b = 0;
+    }
+
+    rgb_to_pwm_buffer(s_led_buf.leds, LED_STRIP_NUM_LEDS);
+}
+
+static void animate_low_battery(uint32_t elapsed_ms)
+{
+    /* Yellow pulsing: brightness varies smoothly. */
+    uint8_t phase = (elapsed_ms / 20) & 0xFF;  /* Cycle every 5120 ms */
+    uint8_t brightness = sin_u8(phase);
+
+    for (uint8_t i = 0; i < LED_STRIP_NUM_LEDS; i++) {
+        s_led_buf.leds[i].r = (brightness * 255) >> 8;
+        s_led_buf.leds[i].g = (brightness * 255) >> 8;
+        s_led_buf.leds[i].b = 0;
+    }
+
+    rgb_to_pwm_buffer(s_led_buf.leds, LED_STRIP_NUM_LEDS);
+}
+
+static void animate_charging(uint32_t elapsed_ms)
+{
+    /* Green breathing: smooth brightness modulation. */
+    uint8_t phase = (elapsed_ms / 20) & 0xFF;  /* Cycle every 5120 ms */
+    uint8_t brightness = sin_u8(phase);
+
+    for (uint8_t i = 0; i < LED_STRIP_NUM_LEDS; i++) {
+        s_led_buf.leds[i].g = (brightness * 255) >> 8;
+        s_led_buf.leds[i].r = 0;
+        s_led_buf.leds[i].b = 0;
+    }
+
+    rgb_to_pwm_buffer(s_led_buf.leds, LED_STRIP_NUM_LEDS);
+}
+
+static void animate_estop(uint32_t elapsed_ms)
+{
+    /* Red strobe: on/off every 125 ms (8 Hz). */
+    bool on = ((elapsed_ms / 125) % 2) == 0;
+
+    for (uint8_t i = 0; i < LED_STRIP_NUM_LEDS; i++) {
+        s_led_buf.leds[i].r = on ? 255 : 0;
+        s_led_buf.leds[i].g = 0;
+        s_led_buf.leds[i].b = 0;
+    }
+
+    rgb_to_pwm_buffer(s_led_buf.leds, LED_STRIP_NUM_LEDS);
+}
+
+/* ================================================================
+ * Public API
+ * ================================================================
+ */
+
+void led_init(void)
+{
+    /* Initialize state machine */
+    s_anim.current_state = LED_STATE_BOOT;
+    s_anim.next_state = LED_STATE_BOOT;
+    s_anim.state_start_ms = 0;
+    s_anim.animation_phase = 0;
+
+    /* Configure GPIO PB4 as TIM3_CH1 output (AF2) */
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+    GPIO_InitTypeDef gpio_init = {0};
+    gpio_init.Pin       = LED_STRIP_PIN;
+    gpio_init.Mode      = GPIO_MODE_AF_PP;
+    gpio_init.Pull      = GPIO_NOPULL;
+    gpio_init.Speed     = GPIO_SPEED_FREQ_HIGH;
+    gpio_init.Alternate = LED_STRIP_AF;
+    HAL_GPIO_Init(LED_STRIP_PORT, &gpio_init);
+
+    /* Configure TIM3: PWM mode, 800 kHz frequency
+     * STM32F722 has 216 MHz on APB1; TIM3 is on APB1 (prescaler 4×).
+     * APB1 clock: 216 MHz / 4 = 54 MHz
+     * For 800 kHz PWM: 54 MHz / 800 kHz = 67.5 → use 67 or 68
+     * With ARR = 67: 54 MHz / 68 = 794 kHz ≈ 800 kHz
+     */
+    __HAL_RCC_TIM3_CLK_ENABLE();
+
+    s_tim_handle.Instance           = LED_STRIP_TIM;
+    s_tim_handle.Init.Prescaler     = 0;  /* No prescaler; APB1 = 54 MHz directly */
+    s_tim_handle.Init.CounterMode   = TIM_COUNTERMODE_UP;
+    s_tim_handle.Init.Period        = LED_PWM_PERIOD - 1;
+    s_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+    s_tim_handle.Init.RepetitionCounter = 0;
+
+    HAL_TIM_PWM_Init(&s_tim_handle);
+
+    /* Configure TIM3_CH1 for PWM */
+    TIM_OC_InitTypeDef oc_init = {0};
+    oc_init.OCMode       = TIM_OCMODE_PWM1;
+    oc_init.Pulse        = 0;  /* Start at 0% duty */
+    oc_init.OCPolarity   = TIM_OCPOLARITY_HIGH;
+    oc_init.OCFastMode   = TIM_OCFAST_DISABLE;
+
+    HAL_TIM_PWM_ConfigChannel(&s_tim_handle, &oc_init, LED_STRIP_CHANNEL);
+    HAL_TIM_PWM_Start(&s_tim_handle, LED_STRIP_CHANNEL);
+
+    /* Initialize LED buffer with boot state */
+    animate_boot(0);
+}
+
+void led_set_state(LEDState state)
+{
+    if (state >= LED_STATE_COUNT) {
+        return;
+    }
+    s_anim.next_state = state;
+}
+
+LEDState led_get_state(void)
+{
+    return s_anim.current_state;
+}
+
+void led_set_color(uint8_t r, uint8_t g, uint8_t b)
+{
+    for (uint8_t i = 0; i < LED_STRIP_NUM_LEDS; i++) {
+        s_led_buf.leds[i].r = r;
+        s_led_buf.leds[i].g = g;
+        s_led_buf.leds[i].b = b;
+    }
+    rgb_to_pwm_buffer(s_led_buf.leds, LED_STRIP_NUM_LEDS);
+}
+
+void led_tick(uint32_t now_ms)
+{
+    /* State transition */
+    if (s_anim.next_state != s_anim.current_state) {
+        s_anim.current_state = s_anim.next_state;
+        s_anim.state_start_ms = now_ms;
+    }
+
+    uint32_t elapsed = now_ms - s_anim.state_start_ms;
+
+    /* Run state-specific animation */
+    switch (s_anim.current_state) {
+        case LED_STATE_BOOT:
+            animate_boot(elapsed);
+            break;
+        case LED_STATE_ARMED:
+            animate_armed();
+            break;
+        case LED_STATE_ERROR:
+            animate_error(elapsed);
+            break;
+        case LED_STATE_LOW_BATT:
+            animate_low_battery(elapsed);
+            break;
+        case LED_STATE_CHARGING:
+            animate_charging(elapsed);
+            break;
+        case LED_STATE_ESTOP:
+            animate_estop(elapsed);
+            break;
+        default:
+            break;
+    }
+}
+
+bool led_is_animating(void)
+{
+    /* Static states: ARMED (always) and ERROR (after first blink) */
+    /* All others animate continuously */
+    return s_anim.current_state != LED_STATE_ARMED;
+}

src/main.c

@@ -19,6 +19,9 @@
 #include "jlink.h"
 #include "ota.h"
 #include "audio.h"
+#include "buzzer.h"
+#include "led.h"
+#include "servo.h"
 #include "power_mgmt.h"
 #include "battery.h"
 #include <math.h>
@@ -150,9 +153,20 @@ int main(void) {
     audio_init();
     audio_play_tone(AUDIO_TONE_STARTUP);
 
+    /* Init piezo buzzer driver (TIM4_CH3 PWM on PB2, Issue #189) */
+    buzzer_init();
+    buzzer_play(BUZZER_PATTERN_ARM_CHIME);
+
+    /* Init WS2812B NeoPixel LED ring (TIM3_CH1 PWM on PB4, Issue #193) */
+    led_init();
+    led_set_state(LED_STATE_BOOT);
+
     /* Init power management — STOP-mode sleep/wake, wake EXTIs configured */
     power_mgmt_init();
 
+    /* Init servo pan-tilt driver for camera head (TIM4 PWM on PB6/PB7, Issue #206) */
+    servo_init();
+
     /* Init mode manager (RC/autonomous blend; CH6 mode switch) */
     mode_manager_t mode;
     mode_manager_init(&mode);
@@ -202,6 +216,15 @@ int main(void) {
         /* Advance audio tone sequencer (non-blocking, call every tick) */
         audio_tick(now);
 
+        /* Advance buzzer pattern sequencer (non-blocking, call every tick) */
+        buzzer_tick(now);
+
+        /* Advance LED animation sequencer (non-blocking, call every tick) */
+        led_tick(now);
+
+        /* Servo pan-tilt animation tick — updates smooth sweeps */
+        servo_tick(now);
+
         /* Sleep LED: software PWM on LED1 (active-low PC15) driven by PM brightness.
          * pm_pwm_phase rolls over each ms; brightness sets duty cycle 0-255. */
         pm_pwm_phase++;

src/servo.c

@@ -0,0 +1,242 @@
+#include "servo.h"
+#include "config.h"
+#include "stm32f7xx_hal.h"
+#include <string.h>
+
+/* ================================================================
+ * Servo PWM Protocol
+ * ================================================================
+ * TIM4 at 50 Hz (20 ms period)
+ * APB1 clock: 54 MHz
+ * Prescaler: 53 (54 MHz / 54 = 1 MHz)
+ * ARR: 19999 (1 MHz / 20000 = 50 Hz)
+ * CCR: 500-2500 (0.5-2.5 ms out of 20 ms)
+ *
+ * Servo pulse mapping:
+ *   500 µs  → 0°   (full left/down)
+ *   1500 µs → 90°  (center)
+ *   2500 µs → 180° (full right/up)
+ */
+
+#define SERVO_PWM_FREQ      50u            /* 50 Hz */
+#define SERVO_PERIOD_MS     20u            /* 20 ms = 1/50 Hz */
+#define SERVO_CLOCK_HZ      1000000u       /* 1 MHz timer clock */
+#define SERVO_PRESCALER     53u            /* APB1 54 MHz / 54 = 1 MHz */
+#define SERVO_ARR           19999u         /* 1 MHz / 20000 = 50 Hz */
+
+typedef struct {
+    uint16_t current_angle_deg[SERVO_COUNT];
+    uint16_t target_angle_deg[SERVO_COUNT];
+    uint16_t pulse_us[SERVO_COUNT];
+
+    /* Sweep state */
+    uint32_t sweep_start_ms[SERVO_COUNT];
+    uint32_t sweep_duration_ms[SERVO_COUNT];
+    uint16_t sweep_start_deg[SERVO_COUNT];
+    uint16_t sweep_end_deg[SERVO_COUNT];
+    bool     is_sweeping[SERVO_COUNT];
+} ServoState;
+
+static ServoState s_servo = {0};
+static TIM_HandleTypeDef s_tim_handle = {0};
+
+/* ================================================================
+ * Helper functions
+ * ================================================================
+ */
+
+static uint16_t angle_to_pulse_us(uint16_t degrees)
+{
+    /* Linear interpolation: 0° → 500µs, 180° → 2500µs */
+    if (degrees > 180) degrees = 180;
+
+    uint32_t pulse = SERVO_MIN_US + (uint32_t)degrees * (SERVO_MAX_US - SERVO_MIN_US) / 180;
+    return (uint16_t)pulse;
+}
+
+static uint16_t pulse_us_to_angle(uint16_t pulse_us)
+{
+    /* Inverse mapping: 500µs → 0°, 2500µs → 180° */
+    if (pulse_us < SERVO_MIN_US) pulse_us = SERVO_MIN_US;
+    if (pulse_us > SERVO_MAX_US) pulse_us = SERVO_MAX_US;
+
+    uint32_t angle = (uint32_t)(pulse_us - SERVO_MIN_US) * 180 / (SERVO_MAX_US - SERVO_MIN_US);
+    return (uint16_t)angle;
+}
+
+static void update_pwm(ServoChannel channel)
+{
+    /* Convert pulse width (500-2500 µs) to CCR value */
+    /* At 1 MHz timer clock: 1 µs = 1 count */
+    uint32_t ccr_value = s_servo.pulse_us[channel];
+
+    if (channel == SERVO_PAN) {
+        __HAL_TIM_SET_COMPARE(&s_tim_handle, SERVO_PAN_CHANNEL, ccr_value);
+    } else {
+        __HAL_TIM_SET_COMPARE(&s_tim_handle, SERVO_TILT_CHANNEL, ccr_value);
+    }
+}
+
+/* ================================================================
+ * Public API
+ * ================================================================
+ */
+
+void servo_init(void)
+{
+    /* Initialize state */
+    memset(&s_servo, 0, sizeof(s_servo));
+
+    /* Center both servos at 90° */
+    for (int i = 0; i < SERVO_COUNT; i++) {
+        s_servo.current_angle_deg[i] = 90;
+        s_servo.target_angle_deg[i] = 90;
+        s_servo.pulse_us[i] = SERVO_CENTER_US;
+    }
+
+    /* Configure GPIO PB6 (CH1) and PB7 (CH2) as TIM4 PWM */
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+
+    GPIO_InitTypeDef gpio_init = {0};
+    gpio_init.Mode      = GPIO_MODE_AF_PP;
+    gpio_init.Pull      = GPIO_NOPULL;
+    gpio_init.Speed     = GPIO_SPEED_FREQ_HIGH;
+    gpio_init.Alternate = SERVO_AF;
+
+    /* Configure PB6 (pan) */
+    gpio_init.Pin = SERVO_PAN_PIN;
+    HAL_GPIO_Init(SERVO_PAN_PORT, &gpio_init);
+
+    /* Configure PB7 (tilt) */
+    gpio_init.Pin = SERVO_TILT_PIN;
+    HAL_GPIO_Init(SERVO_TILT_PORT, &gpio_init);
+
+    /* Configure TIM4: 50 Hz PWM */
+    __HAL_RCC_TIM4_CLK_ENABLE();
+
+    s_tim_handle.Instance           = SERVO_TIM;
+    s_tim_handle.Init.Prescaler     = SERVO_PRESCALER;
+    s_tim_handle.Init.CounterMode   = TIM_COUNTERMODE_UP;
+    s_tim_handle.Init.Period        = SERVO_ARR;
+    s_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+    s_tim_handle.Init.RepetitionCounter = 0;
+
+    HAL_TIM_PWM_Init(&s_tim_handle);
+
+    /* Configure TIM4_CH1 (pan) for PWM */
+    TIM_OC_InitTypeDef oc_init = {0};
+    oc_init.OCMode     = TIM_OCMODE_PWM1;
+    oc_init.Pulse      = SERVO_CENTER_US;
+    oc_init.OCPolarity = TIM_OCPOLARITY_HIGH;
+    oc_init.OCFastMode = TIM_OCFAST_DISABLE;
+
+    HAL_TIM_PWM_ConfigChannel(&s_tim_handle, &oc_init, SERVO_PAN_CHANNEL);
+    HAL_TIM_PWM_Start(&s_tim_handle, SERVO_PAN_CHANNEL);
+
+    /* Configure TIM4_CH2 (tilt) for PWM */
+    oc_init.Pulse = SERVO_CENTER_US;
+    HAL_TIM_PWM_ConfigChannel(&s_tim_handle, &oc_init, SERVO_TILT_CHANNEL);
+    HAL_TIM_PWM_Start(&s_tim_handle, SERVO_TILT_CHANNEL);
+}
+
+void servo_set_angle(ServoChannel channel, uint16_t degrees)
+{
+    if (channel >= SERVO_COUNT) return;
+    if (degrees > 180) degrees = 180;
+
+    s_servo.current_angle_deg[channel] = degrees;
+    s_servo.target_angle_deg[channel] = degrees;
+    s_servo.pulse_us[channel] = angle_to_pulse_us(degrees);
+
+    /* Stop any sweep in progress */
+    s_servo.is_sweeping[channel] = false;
+
+    /* Update PWM immediately */
+    update_pwm(channel);
+}
+
+uint16_t servo_get_angle(ServoChannel channel)
+{
+    if (channel >= SERVO_COUNT) return 0;
+    return s_servo.current_angle_deg[channel];
+}
+
+void servo_set_pulse_us(ServoChannel channel, uint16_t pulse_us)
+{
+    if (channel >= SERVO_COUNT) return;
+    if (pulse_us < SERVO_MIN_US) pulse_us = SERVO_MIN_US;
+    if (pulse_us > SERVO_MAX_US) pulse_us = SERVO_MAX_US;
+
+    s_servo.pulse_us[channel] = pulse_us;
+    s_servo.current_angle_deg[channel] = pulse_us_to_angle(pulse_us);
+    s_servo.target_angle_deg[channel] = s_servo.current_angle_deg[channel];
+
+    /* Stop any sweep in progress */
+    s_servo.is_sweeping[channel] = false;
+
+    /* Update PWM immediately */
+    update_pwm(channel);
+}
+
+void servo_sweep(ServoChannel channel, uint16_t start_deg, uint16_t end_deg, uint32_t duration_ms)
+{
+    if (channel >= SERVO_COUNT) return;
+    if (duration_ms == 0) return;
+    if (start_deg > 180) start_deg = 180;
+    if (end_deg > 180) end_deg = 180;
+
+    s_servo.sweep_start_deg[channel] = start_deg;
+    s_servo.sweep_end_deg[channel] = end_deg;
+    s_servo.sweep_duration_ms[channel] = duration_ms;
+    s_servo.sweep_start_ms[channel] = 0;  /* Will be set on first tick */
+    s_servo.is_sweeping[channel] = true;
+}
+
+void servo_tick(uint32_t now_ms)
+{
+    for (int ch = 0; ch < SERVO_COUNT; ch++) {
+        if (!s_servo.is_sweeping[ch]) continue;
+
+        /* Initialize start time on first call */
+        if (s_servo.sweep_start_ms[ch] == 0) {
+            s_servo.sweep_start_ms[ch] = now_ms;
+        }
+
+        uint32_t elapsed = now_ms - s_servo.sweep_start_ms[ch];
+        uint32_t duration = s_servo.sweep_duration_ms[ch];
+
+        if (elapsed >= duration) {
+            /* Sweep complete */
+            s_servo.is_sweeping[ch] = false;
+            s_servo.current_angle_deg[ch] = s_servo.sweep_end_deg[ch];
+            s_servo.pulse_us[ch] = angle_to_pulse_us(s_servo.sweep_end_deg[ch]);
+        } else {
+            /* Linear interpolation */
+            int16_t start = (int16_t)s_servo.sweep_start_deg[ch];
+            int16_t end = (int16_t)s_servo.sweep_end_deg[ch];
+            int32_t delta = end - start;
+
+            /* angle = start + (delta * elapsed / duration) */
+            int32_t angle_i32 = start + (delta * (int32_t)elapsed / (int32_t)duration);
+            s_servo.current_angle_deg[ch] = (uint16_t)angle_i32;
+            s_servo.pulse_us[ch] = angle_to_pulse_us(s_servo.current_angle_deg[ch]);
+        }
+
+        /* Update PWM */
+        update_pwm((ServoChannel)ch);
+    }
+}
+
+bool servo_is_sweeping(void)
+{
+    for (int i = 0; i < SERVO_COUNT; i++) {
+        if (s_servo.is_sweeping[i]) return true;
+    }
+    return false;
+}
+
+void servo_stop_sweep(ServoChannel channel)
+{
+    if (channel >= SERVO_COUNT) return;
+    s_servo.is_sweeping[channel] = false;
+}

test/test_led.py

@@ -0,0 +1,344 @@
+"""
+test_led.py — WS2812B NeoPixel LED driver tests (Issue #193)
+
+Verifies in Python:
+  - State transitions: boot → armed, error, low_battery, charging, e_stop
+  - Animation timing: chase speed, blink/strobe frequency, pulse duration
+  - LED color encoding: RGB to GRB byte order, MSB-first bit encoding
+  - PWM duty values: bit "0" (~40%) and bit "1" (~56%) detection
+  - Animation sequencing: smooth transitions between states
+  - Sine wave lookup: breathing and pulse envelopes
+"""
+
+import pytest
+
+# ── Constants ─────────────────────────────────────────────────────────────
+
+NUM_LEDS = 8
+BITS_PER_LED = 24  # RGB = 8 bits each
+TOTAL_BITS = NUM_LEDS * BITS_PER_LED
+
+PWM_PERIOD = 270  # 216 MHz / 800 kHz ≈ 270 (integer approximation)
+BIT_0_DUTY = int(PWM_PERIOD * 40 / 100)  # ~108 (40%)
+BIT_1_DUTY = int(PWM_PERIOD * 56 / 100)  # ~151 (56%)
+
+# Animation periods (ms)
+BOOT_CHASE_MS = 100  # ms per LED rotation
+ERROR_BLINK_MS = 250
+ESTOP_STROBE_MS = 125
+PULSE_PERIOD_MS = 5120
+
+
+# ── RGB Color Utility ─────────────────────────────────────────────────────
+
+class RGBColor:
+    def __init__(self, r=0, g=0, b=0):
+        self.r = r
+        self.g = g
+        self.b = b
+
+    def __eq__(self, other):
+        return self.r == other.r and self.g == other.g and self.b == other.b
+
+    def __repr__(self):
+        return f"RGB({self.r},{self.g},{self.b})"
+
+
+# ── WS2812B Encoding Utilities ────────────────────────────────────────────
+
+def rgb_to_pwm_buffer(colors):
+    """Encode LED colors into PWM duty values (GRB byte order, MSB first)."""
+    pwm_buf = []
+
+    for color in colors:
+        # GRB byte order (WS2812 standard)
+        bytes_grb = [color.g, color.r, color.b]
+
+        for byte in bytes_grb:
+            for bit in range(7, -1, -1):
+                bit_val = (byte >> bit) & 1
+                pwm_buf.append(BIT_1_DUTY if bit_val else BIT_0_DUTY)
+
+    return pwm_buf
+
+
+def pwm_buffer_to_rgb(pwm_buf):
+    """Decode PWM duty values back to RGB colors (for verification)."""
+    colors = []
+
+    for led_idx in range(NUM_LEDS):
+        base = led_idx * BITS_PER_LED
+        # GRB byte order
+        g = bytes_from_bits(pwm_buf[base : base + 8])
+        r = bytes_from_bits(pwm_buf[base + 8 : base + 16])
+        b = bytes_from_bits(pwm_buf[base + 16 : base + 24])
+
+        colors.append(RGBColor(r, g, b))
+
+    return colors
+
+
+def bytes_from_bits(pwm_values):
+    """Reconstruct a byte from PWM duty values."""
+    byte = 0
+    for pwm in pwm_values:
+        byte = (byte << 1) | (1 if pwm > (BIT_0_DUTY + BIT_1_DUTY) // 2 else 0)
+    return byte
+
+
+# ── Sine Lookup ───────────────────────────────────────────────────────────
+
+def sin_u8(phase):
+    """Approximate sine wave (0-255) from phase (0-255)."""
+    # Simplified lookup (matching C implementation)
+    sine_lut = [
+        128, 131, 134, 137, 140, 143, 146, 149, 152, 155, 158, 161, 164, 167, 170, 173,
+        176, 179, 182, 185, 188, 191, 193, 196, 199, 201, 204, 206, 209, 211, 214, 216,
+        218, 221, 223, 225, 227, 229, 231, 233, 235, 236, 238, 240, 241, 243, 244, 245,
+        247, 248, 249, 250, 251, 252, 252, 253, 254, 254, 255, 255, 255, 255, 255, 254,
+        254, 253, 252, 252, 251, 250, 249, 248, 247, 245, 244, 243, 241, 240, 238, 236,
+        235, 233, 231, 229, 227, 225, 223, 221, 218, 216, 214, 211, 209, 206, 204, 201,
+        199, 196, 193, 191, 188, 185, 182, 179, 176, 173, 170, 167, 164, 161, 158, 155,
+        152, 149, 146, 143, 140, 137, 134, 131, 128, 125, 122, 119, 116, 113, 110, 107,
+        104, 101, 98, 95, 92, 89, 86, 83, 80, 77, 74, 71, 68, 65, 62, 59,
+        56, 53, 50, 47, 44, 41, 39, 36, 33, 31, 28, 26, 23, 21, 18, 16,
+        14, 11, 9, 7, 5, 3, 1, 0,
+    ]
+    return sine_lut[phase % 256] if phase < len(sine_lut) else sine_lut[255]
+
+
+# ── LED State Machine Simulator ───────────────────────────────────────────
+
+class LEDSimulator:
+    def __init__(self):
+        self.leds = [RGBColor() for _ in range(NUM_LEDS)]
+        self.pwm_buf = [0] * TOTAL_BITS
+        self.current_state = 'BOOT'
+        self.next_state = 'BOOT'
+        self.state_start_ms = 0
+
+    def set_state(self, state):
+        self.next_state = state
+
+    def tick(self, now_ms):
+        # State transition
+        if self.next_state != self.current_state:
+            self.current_state = self.next_state
+            self.state_start_ms = now_ms
+
+        elapsed = now_ms - self.state_start_ms
+
+        # Run animation
+        if self.current_state == 'BOOT':
+            self._animate_boot(elapsed)
+        elif self.current_state == 'ARMED':
+            self._animate_armed()
+        elif self.current_state == 'ERROR':
+            self._animate_error(elapsed)
+        elif self.current_state == 'LOW_BATT':
+            self._animate_low_battery(elapsed)
+        elif self.current_state == 'CHARGING':
+            self._animate_charging(elapsed)
+        elif self.current_state == 'ESTOP':
+            self._animate_estop(elapsed)
+
+        # Encode to PWM buffer
+        self.pwm_buf = rgb_to_pwm_buffer(self.leds)
+
+    def _animate_boot(self, elapsed):
+        for i in range(NUM_LEDS):
+            self.leds[i] = RGBColor()
+        led_idx = (elapsed // BOOT_CHASE_MS) % NUM_LEDS
+        self.leds[led_idx] = RGBColor(b=255)
+
+    def _animate_armed(self):
+        for i in range(NUM_LEDS):
+            self.leds[i] = RGBColor(g=200)
+
+    def _animate_error(self, elapsed):
+        on = ((elapsed // ERROR_BLINK_MS) % 2) == 0
+        for i in range(NUM_LEDS):
+            self.leds[i] = RGBColor(r=255 if on else 0)
+
+    def _animate_low_battery(self, elapsed):
+        phase = (elapsed // 20) & 0xFF
+        brightness = sin_u8(phase)
+        val = (brightness * 255) >> 8
+        for i in range(NUM_LEDS):
+            self.leds[i] = RGBColor(r=val, g=val)
+
+    def _animate_charging(self, elapsed):
+        phase = (elapsed // 20) & 0xFF
+        brightness = sin_u8(phase)
+        val = (brightness * 255) >> 8
+        for i in range(NUM_LEDS):
+            self.leds[i] = RGBColor(g=val)
+
+    def _animate_estop(self, elapsed):
+        on = ((elapsed // ESTOP_STROBE_MS) % 2) == 0
+        for i in range(NUM_LEDS):
+            self.leds[i] = RGBColor(r=255 if on else 0)
+
+
+# ── Tests ──────────────────────────────────────────────────────────────────
+
+def test_state_transitions():
+    """LED state should transition correctly."""
+    sim = LEDSimulator()
+    assert sim.current_state == 'BOOT'
+
+    sim.set_state('ARMED')
+    sim.tick(0)
+    assert sim.current_state == 'ARMED'
+
+    sim.set_state('ERROR')
+    sim.tick(1)
+    assert sim.current_state == 'ERROR'
+
+
+def test_boot_chase_timing():
+    """Boot state: LED should rotate every 100 ms."""
+    sim = LEDSimulator()
+    sim.set_state('BOOT')
+
+    # t=0: LED 0 should be blue
+    sim.tick(0)
+    assert sim.leds[0].b > 0
+    for i in range(1, NUM_LEDS):
+        assert sim.leds[i].b == 0
+
+    # t=100: LED 1 should be blue
+    sim.tick(100)
+    assert sim.leds[1].b > 0
+    for i in range(NUM_LEDS):
+        if i != 1:
+            assert sim.leds[i].b == 0
+
+
+def test_armed_solid_green():
+    """Armed state: all LEDs should be solid green."""
+    sim = LEDSimulator()
+    sim.set_state('ARMED')
+    sim.tick(0)
+
+    for led in sim.leds:
+        assert led.g > 0
+        assert led.r == 0
+        assert led.b == 0
+
+
+def test_error_blinking():
+    """Error state: LEDs should blink red every 250 ms."""
+    sim = LEDSimulator()
+    sim.set_state('ERROR')
+
+    # t=0-249: red on
+    sim.tick(0)
+    for led in sim.leds:
+        assert led.r > 0
+
+    # t=250-499: red off
+    sim.tick(250)
+    for led in sim.leds:
+        assert led.r == 0
+
+    # t=500-749: red on again
+    sim.tick(500)
+    for led in sim.leds:
+        assert led.r > 0
+
+
+def test_low_battery_pulsing():
+    """Low battery: LEDs should pulse yellow with sine envelope."""
+    sim = LEDSimulator()
+    sim.set_state('LOW_BATT')
+
+    # Sample at different points
+    sim.tick(0)
+    v0 = sim.leds[0].r
+
+    sim.tick(1280)  # Quarter period
+    v1 = sim.leds[0].r
+
+    assert v1 > v0  # Should increase from bottom of sine
+
+
+def test_charging_breathing():
+    """Charging: LEDs should breathe green smoothly."""
+    sim = LEDSimulator()
+    sim.set_state('CHARGING')
+
+    # Sample at different points
+    sim.tick(0)
+    v0 = sim.leds[0].g
+
+    sim.tick(1280)  # Quarter period
+    v1 = sim.leds[0].g
+
+    assert v1 > v0  # Should increase
+
+
+def test_estop_strobe():
+    """E-stop: LEDs should strobe red at 8 Hz (125 ms on/off)."""
+    sim = LEDSimulator()
+    sim.set_state('ESTOP')
+
+    # t=0-124: strobe on
+    sim.tick(0)
+    for led in sim.leds:
+        assert led.r > 0
+
+    # t=125-249: strobe off
+    sim.tick(125)
+    for led in sim.leds:
+        assert led.r == 0
+
+
+def test_pwm_duty_encoding():
+    """PWM duty values should encode RGB correctly (GRB, MSB-first)."""
+    colors = [
+        RGBColor(255, 0, 0),  # Red
+        RGBColor(0, 255, 0),  # Green
+        RGBColor(0, 0, 255),  # Blue
+        RGBColor(255, 255, 255),  # White
+    ]
+
+    # Encode to PWM
+    pwm_buf = rgb_to_pwm_buffer(colors + [RGBColor()] * (NUM_LEDS - 4))
+
+    # Verify PWM buffer has correct length
+    assert len(pwm_buf) == TOTAL_BITS
+
+    # Verify bit values are either 0-duty or 1-duty
+    for pwm in pwm_buf:
+        assert pwm == BIT_0_DUTY or pwm == BIT_1_DUTY
+
+
+def test_color_roundtrip():
+    """Colors should survive encode/decode roundtrip."""
+    original = [
+        RGBColor(100, 150, 200),
+        RGBColor(0, 255, 0),
+        RGBColor(255, 0, 0),
+    ] + [RGBColor()] * (NUM_LEDS - 3)
+
+    pwm_buf = rgb_to_pwm_buffer(original)
+    decoded = pwm_buffer_to_rgb(pwm_buf)
+
+    for i in range(NUM_LEDS):
+        assert decoded[i] == original[i]
+
+
+def test_multiple_state_transitions():
+    """Simulate state transitions over time."""
+    sim = LEDSimulator()
+
+    states = ['BOOT', 'ARMED', 'ERROR', 'LOW_BATT', 'CHARGING', 'ESTOP']
+    for state_name in states:
+        sim.set_state(state_name)
+        sim.tick(0)
+        assert sim.current_state == state_name
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])

test/test_servo.py

@@ -0,0 +1,345 @@
+"""
+test_servo.py — Pan-tilt servo driver tests (Issue #206)
+
+Verifies:
+  - PWM frequency: 50 Hz (20 ms period)
+  - Pulse width: 500-2500 µs for 0-180°
+  - Angle conversion: linear mapping
+  - Smooth sweeping: animation timing and interpolation
+  - Multi-servo coordination (pan + tilt independently)
+"""
+
+import pytest
+
+# ── Constants ─────────────────────────────────────────────────────────────
+
+SERVO_MIN_US = 500
+SERVO_MAX_US = 2500
+SERVO_CENTER_US = 1500
+
+PWM_FREQ_HZ = 50
+PERIOD_MS = 20
+
+NUM_SERVOS = 2
+SERVO_PAN = 0
+SERVO_TILT = 1
+
+
+# ── Servo Simulator ────────────────────────────────────────────────────────
+
+class ServoSimulator:
+    def __init__(self):
+        self.current_angle_deg = [90, 90]  # Both centered
+        self.pulse_us = [SERVO_CENTER_US, SERVO_CENTER_US]
+        self.is_sweeping = [False, False]
+        self.sweep_start_deg = [0, 0]
+        self.sweep_end_deg = [0, 0]
+        self.sweep_duration_ms = [0, 0]
+        self.sweep_start_ms = [None, None]
+
+    def angle_to_pulse(self, degrees):
+        """Convert angle (0-180) to pulse width (500-2500 µs)."""
+        if degrees < 0:
+            degrees = 0
+        if degrees > 180:
+            degrees = 180
+        return SERVO_MIN_US + (degrees * (SERVO_MAX_US - SERVO_MIN_US)) // 180
+
+    def pulse_to_angle(self, pulse_us):
+        """Convert pulse width to angle."""
+        if pulse_us < SERVO_MIN_US:
+            pulse_us = SERVO_MIN_US
+        if pulse_us > SERVO_MAX_US:
+            pulse_us = SERVO_MAX_US
+        return (pulse_us - SERVO_MIN_US) * 180 // (SERVO_MAX_US - SERVO_MIN_US)
+
+    def set_angle(self, channel, degrees):
+        """Immediately set servo angle."""
+        self.current_angle_deg[channel] = min(180, max(0, degrees))
+        self.pulse_us[channel] = self.angle_to_pulse(self.current_angle_deg[channel])
+        self.is_sweeping[channel] = False
+
+    def get_angle(self, channel):
+        """Get current servo angle."""
+        return self.current_angle_deg[channel]
+
+    def set_pulse_us(self, channel, pulse_us):
+        """Set servo by pulse width."""
+        if pulse_us < SERVO_MIN_US:
+            pulse_us = SERVO_MIN_US
+        if pulse_us > SERVO_MAX_US:
+            pulse_us = SERVO_MAX_US
+        self.pulse_us[channel] = pulse_us
+        self.current_angle_deg[channel] = self.pulse_to_angle(pulse_us)
+        self.is_sweeping[channel] = False
+
+    def sweep(self, channel, start_deg, end_deg, duration_ms):
+        """Start smooth sweep."""
+        self.sweep_start_deg[channel] = start_deg
+        self.sweep_end_deg[channel] = end_deg
+        self.sweep_duration_ms[channel] = duration_ms
+        self.sweep_start_ms[channel] = None
+        self.is_sweeping[channel] = True
+
+    def tick(self, now_ms):
+        """Update sweep animations."""
+        for ch in range(NUM_SERVOS):
+            if not self.is_sweeping[ch]:
+                continue
+
+            # Initialize start time on first call
+            if self.sweep_start_ms[ch] is None:
+                self.sweep_start_ms[ch] = now_ms
+
+            elapsed = now_ms - self.sweep_start_ms[ch]
+            duration = self.sweep_duration_ms[ch]
+
+            if elapsed >= duration:
+                # Sweep complete
+                self.is_sweeping[ch] = False
+                self.current_angle_deg[ch] = self.sweep_end_deg[ch]
+                self.pulse_us[ch] = self.angle_to_pulse(self.sweep_end_deg[ch])
+            else:
+                # Linear interpolation
+                start = self.sweep_start_deg[ch]
+                end = self.sweep_end_deg[ch]
+                delta = end - start
+                angle = start + (delta * elapsed) // duration
+                self.current_angle_deg[ch] = angle
+                self.pulse_us[ch] = self.angle_to_pulse(angle)
+
+    def is_sweeping_any(self):
+        """Check if any servo is sweeping."""
+        return any(self.is_sweeping)
+
+
+# ── Tests ──────────────────────────────────────────────────────────────────
+
+def test_initialization():
+    """Servos should initialize centered at 90°."""
+    sim = ServoSimulator()
+    assert sim.get_angle(SERVO_PAN) == 90
+    assert sim.get_angle(SERVO_TILT) == 90
+    assert sim.pulse_us[SERVO_PAN] == SERVO_CENTER_US
+    assert sim.pulse_us[SERVO_TILT] == SERVO_CENTER_US
+
+
+def test_angle_to_pulse_conversion():
+    """Angle to pulse conversion should be linear."""
+    sim = ServoSimulator()
+
+    assert sim.angle_to_pulse(0) == SERVO_MIN_US      # 500 µs
+    assert sim.angle_to_pulse(90) == SERVO_CENTER_US  # 1500 µs
+    assert sim.angle_to_pulse(180) == SERVO_MAX_US    # 2500 µs
+
+    # Intermediate angles
+    assert sim.angle_to_pulse(45) == 1000   # 0.5 way: 500 + 500 = 1000
+    assert sim.angle_to_pulse(135) == 2000  # 0.75 way: 500 + 1500 = 2000
+
+
+def test_pulse_to_angle_conversion():
+    """Pulse to angle conversion should invert angle_to_pulse."""
+    sim = ServoSimulator()
+
+    assert sim.pulse_to_angle(SERVO_MIN_US) == 0
+    assert sim.pulse_to_angle(SERVO_CENTER_US) == 90
+    assert sim.pulse_to_angle(SERVO_MAX_US) == 180
+
+    # Intermediate pulses
+    assert sim.pulse_to_angle(1000) == 45
+    assert sim.pulse_to_angle(2000) == 135
+
+
+def test_set_angle_pan():
+    """Pan servo should update angle immediately."""
+    sim = ServoSimulator()
+
+    sim.set_angle(SERVO_PAN, 0)
+    assert sim.get_angle(SERVO_PAN) == 0
+    assert sim.pulse_us[SERVO_PAN] == SERVO_MIN_US
+
+    sim.set_angle(SERVO_PAN, 90)
+    assert sim.get_angle(SERVO_PAN) == 90
+    assert sim.pulse_us[SERVO_PAN] == SERVO_CENTER_US
+
+    sim.set_angle(SERVO_PAN, 180)
+    assert sim.get_angle(SERVO_PAN) == 180
+    assert sim.pulse_us[SERVO_PAN] == SERVO_MAX_US
+
+
+def test_set_angle_tilt():
+    """Tilt servo should work independently."""
+    sim = ServoSimulator()
+
+    sim.set_angle(SERVO_TILT, 45)
+    assert sim.get_angle(SERVO_TILT) == 45
+    assert sim.get_angle(SERVO_PAN) == 90  # Pan unchanged
+
+
+def test_set_pulse_us():
+    """Pulse width setter should update angle correctly."""
+    sim = ServoSimulator()
+
+    sim.set_pulse_us(SERVO_PAN, SERVO_MIN_US)
+    assert sim.get_angle(SERVO_PAN) == 0
+
+    sim.set_pulse_us(SERVO_PAN, SERVO_CENTER_US)
+    assert sim.get_angle(SERVO_PAN) == 90
+
+    sim.set_pulse_us(SERVO_PAN, SERVO_MAX_US)
+    assert sim.get_angle(SERVO_PAN) == 180
+
+
+def test_sweep_timing():
+    """Sweep should complete in specified duration."""
+    sim = ServoSimulator()
+
+    # Pan from 0° to 180° over 2 seconds
+    sim.sweep(SERVO_PAN, 0, 180, 2000)
+
+    # Initial tick
+    sim.tick(0)
+    assert sim.get_angle(SERVO_PAN) == 0
+
+    # Halfway through sweep (t=1000ms)
+    sim.tick(1000)
+    assert sim.get_angle(SERVO_PAN) == 90  # Linear interpolation
+
+    # End of sweep (t=2000ms)
+    sim.tick(2000)
+    assert sim.get_angle(SERVO_PAN) == 180
+    assert not sim.is_sweeping[SERVO_PAN]
+
+
+def test_sweep_interpolation():
+    """Sweep should interpolate smoothly."""
+    sim = ServoSimulator()
+
+    # Sweep from 0° to 180° in 1000ms
+    sim.sweep(SERVO_PAN, 0, 180, 1000)
+
+    angles = []
+    for t in range(0, 1001, 100):
+        sim.tick(t)
+        angles.append(sim.get_angle(SERVO_PAN))
+
+    # Expected: [0, 18, 36, 54, 72, 90, 108, 126, 144, 162, 180]
+    expected = [i * 18 for i in range(11)]
+    assert angles == expected, f"Got {angles}, expected {expected}"
+
+
+def test_reverse_sweep():
+    """Sweep from higher angle to lower angle."""
+    sim = ServoSimulator()
+
+    sim.sweep(SERVO_TILT, 180, 0, 1000)
+
+    sim.tick(0)
+    assert sim.get_angle(SERVO_TILT) == 180
+
+    sim.tick(500)
+    assert sim.get_angle(SERVO_TILT) == 90
+
+    sim.tick(1000)
+    assert sim.get_angle(SERVO_TILT) == 0
+    assert not sim.is_sweeping[SERVO_TILT]
+
+
+def test_sweep_stops_on_immediate_set():
+    """Setting angle immediately should stop sweep."""
+    sim = ServoSimulator()
+
+    sim.sweep(SERVO_PAN, 0, 180, 2000)
+    sim.tick(500)
+
+    # Stop sweep by setting angle
+    sim.set_angle(SERVO_PAN, 45)
+    assert not sim.is_sweeping[SERVO_PAN]
+    assert sim.get_angle(SERVO_PAN) == 45
+
+
+def test_independent_servos():
+    """Pan and tilt servos should sweep independently."""
+    sim = ServoSimulator()
+
+    sim.sweep(SERVO_PAN, 0, 180, 1000)
+    sim.sweep(SERVO_TILT, 180, 0, 2000)
+
+    # Initialize sweep timing
+    sim.tick(0)
+
+    # After 1 second
+    sim.tick(1000)
+    assert sim.get_angle(SERVO_PAN) == 180
+    assert not sim.is_sweeping[SERVO_PAN]
+    assert sim.get_angle(SERVO_TILT) == 90  # Halfway through
+    assert sim.is_sweeping[SERVO_TILT]
+
+    # After 2 seconds
+    sim.tick(2000)
+    assert not sim.is_sweeping[SERVO_PAN]
+    assert sim.get_angle(SERVO_TILT) == 0
+    assert not sim.is_sweeping[SERVO_TILT]
+    assert not sim.is_sweeping_any()
+
+
+def test_fast_sweep():
+    """Very short sweep should work."""
+    sim = ServoSimulator()
+
+    sim.sweep(SERVO_PAN, 45, 135, 100)  # 90° in 100ms
+
+    sim.tick(0)
+    assert sim.get_angle(SERVO_PAN) == 45
+
+    sim.tick(50)
+    assert sim.get_angle(SERVO_PAN) == 90
+
+    sim.tick(100)
+    assert sim.get_angle(SERVO_PAN) == 135
+    assert not sim.is_sweeping[SERVO_PAN]
+
+
+def test_multiple_sweeps():
+    """Multiple sequential sweeps should work."""
+    sim = ServoSimulator()
+
+    # First sweep (0° to 90° in 500ms)
+    sim.sweep(SERVO_PAN, 0, 90, 500)
+    sim.tick(0)
+    sim.tick(500)
+    assert sim.get_angle(SERVO_PAN) == 90
+    assert not sim.is_sweeping[SERVO_PAN]
+
+    # Second sweep (90° to 0° in 500ms, starting at t=500)
+    sim.sweep(SERVO_PAN, 90, 0, 500)
+    sim.tick(500)  # Initialize second sweep
+    sim.tick(1000)  # After 500ms of second sweep
+    assert sim.get_angle(SERVO_PAN) == 0
+    assert not sim.is_sweeping[SERVO_PAN]
+
+
+def test_boundary_angles():
+    """Angles > 180° should clamp to 180°."""
+    sim = ServoSimulator()
+
+    sim.set_angle(SERVO_PAN, 200)
+    assert sim.get_angle(SERVO_PAN) == 180
+
+    sim.set_angle(SERVO_PAN, -10)
+    assert sim.get_angle(SERVO_PAN) == 0
+
+
+def test_pulse_clamping():
+    """Pulse widths outside 500-2500 µs should clamp."""
+    sim = ServoSimulator()
+
+    sim.set_pulse_us(SERVO_PAN, 100)  # Too low
+    assert sim.pulse_us[SERVO_PAN] == SERVO_MIN_US
+
+    sim.set_pulse_us(SERVO_PAN, 3000)  # Too high
+    assert sim.pulse_us[SERVO_PAN] == SERVO_MAX_US
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])

ui/social-bot/src/App.jsx

@@ -47,6 +47,9 @@ import { SettingsPanel } from './components/SettingsPanel.jsx';
 // Camera viewer (issue #177)
 import { CameraViewer } from './components/CameraViewer.jsx';
 
+// Event log (issue #192)
+import { EventLog } from './components/EventLog.jsx';
+
 const TAB_GROUPS = [
   {
     label: 'SOCIAL',
@@ -80,6 +83,13 @@ const TAB_GROUPS = [
       { id: 'missions', label: 'Missions' },
     ],
   },
+  {
+    label: 'MONITORING',
+    color: 'text-yellow-600',
+    tabs: [
+      { id: 'eventlog', label: 'Events' },
+    ],
+  },
   {
     label: 'CONFIG',
     color: 'text-purple-600',
@@ -200,7 +210,7 @@ export default function App() {
       </nav>
 
       {/* ── Content ── */}
-      <main className="flex-1 overflow-y-auto p-4">
+      <main className={`flex-1 ${activeTab === 'eventlog' ? 'flex flex-col' : 'overflow-y-auto'} p-4`}>
         {activeTab === 'status'       && <StatusPanel     subscribe={subscribe} />}
         {activeTab === 'faces'        && <FaceGallery     subscribe={subscribe} callService={callService} />}
         {activeTab === 'conversation' && <ConversationLog subscribe={subscribe} />}
@@ -218,6 +228,8 @@ export default function App() {
         {activeTab === 'fleet'    && <FleetPanel />}
         {activeTab === 'missions' && <MissionPlanner />}
 
+        {activeTab === 'eventlog' && <EventLog subscribe={subscribe} />}
+
         {activeTab === 'settings' && <SettingsPanel subscribe={subscribe} callService={callService} connected={connected} wsUrl={wsUrl} />}
       </main>
 

ui/social-bot/src/components/EventLog.jsx

@@ -0,0 +1,290 @@
+/**
+ * EventLog.jsx — Robot event log viewer
+ *
+ * Displays timestamped, color-coded event cards from:
+ *   /saltybot/emergency        (emergency events)
+ *   /saltybot/docking_status   (docking state changes)
+ *   /diagnostics               (system diagnostics)
+ *
+ * Features:
+ *   - Real-time event streaming
+ *   - Color-coded by event type (red=emergency, blue=docking, cyan=diagnostics)
+ *   - Filter by type
+ *   - Auto-scroll to latest event
+ *   - Configurable max event history (default 200)
+ */
+
+import { useEffect, useRef, useState } from 'react';
+
+const EVENT_TYPES = {
+  EMERGENCY: 'emergency',
+  DOCKING: 'docking',
+  DIAGNOSTIC: 'diagnostic',
+};
+
+const EVENT_COLORS = {
+  emergency: { bg: 'bg-red-950', border: 'border-red-800', text: 'text-red-400', label: 'Emergency' },
+  docking: { bg: 'bg-blue-950', border: 'border-blue-800', text: 'text-blue-400', label: 'Docking' },
+  diagnostic: { bg: 'bg-cyan-950', border: 'border-cyan-800', text: 'text-cyan-400', label: 'Diagnostic' },
+};
+
+const MAX_EVENTS = 200;
+
+function formatTimestamp(ts) {
+  const date = new Date(ts);
+  return date.toLocaleTimeString('en-US', { hour12: false, hour: '2-digit', minute: '2-digit', second: '2-digit' });
+}
+
+function EventCard({ event, colors }) {
+  return (
+    <div className={`rounded border ${colors.border} ${colors.bg} p-3 text-sm space-y-1`}>
+      <div className="flex justify-between items-start gap-2">
+        <span className={`font-bold tracking-widest text-xs ${colors.text}`}>
+          {colors.label}
+        </span>
+        <span className="text-gray-600 text-xs flex-shrink-0">
+          {formatTimestamp(event.timestamp)}
+        </span>
+      </div>
+      <div className="text-gray-300 break-words">
+        {event.message}
+      </div>
+      {event.details && (
+        <div className="text-gray-500 text-xs font-mono pt-1 border-t border-gray-800">
+          {typeof event.details === 'string' ? (
+            event.details
+          ) : (
+            <pre className="overflow-x-auto">{JSON.stringify(event.details, null, 2)}</pre>
+          )}
+        </div>
+      )}
+    </div>
+  );
+}
+
+export function EventLog({ subscribe }) {
+  const [events, setEvents] = useState([]);
+  const [selectedTypes, setSelectedTypes] = useState(new Set(Object.values(EVENT_TYPES)));
+  const [expandedEventId, setExpandedEventId] = useState(null);
+  const scrollRef = useRef(null);
+  const eventIdRef = useRef(0);
+
+  // Auto-scroll to bottom when new events arrive
+  useEffect(() => {
+    if (scrollRef.current && events.length > 0) {
+      setTimeout(() => {
+        scrollRef.current?.scrollIntoView({ behavior: 'smooth', block: 'end' });
+      }, 0);
+    }
+  }, [events.length]);
+
+  // Subscribe to emergency events
+  useEffect(() => {
+    const unsub = subscribe(
+      '/saltybot/emergency',
+      'std_msgs/String',
+      (msg) => {
+        try {
+          const data = typeof msg.data === 'string' ? JSON.parse(msg.data) : msg.data;
+          setEvents((prev) => [
+            ...prev,
+            {
+              id: ++eventIdRef.current,
+              type: EVENT_TYPES.EMERGENCY,
+              timestamp: Date.now(),
+              message: data.message || data.status || JSON.stringify(data),
+              details: data,
+            },
+          ].slice(-MAX_EVENTS));
+        } catch (e) {
+          setEvents((prev) => [
+            ...prev,
+            {
+              id: ++eventIdRef.current,
+              type: EVENT_TYPES.EMERGENCY,
+              timestamp: Date.now(),
+              message: msg.data || 'Unknown emergency event',
+              details: null,
+            },
+          ].slice(-MAX_EVENTS));
+        }
+      }
+    );
+    return unsub;
+  }, [subscribe]);
+
+  // Subscribe to docking status
+  useEffect(() => {
+    const unsub = subscribe(
+      '/saltybot/docking_status',
+      'std_msgs/String',
+      (msg) => {
+        try {
+          const data = typeof msg.data === 'string' ? JSON.parse(msg.data) : msg.data;
+          const statusMsg = data.status || data.state || data.message || JSON.stringify(data);
+          setEvents((prev) => [
+            ...prev,
+            {
+              id: ++eventIdRef.current,
+              type: EVENT_TYPES.DOCKING,
+              timestamp: Date.now(),
+              message: `Docking Status: ${statusMsg}`,
+              details: data,
+            },
+          ].slice(-MAX_EVENTS));
+        } catch (e) {
+          setEvents((prev) => [
+            ...prev,
+            {
+              id: ++eventIdRef.current,
+              type: EVENT_TYPES.DOCKING,
+              timestamp: Date.now(),
+              message: `Docking Status: ${msg.data || 'Unknown'}`,
+              details: null,
+            },
+          ].slice(-MAX_EVENTS));
+        }
+      }
+    );
+    return unsub;
+  }, [subscribe]);
+
+  // Subscribe to diagnostics
+  useEffect(() => {
+    const unsub = subscribe(
+      '/diagnostics',
+      'diagnostic_msgs/DiagnosticArray',
+      (msg) => {
+        try {
+          for (const status of msg.status ?? []) {
+            if (status.level > 0) {
+              // Only log warnings and errors
+              const kv = {};
+              for (const pair of status.values ?? []) {
+                kv[pair.key] = pair.value;
+              }
+              setEvents((prev) => [
+                ...prev,
+                {
+                  id: ++eventIdRef.current,
+                  type: EVENT_TYPES.DIAGNOSTIC,
+                  timestamp: Date.now(),
+                  message: `${status.name}: ${status.message}`,
+                  details: kv,
+                },
+              ].slice(-MAX_EVENTS));
+            }
+          }
+        } catch (e) {
+          // Ignore parsing errors
+        }
+      }
+    );
+    return unsub;
+  }, [subscribe]);
+
+  const filteredEvents = events.filter((event) => selectedTypes.has(event.type));
+
+  const toggleEventType = (type) => {
+    setSelectedTypes((prev) => {
+      const next = new Set(prev);
+      if (next.has(type)) {
+        next.delete(type);
+      } else {
+        next.add(type);
+      }
+      return next;
+    });
+  };
+
+  const clearEvents = () => {
+    setEvents([]);
+    eventIdRef.current = 0;
+  };
+
+  return (
+    <div className="flex flex-col h-full space-y-3">
+      {/* Controls */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4 space-y-3">
+        <div className="flex justify-between items-center flex-wrap gap-2">
+          <div className="text-cyan-700 text-xs font-bold tracking-widest">
+            EVENT LOG
+          </div>
+          <div className="text-gray-600 text-xs">
+            {filteredEvents.length} of {events.length} events
+          </div>
+        </div>
+
+        {/* Filter buttons */}
+        <div className="flex gap-2 flex-wrap">
+          {Object.entries(EVENT_COLORS).map(([typeKey, colors]) => (
+            <button
+              key={typeKey}
+              onClick={() => toggleEventType(typeKey)}
+              className={`px-3 py-1.5 text-xs font-bold tracking-widest rounded border transition-colors ${
+                selectedTypes.has(typeKey)
+                  ? `${colors.bg} ${colors.border} ${colors.text}`
+                  : 'bg-gray-900 border-gray-800 text-gray-600 hover:text-gray-400'
+              }`}
+            >
+              {colors.label}
+            </button>
+          ))}
+          <button
+            onClick={clearEvents}
+            className="ml-auto px-3 py-1.5 text-xs font-bold tracking-widest rounded border border-gray-800 text-gray-600 hover:text-red-400 hover:border-red-800 transition-colors"
+          >
+            CLEAR
+          </button>
+        </div>
+      </div>
+
+      {/* Event list */}
+      <div className="flex-1 overflow-y-auto space-y-2 pr-2">
+        {filteredEvents.length > 0 ? (
+          <>
+            {filteredEvents.map((event) => {
+              const colors = EVENT_COLORS[event.type];
+              return (
+                <div
+                  key={event.id}
+                  onClick={() =>
+                    setExpandedEventId(expandedEventId === event.id ? null : event.id)
+                  }
+                  className="cursor-pointer hover:opacity-80 transition-opacity"
+                >
+                  <EventCard event={event} colors={colors} />
+                </div>
+              );
+            })}
+            <div ref={scrollRef} />
+          </>
+        ) : (
+          <div className="text-center py-12 text-gray-600 text-sm">
+            {events.length === 0 ? (
+              <>
+                <div>No events yet</div>
+                <div className="text-xs text-gray-700 mt-2">
+                  Waiting for events from emergency, docking, and diagnostics topics…
+                </div>
+              </>
+            ) : (
+              <>
+                <div>No events match selected filter</div>
+                <div className="text-xs text-gray-700 mt-2">
+                  {events.length} events available, adjust filters above
+                </div>
+              </>
+            )}
+          </div>
+        )}
+      </div>
+
+      {/* Stats footer */}
+      <div className="bg-gray-950 rounded border border-gray-800 p-2 text-xs text-gray-600 flex justify-between">
+        <span>Displaying {filteredEvents.length} / {events.length} events</span>
+        <span className="text-gray-700">Max capacity: {MAX_EVENTS}</span>
+      </div>
+    </div>
+  );
+}