Merge pull request 'feat(webui): live camera viewer — multi-stream + detection overlays (Issue #177)' (#182) from sl-webui/issue-177-camera-viewer into main

UI (src/hooks/useCamera.js, src/components/CameraViewer.jsx):
  - 7 camera sources: front/left/rear/right CSI, D435i RGB/depth, panoramic
  - Compressed image subscription via rosbridge (sensor_msgs/CompressedImage)
  - Client-side 15fps gate (drops excess frames, reduces JS pressure)
  - Per-camera FPS indicator with quality badge (FULL/GOOD/LOW/NO SIGNAL)
  - Detection overlays: face boxes + names (/social/faces/detections),
    gesture icons (/social/gestures), scene object labels + hazard colours
    (/social/scene/objects); overlay mode selector (off/faces/gestures/objects/all)
  - 360° panoramic equirect viewer with mouse/touch drag azimuth pan
  - Picture-in-picture: up to 3 pinned cameras via ⊕ button
  - One-click recording (MediaRecorder → MP4/WebM download)
  - Snapshot to PNG with detection overlay composite + timestamp watermark
  - Cameras tab added to TELEMETRY group in App.jsx

Jetson (rosbridge bringup):
  - rosbridge_params.yaml: whitelist + /camera/depth/image_rect_raw/compressed,
    /camera/panoramic/compressed, /social/faces/detections,
    /social/gestures, /social/scene/objects
  - rosbridge.launch.py: D435i colour republisher (JPEG 75%) +
    depth republisher (compressedDepth/PNG16 preserving uint16 values)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

chassis/payload_bay_BOM.md

@@ -0,0 +1,165 @@
+# Payload Bay BOM — Issue #170
+**Agent:** sl-mechanical | **Date:** 2026-03-01
+
+Modular dovetail payload rail system. Tool-free slide-and-click module swapping.
+Cross-variant: SaltyLab, SaltyRover, SaltyTank (same rail profile).
+
+---
+
+## A. Rail Hardware
+
+| # | Description | Spec | Qty (per robot) | Notes |
+|---|-------------|------|-----------------|-------|
+| R1 | Aluminium bar stock | 50×12 mm, 6061-T6, 200 mm length | 1–2 | Preferred over printed rail for 2 kg load rating. CNC mill or route dovetail slot per `payload_rail.dxf` profile. |
+| R2 | M4×10 FHCS | Stainless, countersunk | 4–8 | Rail to adapter plate (or direct to deck); FHCS sits flush below rail bottom face |
+| R3 | M4 heat-set insert | M4×5.7 L, Ø5.6 OD | 4–8 | Into deck adapter plate |
+| R4 | Detent ball bearing | Ø6 mm, chrome steel (GCr15) | 2 per module | Module spring detent; standard bearing ball |
+| R5 | Detent spring | Ø5.5 mm OD, 12 mm free length, ~2 N/mm | 2 per module | Lee Spring LC 055A 06 S or equivalent; behind ball in plunger |
+| R6 | M4 thumbscrew (knurled) | M4×12, knurled head Ø14 mm | 1 per module | Safety lock; threads into M4 nut pressed into module side |
+| R7 | M4 hex nut | DIN 934, stainless | 1 per module | Captured in module body for thumbscrew |
+
+## B. Power + Data Connector
+
+| # | Description | Spec | Qty (per rail) | Notes |
+|---|-------------|------|----------------|-------|
+| C1 | Pogo pin | P75-E2 style, Ø2 mm, 6 mm travel, rated 2 A | 4 | Rail-side spring contacts. AliExpress "P75-E2 pogo pin" or Mill-Max 0906 series. |
+| C2 | Brass target pad | Ø4 × 1.5 mm disc | 4 per module | Module-side contact pads. Machine from Ø4 mm brass rod or order PCB pads. Press-fit with Loctite 603. |
+| C3 | JST-XH 2.54 mm header | 4-pin, right-angle | 1 per rail | Rail-side connector to power harness |
+| C4 | JST-XH housing + crimps | 4-pin female | 1 per robot | Wires from robot PSU (5 V, 12 V, GND, UART) |
+| C5 | 20 AWG silicone wire | Red / black / yellow / white, 300 mm each | 4 | Rail connector to robot bus |
+| C6 | Connector housing | `payload_connector_stl` | 1 | Press-fit into rail pocket |
+
+### Pin Map
+
+| Pin | Signal | Wire colour | Max current |
+|-----|--------|-------------|-------------|
+| 1 | GND | Black | Return |
+| 2 | +5 V | Red | 2 A |
+| 3 | +12 V | Yellow | 2 A |
+| 4 | UART (3.3 V) | White | 0.5 A |
+
+> **UART note**: Half-duplex (single wire). Module firmware connects to Jetson Orin NX UART2. Use RS-485 transceiver if module cable > 500 mm or multi-drop needed.
+
+## C. Deck Adapters
+
+| Part | File | Qty | Print | Mass est. |
+|------|------|-----|-------|-----------|
+| SaltyLab adapter | `payload_bay_rail.scad` `lab_adapter_stl` | 1 | PETG, 5 perims, 60% infill | ~30 g |
+| SaltyRover adapter | `payload_bay_rail.scad` `rover_adapter_stl` | 1 | PETG, 5 perims, 60% infill | ~35 g |
+| SaltyTank adapter | `payload_bay_rail.scad` `tank_adapter_stl` | 1 | PETG, 5 perims, 60% infill | ~35 g |
+
+## D. Printed Parts
+
+| Part | File | Qty | Print | Mass est. |
+|------|------|-----|-------|-----------|
+| Rail section (prototype) | `payload_bay_rail.scad` `rail_stl` | 1 | PETG, 5 perims, 60% infill, 0.2 mm layer | ~85 g |
+| Connector housing | `payload_bay_rail.scad` `connector_stl` | 1 | PETG, 5 perims, 100% infill | ~4 g |
+| Detent plunger | `payload_bay_rail.scad` `detent_plunger_stl` | 2 per module | PETG, 5 perims, 80% infill | ~2 g each |
+| Module base (universal) | `payload_bay_modules.scad` `base_stl` | N | PETG, 5 perims, 60% infill | ~18 g |
+| Cargo tray (200 mm) | `payload_bay_modules.scad` `cargo_tray_stl` | 1 | PETG, 4 perims, 30% infill | ~180 g |
+| Camera boom | `payload_bay_modules.scad` `camera_boom_stl` | 1 | PETG, 5 perims, 50% infill | ~95 g |
+| Cup holder | `payload_bay_modules.scad` `cup_holder_stl` | 1 | PETG, 4 perims, 25% infill | ~55 g |
+
+---
+
+## Dovetail Rail — CNC Specification
+
+For aluminium production rail (preferred over printed for 2 kg rating):
+
+```
+Material: 6061-T6 aluminium
+Stock: 50 mm × 12 mm flat bar, length to suit (200 mm, 300 mm, 400 mm)
+
+Dovetail slot (top face, centred):
+  Slot open width at top:    37.2 mm
+  Slot width at bottom:      28.0 mm
+  Slot depth:                 8.0 mm
+  Wall angle from vertical:  30.0° (60° included angle)
+  Surface finish: Ra 1.6 µm (smooth for low-friction sliding)
+
+Detent dimples (slot floor):
+  Diameter: 4.9 mm (ball seats in)
+  Depth: 1.5 mm
+  Pitch: 50 mm
+  First dimple: 25 mm from each end
+
+Safety-lock groove (both side faces, continuous):
+  Groove diameter: 4.5 mm
+  Depth: 1.5 mm
+  Z position: RAIL_T/2 - DOVE_H/2 = 8 mm from top face
+  (CNC with Ø4 mm ball-nose end mill, single pass at Z = -4 mm from top)
+
+Mounting holes (bottom face, countersunk):
+  Diameter: 4.3 mm (M4 clearance)
+  C/sink: Ø8 mm × 82° (M4 FHCS)
+  Pitch: 50 mm
+  First hole: 25 mm from each end
+
+Connector pocket (slot floor, centred in rail length):
+  Width: 26 mm (X), Depth: 8.4 mm (Y), Height: 7 mm (Z into slot floor)
+  Tolerance: +0.2 / 0 mm (press-fit housing)
+
+DXF cross-section: export payload_rail.dxf for supplier drawing.
+```
+
+---
+
+## Load Analysis
+
+| Mode | Load | Safety factor | Method |
+|------|------|---------------|--------|
+| Static payload (detent only) | 0.5 kg | 2× | Ball detent retention force ~10 N |
+| Static payload (thumbscrew locked) | 2.0 kg | 2× | Dovetail shear area ~800 mm² Al |
+| Dynamic (robot motion, 2 m/s²) | 2.0 kg | 1.5× | Inertial force = 2 kg × 2 m/s² = 4 N; detent holds 10 N |
+| Dovetail shear (PETG printed) | 1.2 kg | 1.5× | PETG tensile ~50 MPa; recommend Al rail for rated 2 kg |
+
+> **⚠ For 2 kg payload: use machined aluminium rail. Printed PETG rail is prototype/light-duty only (<0.8 kg payload).**
+
+---
+
+## Module Developer Guide
+
+### Adding a new module in 5 steps
+
+1. **Copy the template** at the bottom of `payload_bay_modules.scad`.
+2. **Set `MY_LEN`** — must be a multiple of 50 mm (detent pitch) for repeatable positioning.
+3. **Call `_module_base(MY_LEN, n_detents)`** as the first statement in your module.
+4. **Build payload geometry** starting at `Z = 0` (rail top face). Keep total height ≤ 200 mm for robot clearance under doorways.
+5. **Verify connector alignment** — when module is slid to its operating position, the 4 target pads on the tongue bottom must align with `CONN_Y` on the rail (default: 100 mm from rail entry end). Adjust `conn_offset` if needed.
+
+### Constraints
+
+| Parameter | Limit |
+|-----------|-------|
+| Module length | Min 60 mm, max 400 mm |
+| Module height above rail | Max 200 mm (clearance) |
+| Payload mass | ≤ 2 kg (Al rail + thumbscrew locked) |
+| Module width | Max 120 mm (robot shoulder clearance) |
+| Connector draw | Max 2 A per power pin (5 V or 12 V) |
+
+---
+
+## Export Commands
+
+```bash
+# Rail DXF (for CNC / waterjet machining)
+openscad payload_bay_rail.scad -D 'RENDER="rail_2d"'            -o payload_rail_profile.dxf
+
+# Rail STL (PETG prototype)
+openscad payload_bay_rail.scad -D 'RENDER="rail_stl"'           -o payload_rail_200mm.stl
+
+# Rail accessories
+openscad payload_bay_rail.scad -D 'RENDER="connector_stl"'      -o payload_connector.stl
+openscad payload_bay_rail.scad -D 'RENDER="detent_plunger_stl"' -o payload_detent_plunger.stl
+
+# Deck adapters
+openscad payload_bay_rail.scad -D 'RENDER="lab_adapter_stl"'    -o payload_adapter_lab.stl
+openscad payload_bay_rail.scad -D 'RENDER="rover_adapter_stl"'  -o payload_adapter_rover.stl
+openscad payload_bay_rail.scad -D 'RENDER="tank_adapter_stl"'   -o payload_adapter_tank.stl
+
+# Example modules
+openscad payload_bay_modules.scad -D 'RENDER="cargo_tray_stl"'  -o payload_cargo_tray.stl
+openscad payload_bay_modules.scad -D 'RENDER="camera_boom_stl"' -o payload_camera_boom.stl
+openscad payload_bay_modules.scad -D 'RENDER="cup_holder_stl"'  -o payload_cup_holder.stl
+openscad payload_bay_modules.scad -D 'RENDER="target_pad_2d"'   -o payload_target_pad.dxf
+```

chassis/payload_bay_modules.scad

@@ -0,0 +1,462 @@
+// ============================================================
+// payload_bay_modules.scad — Payload Bay Module Template + Examples
+// Issue: #170  Agent: sl-mechanical  Date: 2026-03-01
+// ============================================================
+//
+// ── HOW TO CREATE A NEW MODULE ──────────────────────────────────────────────
+//
+//   1. Copy the "MODULE TEMPLATE" section at the bottom of this file.
+//   2. Set MODULE_L to your module's Y length (min 60 mm).
+//   3. Add your payload geometry on top of the _module_base() call.
+//   4. The _module_base() provides:
+//        • Male dovetail tongue (slides into rail slot)
+//        • Spring detent bore(s) (for Ø6 mm ball + spring plunger)
+//        • Connector target pads (4× Ø4 mm brass, matching rail pogo pins)
+//        • Safety-lock M4 thumbscrew bore (side of tongue)
+//        • Bottom-face flush with rail top (Z = 0 at rail top / module base)
+//   5. Your payload geometry sits at Z ≥ 0 (above the rail top face).
+//   6. Add a RENDER dispatch entry and export command.
+//
+// ── DOVETAIL TONGUE GEOMETRY ────────────────────────────────────────────────
+//
+//   TONGUE_BOT = DOVE_SLOT_BOT - 2*DOVE_CLEAR  = 28.0 - 0.6 = 27.4 mm
+//   TONGUE_TOP = DOVE_SLOT_TOP + 2*DOVE_CLEAR  = 37.2 + 0.6 = 37.8 mm
+//   TONGUE_H   = DOVE_H + 0.2  (slight extra depth, no binding at corners)
+//
+//   Tongue runs full module length (-Y to +Y in module coords).
+//   Module body sits on top of tongue at Z = 0.
+//
+// ── CONNECTOR PADS ──────────────────────────────────────────────────────────
+//   4× Ø4 mm brass discs press-fit into tongue bottom face.
+//   Pad positions: must align with rail connector at CONN_Y when module
+//   is slid to its intended position (any detent step).
+//   Default: pads centred in module length → module must be placed so its
+//   centre aligns with CONN_Y on rail.  Or: set MODULE_CONN_OFFSET to shift.
+//
+// ── PAYLOAD RATING ──────────────────────────────────────────────────────────
+//   2 kg rated payload when safety-lock thumbscrew is tightened.
+//   Detent-only (no thumbscrew): ~0.5 kg (impact / vibration condition).
+//
+// ── RENDERED EXAMPLES ────────────────────────────────────────────────────────
+//   Part 1 — cargo_tray()     200×100 mm cargo tray, 30 mm walls
+//   Part 2 — camera_boom()    L-arm with sensor_rail-compatible head
+//   Part 3 — cup_holder()     Ø80 mm tapered cup cradle
+//
+// RENDER options:
+//   "assembly"          all 3 modules on ghost rail
+//   "base_stl"          module base / tongue only (universal)
+//   "cargo_tray_stl"    cargo tray module
+//   "camera_boom_stl"   camera boom module
+//   "cup_holder_stl"    cup holder module
+//   "target_pad_2d"     DXF — Ø4 mm brass target pad profile
+//
+// Export:
+//   openscad payload_bay_modules.scad -D 'RENDER="base_stl"'        -o payload_base.stl
+//   openscad payload_bay_modules.scad -D 'RENDER="cargo_tray_stl"'  -o payload_cargo_tray.stl
+//   openscad payload_bay_modules.scad -D 'RENDER="camera_boom_stl"' -o payload_camera_boom.stl
+//   openscad payload_bay_modules.scad -D 'RENDER="cup_holder_stl"'  -o payload_cup_holder.stl
+// ============================================================
+
+$fn = 64;
+e   = 0.01;
+
+// ── Rail geometry constants (must match payload_bay_rail.scad) ────────────────
+RAIL_W         = 50.0;
+RAIL_T         = 12.0;
+DOVE_ANGLE     = 30.0;
+DOVE_H         =  8.0;
+DOVE_SLOT_BOT  = 28.0;
+DOVE_SLOT_TOP  = DOVE_SLOT_BOT + 2 * DOVE_H * tan(DOVE_ANGLE);
+DOVE_CLEAR     =  0.3;
+DETENT_PITCH   = 50.0;
+DETENT_BALL_D  =  6.0;
+DETENT_SPG_OD  =  6.2;
+DETENT_SPG_L   = 16.0;
+CONN_PIN_SPC   =  5.0;
+CONN_N_PINS    =  4;
+CONN_HOUSING_D =  8.0;
+
+// ── Module tongue (male dovetail) geometry ────────────────────────────────────
+TONGUE_BOT     = DOVE_SLOT_BOT - 2*DOVE_CLEAR;  // 27.4 mm
+TONGUE_TOP     = DOVE_SLOT_TOP + 2*DOVE_CLEAR;  // 37.8 mm
+TONGUE_H       = DOVE_H + 0.2;                  // 8.2 mm (slight extra depth)
+
+// ── Connector target pad ──────────────────────────────────────────────────────
+TARGET_PAD_D   =  4.0;    // brass pad OD (slightly larger than pogo Ø2 mm)
+TARGET_PAD_T   =  1.5;    // brass pad thickness
+TARGET_PAD_RECESS = 1.3;  // press-fit recess depth (pad is 0.2 mm proud)
+// 4 pads at CONN_PIN_SPC pitch, centred in module tongue
+CONN_SPAN      = (CONN_N_PINS - 1) * CONN_PIN_SPC;  // 15 mm
+
+// ── Module safety lock ────────────────────────────────────────────────────────
+LOCK_BOLT_D    =  4.3;    // M4 thumbscrew bore through tongue side
+LOCK_BOLT_Z    =  TONGUE_H/2; // Z of thumbscrew CL from tongue bottom
+// Thumbscrew tightens against continuous groove in rail side.
+
+// Fasteners
+M3_D = 3.3;
+M4_D = 4.3;
+
+// ============================================================
+// RENDER DISPATCH
+// ============================================================
+RENDER = "assembly";
+
+if      (RENDER == "assembly")        assembly();
+else if (RENDER == "base_stl")        _module_base(80);
+else if (RENDER == "cargo_tray_stl")  cargo_tray();
+else if (RENDER == "camera_boom_stl") camera_boom();
+else if (RENDER == "cup_holder_stl")  cup_holder();
+else if (RENDER == "target_pad_2d") {
+    projection(cut = true) translate([0, 0, -0.5])
+        linear_extrude(1) circle(d = TARGET_PAD_D);
+}
+
+// ============================================================
+// ASSEMBLY PREVIEW
+// ============================================================
+module assembly() {
+    // Ghost rail
+    %color("Silver", 0.25)
+        translate([0, 0, -RAIL_T])
+            cube([RAIL_W, 600, RAIL_T], center = true);
+
+    // Cargo tray at Y=50 (first detent position)
+    color("OliveDrab", 0.85)
+        translate([0, 50, 0])
+            cargo_tray();
+
+    // Cup holder at Y=300
+    color("RoyalBlue", 0.85)
+        translate([0, 300, 0])
+            cup_holder();
+
+    // Camera boom at Y=500
+    color("DarkSlateGray", 0.85)
+        translate([0, 500, 0])
+            camera_boom();
+}
+
+// ============================================================
+// UNIVERSAL MODULE BASE  (_module_base)
+// ============================================================
+// All modules use this as their foundation.
+// Provides: male dovetail tongue, detent bore, connector pads, lock bore.
+//
+// module_len : module length in Y (≥ 60 mm)
+// n_detents  : how many ball detent bores to include (1 or 2)
+// conn_offset: Y offset of connector pads from module centre (default 0)
+//
+// After calling _module_base(), add payload geometry at Z = 0 and above.
+// The tongue occupies Z = -(TONGUE_H) to Z = 0.
+// Rail top face is at Z = 0.
+module _module_base(module_len, n_detents = 1, conn_offset = 0) {
+    ml  = module_len;
+
+    difference() {
+        // ── Dovetail tongue (male) ────────────────────────────────────
+        translate([0, ml/2, -TONGUE_H/2])
+            linear_extrude(TONGUE_H, center = true, twist = 0,
+                           convexity = 2)
+                _tongue_profile_2d();
+
+        // ── Spring detent bore(s) (up through tongue top face) ────────
+        // 1 detent: at module centre; 2 detents: at ±25 mm from centre
+        for (i = [0 : n_detents - 1]) {
+            dy = (n_detents == 1)
+                    ? ml/2
+                    : ml/2 + (i - (n_detents-1)/2) * DETENT_PITCH;
+            translate([0, dy, -(TONGUE_H/2) - DETENT_SPG_L/2])
+                cylinder(d = DETENT_SPG_OD,
+                         h = DETENT_SPG_L + TONGUE_H/2 + e);
+        }
+
+        // ── Connector target pad recesses (tongue bottom face) ────────
+        for (i = [0 : CONN_N_PINS - 1]) {
+            px = (i - (CONN_N_PINS-1)/2) * CONN_PIN_SPC;
+            translate([px,
+                       ml/2 + conn_offset,
+                       -TONGUE_H + e])
+                cylinder(d = TARGET_PAD_D + 0.1,
+                         h = TARGET_PAD_RECESS + e);
+        }
+
+        // ── Safety-lock M4 thumbscrew bore (right side of tongue) ─────
+        // Bore exits tongue right face, tip bears on rail side groove
+        translate([TONGUE_TOP/2 + e, ml/2, LOCK_BOLT_Z - TONGUE_H])
+            rotate([0, 90, 0])
+                cylinder(d = LOCK_BOLT_D,
+                         h = (TONGUE_TOP - TONGUE_BOT)/2 + 6 + e);
+
+        // ── Lead-in chamfer on entry end of tongue ────────────────────
+        // 2 mm chamfer on bottom corners of tongue at Y=0 end
+        translate([0, 0, -TONGUE_H])
+            rotate([45, 0, 0])
+                cube([TONGUE_TOP + 2*e, 4, 4], center = true);
+    }
+}
+
+// ── Tongue 2D cross-section (male dovetail, trapezoid wider at bottom) ────────
+// Module tongue: narrower at top (entering slot), wider at bottom (interlocking).
+// Note orientation: tongue points DOWN (-Z), so wider face is at bottom (-Z).
+module _tongue_profile_2d() {
+    polygon([
+        [-TONGUE_TOP/2,  0],         // top-left  (at Z = 0, flush with rail top)
+        [ TONGUE_TOP/2,  0],         // top-right
+        [ TONGUE_BOT/2, -TONGUE_H],  // bottom-right (interlocking face)
+        [-TONGUE_BOT/2, -TONGUE_H],  // bottom-left
+    ]);
+}
+
+// ============================================================
+// PART 1 — CARGO TRAY
+// ============================================================
+// 200×100 mm open tray for transporting small items.
+// 30 mm walls, chamfered rim, 4× drainage holes.
+// Two Velcro strip slots on tray floor for cargo retention.
+// Module length: 200 mm (4 detent positions).
+// Weight budget: tray printed ~180 g; payload up to 2 kg.
+TRAY_L       = 200.0;   // module Y length
+TRAY_W       = 100.0;   // tray interior width (X)
+TRAY_WALL    =   3.0;   // tray wall thickness
+TRAY_H       =  30.0;   // tray wall height above module base
+TRAY_FLOOR_T =   3.0;   // tray floor thickness
+
+module cargo_tray() {
+    // Mount base on rail (2 detents at ±50 mm from module centre)
+    _module_base(TRAY_L, n_detents = 2);
+
+    difference() {
+        union() {
+            // ── Tray body on top of base ────────────────────────────
+            translate([-TRAY_W/2 - TRAY_WALL, 0,
+                       0])
+                cube([TRAY_W + 2*TRAY_WALL,
+                      TRAY_L,
+                      TRAY_FLOOR_T + TRAY_H]);
+
+            // ── Corner gussets (stiffening for 2 kg load) ───────────
+            for (cx = [-1, 1]) for (cy = [0, 1])
+                hull() {
+                    translate([cx * TRAY_W/2,
+                               cy * (TRAY_L - TRAY_WALL),
+                               0])
+                        cube([TRAY_WALL + e, TRAY_WALL + e,
+                              TRAY_FLOOR_T + TRAY_H * 0.6], center = true);
+                    translate([cx * (TRAY_W/2 - 10),
+                               cy * (TRAY_L - TRAY_WALL),
+                               0])
+                        cylinder(d = TRAY_WALL * 2, h = e);
+                }
+        }
+
+        // ── Tray interior cavity ─────────────────────────────────────
+        translate([-TRAY_W/2, TRAY_WALL,
+                   TRAY_FLOOR_T])
+            cube([TRAY_W, TRAY_L - 2*TRAY_WALL,
+                  TRAY_H + e]);
+
+        // ── Drainage holes (4× Ø8 mm in floor) ──────────────────────
+        for (dx = [-TRAY_W/4, TRAY_W/4])
+        for (dy = [TRAY_L/4, 3*TRAY_L/4])
+            translate([dx, dy, -e])
+                cylinder(d = 8, h = TRAY_FLOOR_T + 2*e);
+
+        // ── Velcro slot × 2 (25 mm wide grooves in floor) ────────────
+        for (dy = [TRAY_L/3, 2*TRAY_L/3])
+            translate([0, dy, TRAY_FLOOR_T - 1.5])
+                cube([TRAY_W - 10, 25, 2 + e], center = true);
+
+        // ── Rim chamfer (top inner edge, ergonomic) ───────────────────
+        translate([0, TRAY_L/2, TRAY_FLOOR_T + TRAY_H + e])
+            cube([TRAY_W + 2*TRAY_WALL + 2*e,
+                  TRAY_L + 2*e, 4], center = true);
+
+        // ── Side slots for bungee cord retention (3× each long side) ─
+        for (sx = [-1, 1])
+        for (sy = [TRAY_L/4, TRAY_L/2, 3*TRAY_L/4])
+            translate([sx * (TRAY_W/2 + TRAY_WALL/2),
+                       sy, TRAY_FLOOR_T + TRAY_H/2])
+                cube([TRAY_WALL + 2*e, 8, 6], center = true);
+    }
+}
+
+// ============================================================
+// PART 2 — CAMERA BOOM
+// ============================================================
+// L-shaped arm: vertical mast + horizontal boom.
+// Boom head accepts sensor_rail 2020 T-slot (RAIL_W/2 bolt pattern).
+// Sensor head can be rotated 0/90/180° and locked with M4 bolt.
+// Module length: 80 mm; arm rises 120 mm, boom extends 80 mm forward.
+BOOM_MODULE_L  =  80.0;
+BOOM_MAST_H    = 120.0;   // mast height above rail top (Z)
+BOOM_ARM_L     =  80.0;   // horizontal boom length (+Y forward)
+BOOM_ARM_W     =  20.0;   // arm cross-section width
+BOOM_ARM_T     =  20.0;   // arm cross-section height
+BOOM_HEAD_W    =  50.0;   // sensor head width (matches 2020 rail flange)
+BOOM_HEAD_H    =  20.0;   // sensor head plate height
+BOOM_HEAD_T    =   5.0;   // sensor head plate thickness
+
+module camera_boom() {
+    _module_base(BOOM_MODULE_L, n_detents = 1);
+
+    difference() {
+        union() {
+            // ── Mast (vertical column from module body) ──────────────
+            translate([-BOOM_ARM_W/2, BOOM_MODULE_L/2 - BOOM_ARM_T/2, 0])
+                cube([BOOM_ARM_W, BOOM_ARM_T, BOOM_MAST_H]);
+
+            // ── Horizontal boom (from mast top, extends +Y) ──────────
+            translate([-BOOM_ARM_W/2,
+                       BOOM_MODULE_L/2 - BOOM_ARM_T/2,
+                       BOOM_MAST_H - BOOM_ARM_W])
+                cube([BOOM_ARM_W, BOOM_ARM_L, BOOM_ARM_W]);
+
+            // ── Sensor head plate (at boom tip) ──────────────────────
+            translate([-BOOM_HEAD_W/2,
+                       BOOM_MODULE_L/2 - BOOM_ARM_T/2 + BOOM_ARM_L,
+                       BOOM_MAST_H - BOOM_ARM_W - BOOM_HEAD_H/2 + BOOM_ARM_W/2])
+                cube([BOOM_HEAD_W, BOOM_HEAD_T, BOOM_HEAD_H]);
+
+            // ── Junction gussets (mast + horizontal boom) ────────────
+            translate([-BOOM_ARM_W/2,
+                       BOOM_MODULE_L/2 - BOOM_ARM_T/2,
+                       BOOM_MAST_H - BOOM_ARM_W])
+                rotate([45, 0, 0])
+                    cube([BOOM_ARM_W, BOOM_ARM_W * 0.7, BOOM_ARM_W * 0.7]);
+        }
+
+        // ── 2020 sensor-rail bolt pattern in head plate ───────────────
+        // 2× M5 slots (matches sensor_rail.scad tank_clamp slot geometry)
+        for (sz = [-BOOM_HEAD_H/4, BOOM_HEAD_H/4])
+            translate([0,
+                       BOOM_MODULE_L/2 - BOOM_ARM_T/2 + BOOM_ARM_L + BOOM_HEAD_T + e,
+                       BOOM_MAST_H - BOOM_ARM_W/2 + sz])
+                rotate([90, 0, 0])
+                    hull() {
+                        translate([-6, 0, 0])
+                            cylinder(d = 5.3, h = BOOM_HEAD_T + 2*e);
+                        translate([+6, 0, 0])
+                            cylinder(d = 5.3, h = BOOM_HEAD_T + 2*e);
+                    }
+
+        // ── Tilt angle slots (3 positions: 0°, ±15°) ─────────────────
+        for (ta = [-15, 0, 15]) {
+            translate([0,
+                       BOOM_MODULE_L/2 - BOOM_ARM_T/2 + BOOM_ARM_L/2,
+                       BOOM_MAST_H - BOOM_ARM_W/2])
+                rotate([ta, 0, 0])
+                    translate([0, BOOM_ARM_L/2, 0])
+                        cylinder(d = 4.3, h = BOOM_ARM_W + 2*e,
+                                 center = true);
+        }
+
+        // ── Cable tie slots in mast ───────────────────────────────────
+        for (cz = [BOOM_MAST_H * 0.3, BOOM_MAST_H * 0.6])
+            translate([0, BOOM_MODULE_L/2, cz])
+                cube([BOOM_ARM_W + 2*e, 4, 4], center = true);
+
+        // ── Lightening pockets in mast ────────────────────────────────
+        translate([0, BOOM_MODULE_L/2, BOOM_MAST_H * 0.4])
+            cube([BOOM_ARM_W - 6, BOOM_ARM_T - 6,
+                  BOOM_MAST_H * 0.4], center = true);
+    }
+}
+
+// ============================================================
+// PART 3 — CUP HOLDER
+// ============================================================
+// Tapered cup cradle for standard travel mugs / water bottles.
+// Inner diameter: 80 mm at top, 68 mm at bottom (matches Ø70 mm typical mug).
+// Flexible gripper ribs (cut slots) provide spring retention.
+// Drain hole at bottom for condensation.
+// Module length: 80 mm.
+CUP_MODULE_L  =  80.0;
+CUP_INNER_TOP = 80.0;    // inner bore OD at top
+CUP_INNER_BOT = 68.0;    // inner bore OD at bottom (taper for grip)
+CUP_OUTER_T   =  4.0;    // wall thickness
+CUP_H         = 80.0;    // cup holder height
+CUP_GRIP_SLOTS = 8;      // number of flex slots (spring grip)
+CUP_SLOT_W    =  2.5;    // flex slot width
+CUP_SLOT_H    = 40.0;    // flex slot height (from top)
+
+module cup_holder() {
+    _module_base(CUP_MODULE_L, n_detents = 1);
+
+    difference() {
+        union() {
+            // ── Outer shell (tapered cylinder) ───────────────────────
+            cylinder(d1 = CUP_INNER_BOT + 2*CUP_OUTER_T,
+                     d2 = CUP_INNER_TOP + 2*CUP_OUTER_T,
+                     h  = CUP_H);
+
+            // ── Base flange (connects to module body footprint) ───────
+            hull() {
+                cylinder(d = CUP_INNER_BOT + 2*CUP_OUTER_T + 4,
+                         h = 4);
+                translate([0, CUP_MODULE_L/2, 0])
+                    cube([RAIL_W, CUP_MODULE_L, 4], center = true);
+            }
+        }
+
+        // ── Inner bore (tapered cup cavity) ──────────────────────────
+        translate([0, 0, CUP_OUTER_T])
+            cylinder(d1 = CUP_INNER_BOT, d2 = CUP_INNER_TOP,
+                     h  = CUP_H + e);
+
+        // ── Base drain hole ──────────────────────────────────────────
+        translate([0, 0, -e])
+            cylinder(d = 12, h = CUP_OUTER_T + 2*e);
+
+        // ── Flex grip slots (from top down) ──────────────────────────
+        // Slots allow upper rim to flex inward and grip cup body
+        for (i = [0 : CUP_GRIP_SLOTS - 1]) {
+            angle = i * 360 / CUP_GRIP_SLOTS;
+            rotate([0, 0, angle])
+                translate([CUP_INNER_TOP/2 + CUP_OUTER_T/2, 0, CUP_H - CUP_SLOT_H])
+                    cube([CUP_OUTER_T + 2*e, CUP_SLOT_W, CUP_SLOT_H + e],
+                         center = true);
+        }
+
+        // ── Exterior branding recess (optional label area) ────────────
+        translate([CUP_INNER_BOT/2 + CUP_OUTER_T/2 - 0.5, 0, CUP_H/2])
+            rotate([0, 90, 0])
+                cube([25, 40, 1 + e], center = true);
+    }
+}
+
+// ============================================================
+// ══ MODULE TEMPLATE ═══════════════════════════════════════════
+// ══ Copy this block to create a new payload module ════════════
+// ============================================================
+//
+// module my_new_module() {
+//     MY_LEN = 120.0;   // module length — must be multiple of DETENT_PITCH (50 mm)
+//
+//     // Always start with the base (provides tongue, pads, detent bore)
+//     _module_base(MY_LEN, n_detents = 2);
+//
+//     // Add your payload geometry here.
+//     // Z = 0 is the rail top face / module mounting face.
+//     // Build upward from Z = 0.
+//
+//     difference() {
+//         union() {
+//             // Example: a simple platform
+//             translate([-(RAIL_W + 10)/2, 0, 0])
+//                 cube([RAIL_W + 10, MY_LEN, 10]);
+//
+//             // Add your geometry...
+//         }
+//
+//         // Add your cutouts...
+//     }
+// }
+//
+// ── Don't forget to: ──────────────────────────────────────────
+//   1. Add  else if (RENDER == "my_module_stl")  my_new_module();
+//      in the RENDER DISPATCH block above.
+//   2. Add export command in BOM / README.
+//   3. Test: verify tongue fits rail slot (should slide with 0.3 mm clearance).
+//   4. Verify connector pad positions align with CONN_Y on rail.
+// ============================================================

chassis/payload_bay_rail.scad

@@ -0,0 +1,429 @@
+// ============================================================
+// payload_bay_rail.scad — Modular Payload Bay Rail System
+// Issue: #170  Agent: sl-mechanical  Date: 2026-03-01
+// ============================================================
+//
+// Dovetail rail mounted on robot top deck.  Payload modules slide on
+// from either end and are retained by a spring-loaded ball detent plus
+// an optional M4 thumbscrew safety lock.
+//
+// Dovetail geometry (60° included angle — balanced for print + load):
+//
+//    ← RAIL_W (50 mm) →
+//    ┌──────────────────┐  ← rail top face (Z = RAIL_T)
+//    │  ╲            ╱  │
+//    │   ╲__________╱   │  ← dovetail slot (female, cut into top)
+//    │   (DOVE_SLOT)    │
+//    └──────────────────┘  ← rail bottom face (Z = 0)
+//
+// DOVE_ANGLE = 30° from vertical (= 60° included).
+// Slot width at top  = DOVE_SLOT_TOP  (open face)
+// Slot width at bottom = DOVE_SLOT_BOT (inner base of slot)
+// Slot depth = DOVE_H
+//
+// Module tongue (male dovetail) slides in with 0.3 mm clearance each side.
+//
+// Spring detent: Ø6 mm steel ball in module, spring behind, seats in
+//   Ø4.9 mm dimples drilled into rail slot bottom at DETENT_PITCH spacing.
+//   Provides tactile click-lock at each indexed position.
+//
+// Safety lock: M4 thumbscrew through module side, tightens against rail
+//   side wall.  For vibration environments or >1 kg payload.
+//
+// Power+data connector: 4-pin pogo array in rail at fixed position.
+//   Pins: GND | +5 V | +12 V | UART (half-duplex)
+//   Module has matching brass target pads (Ø4 mm).
+//   Connector position: centred in rail length, at CONN_Y from one end.
+//
+// Cross-variant adapter plates (this file):
+//   lab_rail_adapter()   — SaltyLab chassis top (Ø25 mm stem clear)
+//   rover_rail_adapter() — SaltyRover deck (M4 grid)
+//   tank_rail_adapter()  — SaltyTank deck (M4 grid)
+//
+// Coordinate convention:
+//   Rail runs along Y axis.  Cross-section in X-Z plane.
+//   Z = 0 at rail bottom face (= robot deck top face).
+//   Module slides in +Y direction.
+//
+// RENDER options:
+//   "assembly"            rail + adapter + module ghost
+//   "rail_2d"             DXF — dovetail cross-section (CNC/waterjet)
+//   "rail_stl"            STL — printable rail section (PETG prototype)
+//   "connector_stl"       STL — pogo connector housing insert
+//   "detent_plunger_stl"  STL — spring detent plunger (print ×2 per module)
+//   "lab_adapter_stl"     STL — SaltyLab deck adapter
+//   "rover_adapter_stl"   STL — SaltyRover deck adapter
+//   "tank_adapter_stl"    STL — SaltyTank deck adapter
+//
+// Export:
+//   openscad payload_bay_rail.scad -D 'RENDER="rail_2d"'            -o payload_rail.dxf
+//   openscad payload_bay_rail.scad -D 'RENDER="rail_stl"'           -o payload_rail.stl
+//   openscad payload_bay_rail.scad -D 'RENDER="connector_stl"'      -o payload_connector.stl
+//   openscad payload_bay_rail.scad -D 'RENDER="detent_plunger_stl"' -o payload_detent.stl
+//   openscad payload_bay_rail.scad -D 'RENDER="lab_adapter_stl"'    -o payload_lab_adapter.stl
+//   openscad payload_bay_rail.scad -D 'RENDER="rover_adapter_stl"'  -o payload_rover_adapter.stl
+//   openscad payload_bay_rail.scad -D 'RENDER="tank_adapter_stl"'   -o payload_tank_adapter.stl
+// ============================================================
+
+$fn = 64;
+e   = 0.01;
+
+// ── Dovetail rail cross-section ───────────────────────────────────────────────
+RAIL_W       = 50.0;    // rail total width (X)
+RAIL_T       = 12.0;    // rail total height (Z)
+RAIL_R       =  2.0;    // outer corner radius
+RAIL_LEN     = 200.0;   // default rail section length (Y)
+
+// Dovetail slot geometry
+DOVE_ANGLE   = 30.0;    // degrees from vertical (60° included angle)
+DOVE_H       =  8.0;    // slot depth (Z into rail from top)
+DOVE_SLOT_BOT=  28.0;   // slot width at bottom (inner)
+// Derived: slot width at top = DOVE_SLOT_BOT + 2 * DOVE_H * tan(DOVE_ANGLE)
+DOVE_SLOT_TOP= DOVE_SLOT_BOT + 2 * DOVE_H * tan(DOVE_ANGLE);  // ≈ 37.2 mm
+
+// Module tongue (male) clearance: 0.3 mm per side
+DOVE_CLEAR   =  0.3;
+// → module tongue: bot_w = DOVE_SLOT_BOT - 2*DOVE_CLEAR,  top_w = DOVE_SLOT_TOP + 2*DOVE_CLEAR
+
+// ── Spring ball detent ────────────────────────────────────────────────────────
+// Ø6 mm steel ball presses up through module tongue into dimples in rail slot.
+DETENT_BALL_D  =  6.0;    // ball diameter
+DETENT_HOLE_D  =  4.9;    // dimple bore in rail slot bottom (ball seats in)
+DETENT_DEPTH   =  1.5;    // dimple depth (ball sinks in this far)
+DETENT_PITCH   = 50.0;    // dimple spacing along rail (Y) — module index positions
+DETENT_SPG_OD  =  6.2;    // plunger bore OD (ball + spring housing in module)
+DETENT_SPG_L   = 16.0;    // spring pocket depth in module tongue
+
+// ── Safety lock (M4 thumbscrew through module side into rail side groove) ─────
+LOCK_GROOVE_D  =  4.5;    // groove in rail side wall (M4 thumbscrew tip seats in)
+LOCK_GROOVE_DEPTH = 1.5;  // groove depth into rail side
+// Lock groove runs full rail length (continuous slot) for tool-free slide + lock anywhere
+
+// ── 4-pin power+data connector ───────────────────────────────────────────────
+// Pogo pin array mounted in rail body at CONN_Y from entry end.
+// Pin map: 1=GND  2=+5V  3=+12V  4=UART
+// Pogo: Ø2 mm spring contact (P75-E2 style), rated 2 A (power), 0.5 A (signal)
+CONN_Y        = RAIL_LEN / 2;   // connector centred in rail section
+CONN_PIN_D    =  2.2;           // pogo bore (2 mm pin + 0.2 mm clearance)
+CONN_PIN_SPC  =  5.0;           // pin centre-to-centre spacing
+CONN_N_PINS   =  4;             // GND / +5V / +12V / UART
+CONN_HOUSING_W= CONN_N_PINS * CONN_PIN_SPC + 4;  // housing width (X)
+CONN_HOUSING_D=  8.0;           // housing depth (Y, inside rail)
+CONN_HOUSING_H= DOVE_H - 1.0;  // housing height; sits inside slot (flush with slot floor)
+// Connector pogo pins point upward into module pad targets.
+
+// ── Deck mounting holes ───────────────────────────────────────────────────────
+MOUNT_PITCH  = 50.0;   // M4 FHCS hole pitch along rail (countersunk from bottom)
+MOUNT_INSET  = 25.0;   // first hole Y from rail end
+MOUNT_D      =  4.3;   // M4 clearance
+CSINK_D      =  8.0;   // M4 FHCS head diameter
+
+// ── Cross-variant adapter plates ─────────────────────────────────────────────
+ADAPT_T      =  4.0;   // adapter plate thickness
+ADAPT_OVHG   = 10.0;   // adapter overhang past rail edge each side (flange width)
+
+// SaltyLab deck: stem bore at centre
+LAB_STEM_BORE =  26.0;   // clear stem Ø25 mm
+
+// SaltyRover deck: M4 bolt grid (spacing from rover_chassis_r2.scad)
+ROVER_BOLT_SPC = 40.0;
+
+// SaltyTank deck: M4 bolt grid (spacing from saltytank_chassis.scad)
+TANK_BOLT_SPC  = 40.0;
+
+// Fasteners
+M3_D = 3.3;
+M4_D = 4.3;
+
+// ============================================================
+// RENDER DISPATCH
+// ============================================================
+RENDER = "assembly";
+
+if      (RENDER == "assembly")           assembly();
+else if (RENDER == "rail_2d")
+    projection(cut = true) translate([0, 0, -0.5])
+        linear_extrude(1) rail_profile_2d();
+else if (RENDER == "rail_stl")           rail_section(RAIL_LEN);
+else if (RENDER == "connector_stl")      connector_housing();
+else if (RENDER == "detent_plunger_stl") detent_plunger();
+else if (RENDER == "lab_adapter_stl")    lab_rail_adapter();
+else if (RENDER == "rover_adapter_stl")  rover_rail_adapter();
+else if (RENDER == "tank_adapter_stl")   tank_rail_adapter();
+
+// ============================================================
+// ASSEMBLY PREVIEW
+// ============================================================
+module assembly() {
+    // Rail section
+    color("Silver", 0.85) rail_section(RAIL_LEN);
+
+    // Rover adapter under rail
+    color("SteelBlue", 0.70)
+        translate([0, 0, -ADAPT_T])
+            rover_rail_adapter();
+
+    // Ghost module sliding on
+    %color("OliveDrab", 0.3)
+        translate([0, 60, RAIL_T])
+            cube([RAIL_W + 10, 100, 40], center = true);
+
+    // Connector position marker
+    %color("Gold", 0.5)
+        translate([0, CONN_Y, RAIL_T - DOVE_H])
+            cube([CONN_HOUSING_W, CONN_HOUSING_D, CONN_HOUSING_H],
+                 center = true);
+
+    // Detent dimple markers
+    for (dy = [MOUNT_INSET : DETENT_PITCH : RAIL_LEN - MOUNT_INSET])
+        %color("Red", 0.6)
+            translate([0, dy, RAIL_T - DOVE_H])
+                cylinder(d = DETENT_HOLE_D, h = DETENT_DEPTH);
+}
+
+// ============================================================
+// RAIL CROSS-SECTION 2D  (DXF export)
+// ============================================================
+// Outer profile minus dovetail slot.
+// For CNC milling from 50×12 mm aluminium bar, or waterjet from plate.
+// Also used for PETG prototype extrusion.
+module rail_profile_2d() {
+    difference() {
+        // Outer rail cross-section (rounded rect)
+        minkowski() {
+            square([RAIL_W - 2*RAIL_R, RAIL_T - 2*RAIL_R],
+                   center = true);
+            circle(r = RAIL_R);
+        }
+
+        // Dovetail slot (trapezoid, open at top)
+        translate([0, RAIL_T/2])
+            _dovetail_slot_2d();
+    }
+}
+
+// ── Dovetail slot 2D (trapezoid with open top) ───────────────────────────────
+module _dovetail_slot_2d() {
+    // Trapezoid: wider at top (open face), narrower at bottom.
+    // Points listed clockwise from top-left:
+    polygon([
+        [-DOVE_SLOT_TOP/2,  0],                  // top-left
+        [ DOVE_SLOT_TOP/2,  0],                  // top-right
+        [ DOVE_SLOT_BOT/2, -DOVE_H],             // bottom-right
+        [-DOVE_SLOT_BOT/2, -DOVE_H],             // bottom-left
+    ]);
+}
+
+// ── Dovetail slot for difference() operations (3D volume) ────────────────────
+module _dovetail_slot_3d(length) {
+    translate([0, -e, RAIL_T])
+        linear_extrude(DOVE_H + e)
+            offset(delta = e)
+                _dovetail_slot_2d();
+}
+
+// ============================================================
+// RAIL SECTION  (3D — printable or aluminium)
+// ============================================================
+module rail_section(len = RAIL_LEN) {
+    difference() {
+        // ── Extruded profile ────────────────────────────────────────
+        linear_extrude(len)
+            rotate([0, 0, 90])
+                rail_profile_2d();
+
+        // ── Dovetail slot ────────────────────────────────────────────
+        translate([0, -e, RAIL_T])
+            rotate([0, 0, 0])
+                linear_extrude(len + 2*e)
+                    rotate([0, 0, 90])
+                        offset(delta = e)
+                            _dovetail_slot_2d();
+
+        // ── Deck mounting holes (M4 FHCS, from bottom) ───────────────
+        for (my = [MOUNT_INSET : MOUNT_PITCH : len - MOUNT_INSET])
+            translate([0, my, -e])
+                cylinder(d = MOUNT_D, h = RAIL_T - DOVE_H + 2*e);
+        // Countersinks on bottom face
+        for (my = [MOUNT_INSET : MOUNT_PITCH : len - MOUNT_INSET])
+            translate([0, my, -e])
+                cylinder(d1 = CSINK_D, d2 = MOUNT_D,
+                         h = (CSINK_D - MOUNT_D) / (2 * tan(41)));
+
+        // ── Spring detent dimples (slot bottom, at DETENT_PITCH) ──────
+        for (dy = [MOUNT_INSET : DETENT_PITCH : len - MOUNT_INSET])
+            translate([0, dy, RAIL_T - DOVE_H - e])
+                cylinder(d = DETENT_HOLE_D, h = DETENT_DEPTH + e);
+
+        // ── Safety-lock groove (continuous slot, both sides of rail) ──
+        // M4 thumbscrew tip seats anywhere along groove
+        for (sx = [-1, 1])
+            translate([sx * (RAIL_W/2 + e), -e,
+                       RAIL_T - DOVE_H/2])
+                rotate([0, 90, 0])
+                    hull() {
+                        translate([0, 0, 0])
+                            cylinder(d = LOCK_GROOVE_D, h = RAIL_W + 2*e);
+                    }
+
+        // ── Connector housing pocket (at CONN_Y) ──────────────────────
+        translate([0, CONN_Y, RAIL_T - DOVE_H - e])
+            cube([CONN_HOUSING_W + 0.4,
+                  CONN_HOUSING_D + 0.4,
+                  CONN_HOUSING_H + e], center = true);
+
+        // ── Lightening slots (rail body between mounting holes) ────────
+        for (my = [MOUNT_INSET + 25 : MOUNT_PITCH : len - MOUNT_INSET - 25])
+            translate([0, my, RAIL_T/4])
+                cube([RAIL_W - 16, 20, RAIL_T/2 + 2*e], center = true);
+    }
+}
+
+// ============================================================
+// CONNECTOR HOUSING  (pogo-pin insert, press-fits into rail pocket)
+// ============================================================
+// 4× spring-loaded pogo pins (P75-E2, Ø2 mm, 6 mm travel).
+// Printed housing press-fits into rail pocket; pins protrude up into module.
+// Module has 4× Ø4 mm brass target pads at matching pitch.
+//
+// Pin map (left to right, looking at rail top from +Z):
+//   Pin 1: GND  Pin 2: +5 V  Pin 3: +12 V  Pin 4: UART
+module connector_housing() {
+    hw = CONN_HOUSING_W;
+    hd = CONN_HOUSING_D;
+    hh = CONN_HOUSING_H;
+
+    difference() {
+        // Housing body (press-fit into rail pocket)
+        cube([hw, hd, hh], center = true);
+
+        // 4× pogo pin bores (through housing, top to bottom)
+        for (i = [0 : CONN_N_PINS - 1]) {
+            px = (i - (CONN_N_PINS - 1) / 2) * CONN_PIN_SPC;
+            translate([px, 0, -hh/2 - e])
+                cylinder(d = CONN_PIN_D, h = hh + 2*e);
+        }
+
+        // Wire exit slot (bottom, routes into rail body)
+        translate([0, 0, -hh/2 - e])
+            cube([hw - 6, hd/2, hh/3 + e], center = true);
+
+        // Retention barbs (prevent housing pulling out of pocket)
+        for (sx = [-1, 1])
+            translate([sx * (hw/2 - 1), 0, hh/4])
+                rotate([0, sx * 15, 0])
+                    cube([2, hd + 2*e, 2.5], center = true);
+    }
+
+    // Pin polarity label recess (on top face, GND side)
+    difference() {
+        translate([0, 0, 0]) cube([0, 0, 0]); // null
+        translate([-(CONN_N_PINS * CONN_PIN_SPC)/2 + 1, 0, hh/2 - 0.4])
+            cube([3, hd * 0.6, 0.5 + e], center = true);
+    }
+}
+
+// ============================================================
+// DETENT PLUNGER  (lives in module tongue; print 2× per module)
+// ============================================================
+// Press-fit into Ø6.2 mm bore in module tongue.
+// Includes spring pocket; ball seated on top.
+// Spring: Ø5.5 mm OD, 12 mm free length, ~2 N/mm (light detent).
+// Ball: Ø6 mm steel (standard bearing ball, purchase).
+module detent_plunger() {
+    bore_d   = DETENT_BALL_D + 0.2;   // 6.2 mm
+    body_od  = DETENT_SPG_OD;
+    body_len = DETENT_SPG_L;
+    spg_d    = 5.8;    // spring OD
+    spg_pocket = 10.0; // spring pocket depth (bottom of housing)
+
+    difference() {
+        cylinder(d = body_od, h = body_len);
+
+        // Ball socket (top — partial sphere, retains ball)
+        translate([0, 0, body_len])
+            sphere(d = bore_d);
+        translate([0, 0, body_len - bore_d/4])
+            cylinder(d = bore_d, h = bore_d/2 + e);
+
+        // Spring pocket (bottom)
+        translate([0, 0, -e])
+            cylinder(d = spg_d + 0.3, h = spg_pocket + e);
+
+        // Retention lip (allows push-in but prevents pullout before spring seated)
+        translate([0, 0, spg_pocket])
+            cylinder(d1 = spg_d + 0.3, d2 = spg_d - 1,
+                     h = 1.5);
+    }
+}
+
+// ============================================================
+// CROSS-VARIANT DECK ADAPTER PLATES
+// ============================================================
+// Thin plates that bolt to the robot deck and provide M4 threaded
+// studs (or through holes) for the rail mounting holes.
+// All adapters: RAIL_LEN × (RAIL_W + 2×ADAPT_OVHG) footprint.
+
+module _adapter_base() {
+    adapt_l = RAIL_LEN;
+    adapt_w = RAIL_W + 2*ADAPT_OVHG;
+    difference() {
+        // Plate
+        cube([adapt_w, adapt_l, ADAPT_T], center = true);
+
+        // Rail mounting holes (M4 FHCS up through adapter into rail bottom)
+        for (my = [MOUNT_INSET : MOUNT_PITCH : adapt_l - MOUNT_INSET])
+            translate([0, my - adapt_l/2, -ADAPT_T/2 - e])
+                cylinder(d = MOUNT_D, h = ADAPT_T + 2*e);
+
+        // Corner lightening
+        for (cx = [-1, 1]) for (cy = [-1, 1])
+            translate([cx * (adapt_w/2 - 12),
+                       cy * (adapt_l/2 - 20), 0])
+                cylinder(d = 10, h = ADAPT_T + 2*e, center = true);
+    }
+}
+
+// SaltyLab adapter: clears Ø25 mm stem, 4× M4 to lab chassis ring
+module lab_rail_adapter() {
+    difference() {
+        _adapter_base();
+        // Stem bore clearance (at centre of adapter)
+        cylinder(d = LAB_STEM_BORE, h = ADAPT_T + 2*e, center = true);
+        // 4× M4 mounting to lab chassis top ring (Ø44 mm bolt circle)
+        for (a = [45, 135, 225, 315])
+            translate([22*cos(a), 22*sin(a), -ADAPT_T/2 - e])
+                cylinder(d = M4_D, h = ADAPT_T + 2*e);
+    }
+}
+
+// SaltyRover adapter: 4× M4 to rover deck bolt grid
+module rover_rail_adapter() {
+    adapt_l = RAIL_LEN;
+    adapt_w = RAIL_W + 2*ADAPT_OVHG;
+    difference() {
+        _adapter_base();
+        // 2 rows × 3 cols of M4 bolts into rover deck
+        for (rx = [-ROVER_BOLT_SPC/2, ROVER_BOLT_SPC/2])
+        for (ry = [-adapt_l/3, 0, adapt_l/3])
+            translate([rx, ry, -ADAPT_T/2 - e])
+                cylinder(d = M4_D, h = ADAPT_T + 2*e);
+    }
+}
+
+// SaltyTank adapter: M4 to tank deck; relieved for deck cable slots
+module tank_rail_adapter() {
+    adapt_l = RAIL_LEN;
+    adapt_w = RAIL_W + 2*ADAPT_OVHG;
+    difference() {
+        _adapter_base();
+        // 2 rows × 3 cols of M4 bolts into tank deck
+        for (rx = [-TANK_BOLT_SPC/2, TANK_BOLT_SPC/2])
+        for (ry = [-adapt_l/3, 0, adapt_l/3])
+            translate([rx, ry, -ADAPT_T/2 - e])
+                cylinder(d = M4_D, h = ADAPT_T + 2*e);
+        // Deck cable slot clearance (tank deck has centre cable channel)
+        translate([0, 0, 0])
+            cube([10, adapt_l - 40, ADAPT_T + 2*e], center = true);
+    }
+}

jetson/ros2_ws/src/saltybot_bringup/config/rosbridge_params.yaml

@@ -40,6 +40,11 @@ rosbridge_websocket:
        "/person/target",
        "/person/detections",
        "/camera/*/image_raw/compressed",
+       "/camera/depth/image_rect_raw/compressed",
+       "/camera/panoramic/compressed",
+       "/social/faces/detections",
+       "/social/gestures",
+       "/social/scene/objects",
        "/scan",
        "/cmd_vel",
        "/saltybot/imu",

jetson/ros2_ws/src/saltybot_bringup/launch/rosbridge.launch.py

@@ -94,4 +94,33 @@ def generate_launch_description():
         for name in _CAMERAS
     ]
 
-    return LaunchDescription([rosbridge] + republishers)
+    # ── D435i colour republisher (Issue #177) ────────────────────────────────
+    d435i_color = Node(
+        package='image_transport',
+        executable='republish',
+        name='compress_d435i_color',
+        arguments=['raw', 'compressed'],
+        remappings=[
+            ('in',             '/camera/color/image_raw'),
+            ('out/compressed', '/camera/color/image_raw/compressed'),
+        ],
+        parameters=[{'compressed.jpeg_quality': _JPEG_QUALITY}],
+        output='screen',
+    )
+
+    # ── D435i depth republisher (Issue #177) ─────────────────────────────────
+    # Depth stream as compressedDepth (PNG16) — preserves uint16 depth values.
+    # Browser displays as greyscale PNG (darker = closer).
+    d435i_depth = Node(
+        package='image_transport',
+        executable='republish',
+        name='compress_d435i_depth',
+        arguments=['raw', 'compressedDepth'],
+        remappings=[
+            ('in',                  '/camera/depth/image_rect_raw'),
+            ('out/compressedDepth', '/camera/depth/image_rect_raw/compressed'),
+        ],
+        output='screen',
+    )
+
+    return LaunchDescription([rosbridge] + republishers + [d435i_color, d435i_depth])

jetson/ros2_ws/src/saltybot_dynamic_obs_msgs/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.8)
+project(saltybot_dynamic_obs_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/TrackedObject.msg"
+  "msg/MovingObjectArray.msg"
+  DEPENDENCIES std_msgs geometry_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

jetson/ros2_ws/src/saltybot_dynamic_obs_msgs/msg/MovingObjectArray.msg

@@ -0,0 +1,12 @@
+# MovingObjectArray — all currently tracked moving obstacles.
+#
+# Published at ~10 Hz on /saltybot/moving_objects.
+# Only confirmed tracks (hits >= confirm_frames) appear here.
+
+std_msgs/Header header
+
+saltybot_dynamic_obs_msgs/TrackedObject[] objects
+
+uint32 active_count        # number of confirmed tracks
+uint32 tentative_count     # tracks not yet confirmed
+float32 detector_latency_ms # pipeline latency hint

jetson/ros2_ws/src/saltybot_dynamic_obs_msgs/msg/TrackedObject.msg

@@ -0,0 +1,21 @@
+# TrackedObject — a single tracked moving obstacle.
+#
+# predicted_path[i] is the estimated pose at predicted_times[i] seconds from now.
+# All poses are in the same frame as header.frame_id (typically 'odom').
+
+std_msgs/Header header
+
+uint32 object_id               # stable ID across frames (monotonically increasing)
+
+geometry_msgs/PoseWithCovariance pose    # current best-estimate pose (x, y, yaw)
+geometry_msgs/Vector3            velocity # vx, vy in m/s (vz = 0 for ground objects)
+
+geometry_msgs/Pose[] predicted_path   # future positions at predicted_times
+float32[]            predicted_times  # seconds from header.stamp for each pose
+
+float32 speed_mps    # scalar |v|
+float32 confidence   # 0.0–1.0 (higher after more confirmed frames)
+
+uint32 age_frames    # frames since first detection
+uint32 hits          # number of successful associations
+bool   is_valid      # false if in tentative / just-created state

jetson/ros2_ws/src/saltybot_dynamic_obs_msgs/package.xml

@@ -0,0 +1,23 @@
+<?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_dynamic_obs_msgs</name>
+  <version>0.1.0</version>
+  <description>Custom message types for dynamic obstacle tracking.</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>geometry_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_dynamic_obstacles/config/dynamic_obstacles_params.yaml

@@ -0,0 +1,52 @@
+# saltybot_dynamic_obstacles — runtime parameters
+#
+# Requires:
+#   /scan  (sensor_msgs/LaserScan)  — RPLIDAR A1M8 at ~5.5 Hz
+#
+# LIDAR scan is published by rplidar_ros node.
+# Make sure RPLIDAR is running before starting this stack.
+
+dynamic_obs_tracker:
+  ros__parameters:
+    max_tracks:          20      # max simultaneous tracked objects
+    confirm_frames:       3      # hits before a track is published
+    max_missed_frames:    6      # missed frames before track deletion
+    assoc_dist_m:         1.5    # max assignment distance (Hungarian)
+    prediction_hz:       10.0    # tracker update + publish rate
+    horizon_s:            2.5    # prediction look-ahead
+    pred_step_s:          0.5    # time between predicted waypoints
+    odom_frame:          'odom'
+    min_speed_mps:        0.05   # suppress near-stationary tracks
+    max_range_m:          8.0    # ignore detections beyond this
+
+dynamic_obs_costmap:
+  ros__parameters:
+    inflation_radius_m:   0.35   # safety bubble around each predicted point
+    ring_points:          8      # polygon points for inflation circle
+    clear_on_empty:       true   # push empty cloud to clear stale Nav2 markings
+
+
+# ── Nav2 costmap integration ───────────────────────────────────────────────────
+# In your nav2_params.yaml, under local_costmap or global_costmap > plugins, add
+# an ObstacleLayer with:
+#
+#   obstacle_layer:
+#     plugin: "nav2_costmap_2d::ObstacleLayer"
+#     enabled: true
+#     observation_sources: static_scan dynamic_obs
+#     static_scan:
+#       topic: /scan
+#       data_type: LaserScan
+#       ...
+#     dynamic_obs:
+#       topic: /saltybot/dynamic_obs_cloud
+#       data_type: PointCloud2
+#       sensor_frame: odom
+#       obstacle_max_range: 10.0
+#       raytrace_max_range: 10.0
+#       marking: true
+#       clearing: false
+#
+# This feeds the predicted trajectory smear directly into Nav2's obstacle
+# inflation, forcing the planner to route around the predicted future path
+# of every tracked moving object.

jetson/ros2_ws/src/saltybot_dynamic_obstacles/launch/dynamic_obstacles.launch.py

@@ -0,0 +1,75 @@
+"""
+dynamic_obstacles.launch.py — Dynamic obstacle tracking + Nav2 costmap layer.
+
+Starts:
+  dynamic_obs_tracker  — LIDAR motion detection + Kalman tracking @10 Hz
+  dynamic_obs_costmap  — Predicted-trajectory → PointCloud2 for Nav2
+
+Launch args:
+  max_tracks         int    '20'
+  assoc_dist_m       float  '1.5'
+  horizon_s          float  '2.5'
+  inflation_radius_m float  '0.35'
+
+Verify:
+  ros2 topic hz /saltybot/moving_objects        # ~10 Hz
+  ros2 topic echo /saltybot/moving_objects      # TrackedObject list
+  ros2 topic hz /saltybot/dynamic_obs_cloud     # ~10 Hz (when objects present)
+  rviz2 → add MarkerArray → /saltybot/moving_objects_viz
+
+Nav2 costmap integration:
+  In your costmap_params.yaml ObstacleLayer observation_sources, add:
+    dynamic_obs:
+      topic: /saltybot/dynamic_obs_cloud
+      data_type: PointCloud2
+      marking: true
+      clearing: false
+"""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    args = [
+        DeclareLaunchArgument('max_tracks',         default_value='20'),
+        DeclareLaunchArgument('assoc_dist_m',        default_value='1.5'),
+        DeclareLaunchArgument('horizon_s',           default_value='2.5'),
+        DeclareLaunchArgument('inflation_radius_m',  default_value='0.35'),
+        DeclareLaunchArgument('min_speed_mps',       default_value='0.05'),
+    ]
+
+    tracker = Node(
+        package='saltybot_dynamic_obstacles',
+        executable='dynamic_obs_tracker',
+        name='dynamic_obs_tracker',
+        output='screen',
+        parameters=[{
+            'max_tracks':       LaunchConfiguration('max_tracks'),
+            'assoc_dist_m':     LaunchConfiguration('assoc_dist_m'),
+            'horizon_s':        LaunchConfiguration('horizon_s'),
+            'min_speed_mps':    LaunchConfiguration('min_speed_mps'),
+            'prediction_hz':    10.0,
+            'confirm_frames':   3,
+            'max_missed_frames': 6,
+            'pred_step_s':      0.5,
+            'odom_frame':       'odom',
+            'max_range_m':      8.0,
+        }],
+    )
+
+    costmap = Node(
+        package='saltybot_dynamic_obstacles',
+        executable='dynamic_obs_costmap',
+        name='dynamic_obs_costmap',
+        output='screen',
+        parameters=[{
+            'inflation_radius_m': LaunchConfiguration('inflation_radius_m'),
+            'ring_points':        8,
+            'clear_on_empty':     True,
+        }],
+    )
+
+    return LaunchDescription(args + [tracker, costmap])

jetson/ros2_ws/src/saltybot_dynamic_obstacles/package.xml

@@ -0,0 +1,29 @@
+<?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_dynamic_obstacles</name>
+  <version>0.1.0</version>
+  <description>
+    Dynamic obstacle detection, multi-object Kalman tracking, trajectory
+    prediction, and Nav2 costmap layer integration for SaltyBot.
+  </description>
+  <maintainer email="robot@saltylab.local">SaltyLab</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>saltybot_dynamic_obs_msgs</depend>
+
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-scipy</exec_depend>
+
+  <test_depend>pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_dynamic_obstacles/saltybot_dynamic_obstacles/costmap_layer_node.py

@@ -0,0 +1,178 @@
+"""
+costmap_layer_node.py — Nav2 costmap integration for dynamic obstacles.
+
+Converts predicted trajectories from /saltybot/moving_objects into a
+PointCloud2 fed into Nav2's ObstacleLayer.  Each predicted future position
+is added as a point, creating a "smeared" dynamic obstacle zone that
+covers the full 2-3 s prediction horizon.
+
+Nav2 ObstacleLayer config (in costmap_params.yaml):
+  obstacle_layer:
+    enabled: true
+    observation_sources: dynamic_obs
+    dynamic_obs:
+      topic: /saltybot/dynamic_obs_cloud
+      sensor_frame: odom
+      data_type: PointCloud2
+      obstacle_max_range: 12.0
+      obstacle_min_range: 0.0
+      raytrace_max_range: 12.0
+      marking: true
+      clearing: false   # let the tracker handle clearing
+
+The node also clears old obstacle points when tracks are dropped, by
+publishing a clearing cloud to /saltybot/dynamic_obs_clear.
+
+Subscribes:
+  /saltybot/moving_objects     saltybot_dynamic_obs_msgs/MovingObjectArray
+
+Publishes:
+  /saltybot/dynamic_obs_cloud  sensor_msgs/PointCloud2   marking cloud
+  /saltybot/dynamic_obs_clear  sensor_msgs/PointCloud2   clearing cloud
+
+Parameters:
+  inflation_radius_m  float  0.35   (each predicted point inflated by this radius)
+  ring_points         int    8      (polygon approximation of inflation circle)
+  clear_on_empty      bool   true   (publish clear cloud when no objects tracked)
+"""
+
+from __future__ import annotations
+
+import math
+import struct
+from typing import List
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+from sensor_msgs.msg import PointCloud2, PointField
+from std_msgs.msg import Header
+
+try:
+    from saltybot_dynamic_obs_msgs.msg import MovingObjectArray
+    _MSGS_AVAILABLE = True
+except ImportError:
+    _MSGS_AVAILABLE = False
+
+
+_RELIABLE_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.RELIABLE,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=10,
+)
+
+
+def _make_pc2(header: Header, points_xyz: List[tuple]) -> PointCloud2:
+    """Pack a list of (x, y, z) into a PointCloud2 message."""
+    fields = [
+        PointField(name='x', offset=0,  datatype=PointField.FLOAT32, count=1),
+        PointField(name='y', offset=4,  datatype=PointField.FLOAT32, count=1),
+        PointField(name='z', offset=8,  datatype=PointField.FLOAT32, count=1),
+    ]
+    point_step = 12   # 3 × float32
+    data = bytearray(len(points_xyz) * point_step)
+    for i, (x, y, z) in enumerate(points_xyz):
+        struct.pack_into('<fff', data, i * point_step, x, y, z)
+
+    pc = PointCloud2()
+    pc.header      = header
+    pc.height      = 1
+    pc.width       = len(points_xyz)
+    pc.fields      = fields
+    pc.is_bigendian = False
+    pc.point_step  = point_step
+    pc.row_step    = point_step * len(points_xyz)
+    pc.data        = bytes(data)
+    pc.is_dense    = True
+    return pc
+
+
+class CostmapLayerNode(Node):
+
+    def __init__(self):
+        super().__init__('dynamic_obs_costmap')
+
+        self.declare_parameter('inflation_radius_m', 0.35)
+        self.declare_parameter('ring_points',         8)
+        self.declare_parameter('clear_on_empty',      True)
+
+        self._infl_r      = self.get_parameter('inflation_radius_m').value
+        self._ring_n      = self.get_parameter('ring_points').value
+        self._clear_empty = self.get_parameter('clear_on_empty').value
+
+        # Pre-compute ring offsets for inflation
+        self._ring_offsets = [
+            (self._infl_r * math.cos(2 * math.pi * i / self._ring_n),
+             self._infl_r * math.sin(2 * math.pi * i / self._ring_n))
+            for i in range(self._ring_n)
+        ]
+
+        if _MSGS_AVAILABLE:
+            self.create_subscription(
+                MovingObjectArray,
+                '/saltybot/moving_objects',
+                self._on_objects,
+                _RELIABLE_QOS,
+            )
+        else:
+            self.get_logger().warning(
+                '[costmap_layer] saltybot_dynamic_obs_msgs not built — '
+                'will not subscribe to MovingObjectArray'
+            )
+
+        self._mark_pub  = self.create_publisher(
+            PointCloud2, '/saltybot/dynamic_obs_cloud', 10
+        )
+        self._clear_pub = self.create_publisher(
+            PointCloud2, '/saltybot/dynamic_obs_clear', 10
+        )
+
+        self.get_logger().info(
+            f'dynamic_obs_costmap ready — '
+            f'inflation={self._infl_r}m ring_pts={self._ring_n}'
+        )
+
+    # ── Callback ──────────────────────────────────────────────────────────────
+
+    def _on_objects(self, msg: 'MovingObjectArray') -> None:
+        hdr = msg.header
+        mark_pts: List[tuple] = []
+
+        for obj in msg.objects:
+            if not obj.is_valid:
+                continue
+
+            # Current position
+            self._add_inflated(obj.pose.pose.position.x,
+                               obj.pose.pose.position.y, mark_pts)
+
+            # Predicted future positions
+            for pose in obj.predicted_path:
+                self._add_inflated(pose.position.x, pose.position.y, mark_pts)
+
+        if mark_pts:
+            self._mark_pub.publish(_make_pc2(hdr, mark_pts))
+        elif self._clear_empty:
+            # Publish tiny clear cloud so Nav2 clears stale markings
+            self._clear_pub.publish(_make_pc2(hdr, []))
+
+    def _add_inflated(self, cx: float, cy: float, pts: List[tuple]) -> None:
+        """Add the centre + ring of inflation points at height 0.5 m."""
+        pts.append((cx, cy, 0.5))
+        for ox, oy in self._ring_offsets:
+            pts.append((cx + ox, cy + oy, 0.5))
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = CostmapLayerNode()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_dynamic_obstacles/saltybot_dynamic_obstacles/dynamic_obs_node.py

@@ -0,0 +1,319 @@
+"""
+dynamic_obs_node.py — ROS2 node: LIDAR moving-object detection + Kalman tracking.
+
+Pipeline:
+  1. Subscribe /scan (RPLIDAR LaserScan, ~5.5 Hz).
+  2. ObjectDetector performs background subtraction → moving blobs.
+  3. TrackerManager runs Hungarian assignment + Kalman predict/update at 10 Hz.
+  4. Publish /saltybot/moving_objects (MovingObjectArray).
+  5. Publish /saltybot/moving_objects_viz (MarkerArray) for RViz.
+
+The 10 Hz timer drives the tracker regardless of scan rate, so prediction
+continues between scans (pure-predict steps).
+
+Subscribes:
+  /scan                    sensor_msgs/LaserScan   RPLIDAR A1M8
+
+Publishes:
+  /saltybot/moving_objects          saltybot_dynamic_obs_msgs/MovingObjectArray
+  /saltybot/moving_objects_viz      visualization_msgs/MarkerArray
+
+Parameters:
+  max_tracks          int    20
+  confirm_frames      int    3
+  max_missed_frames   int    6
+  assoc_dist_m        float  1.5
+  prediction_hz       float  10.0     (tracker + publish rate)
+  horizon_s           float  2.5
+  pred_step_s         float  0.5
+  odom_frame          str    'odom'
+  min_speed_mps       float  0.05     (suppress near-zero velocity tracks)
+  max_range_m         float  8.0
+"""
+
+import time
+import math
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+from sensor_msgs.msg import LaserScan
+from geometry_msgs.msg import Pose, Point, Quaternion, Vector3
+from std_msgs.msg import Header, ColorRGBA
+from visualization_msgs.msg import Marker, MarkerArray
+
+try:
+    from saltybot_dynamic_obs_msgs.msg import TrackedObject, MovingObjectArray
+    _MSGS_AVAILABLE = True
+except ImportError:
+    _MSGS_AVAILABLE = False
+
+from .object_detector import ObjectDetector
+from .tracker_manager import TrackerManager, Track
+
+
+_SENSOR_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=5,
+)
+
+
+def _yaw_quat(yaw: float) -> Quaternion:
+    q = Quaternion()
+    q.w = math.cos(yaw * 0.5)
+    q.z = math.sin(yaw * 0.5)
+    return q
+
+
+class DynamicObsNode(Node):
+
+    def __init__(self):
+        super().__init__('dynamic_obs_tracker')
+
+        # ── Parameters ──────────────────────────────────────────────────────
+        self.declare_parameter('max_tracks',         20)
+        self.declare_parameter('confirm_frames',      3)
+        self.declare_parameter('max_missed_frames',   6)
+        self.declare_parameter('assoc_dist_m',        1.5)
+        self.declare_parameter('prediction_hz',      10.0)
+        self.declare_parameter('horizon_s',           2.5)
+        self.declare_parameter('pred_step_s',         0.5)
+        self.declare_parameter('odom_frame',         'odom')
+        self.declare_parameter('min_speed_mps',       0.05)
+        self.declare_parameter('max_range_m',         8.0)
+
+        max_tracks    = self.get_parameter('max_tracks').value
+        confirm_f     = self.get_parameter('confirm_frames').value
+        max_missed    = self.get_parameter('max_missed_frames').value
+        assoc_dist    = self.get_parameter('assoc_dist_m').value
+        pred_hz       = self.get_parameter('prediction_hz').value
+        horizon_s     = self.get_parameter('horizon_s').value
+        pred_step     = self.get_parameter('pred_step_s').value
+        self._frame   = self.get_parameter('odom_frame').value
+        self._min_spd = self.get_parameter('min_speed_mps').value
+        self._max_rng = self.get_parameter('max_range_m').value
+
+        # ── Core modules ────────────────────────────────────────────────────
+        self._detector = ObjectDetector(
+            grid_radius_m=min(self._max_rng + 2.0, 12.0),
+            max_cluster=int((self._max_rng / 0.1) ** 2 * 0.5),
+        )
+        self._tracker = TrackerManager(
+            max_tracks=max_tracks,
+            confirm_frames=confirm_f,
+            max_missed=max_missed,
+            assoc_dist_m=assoc_dist,
+            horizon_s=horizon_s,
+            pred_step_s=pred_step,
+        )
+        self._horizon_s  = horizon_s
+        self._pred_step  = pred_step
+
+        # ── State ────────────────────────────────────────────────────────────
+        self._latest_scan: LaserScan | None = None
+        self._last_track_t: float = time.monotonic()
+        self._scan_processed_stamp: float | None = None
+
+        # ── Subscriptions ────────────────────────────────────────────────────
+        self.create_subscription(LaserScan, '/scan', self._on_scan, _SENSOR_QOS)
+
+        # ── Publishers ───────────────────────────────────────────────────────
+        if _MSGS_AVAILABLE:
+            self._obj_pub = self.create_publisher(
+                MovingObjectArray, '/saltybot/moving_objects', 10
+            )
+        else:
+            self._obj_pub = None
+            self.get_logger().warning(
+                '[dyn_obs] saltybot_dynamic_obs_msgs not built — '
+                'MovingObjectArray will not be published'
+            )
+
+        self._viz_pub = self.create_publisher(
+            MarkerArray, '/saltybot/moving_objects_viz', 10
+        )
+
+        # ── Timer ────────────────────────────────────────────────────────────
+        self.create_timer(1.0 / pred_hz, self._track_tick)
+
+        self.get_logger().info(
+            f'dynamic_obs_tracker ready — '
+            f'max_tracks={max_tracks} horizon={horizon_s}s assoc={assoc_dist}m'
+        )
+
+    # ── Scan callback ─────────────────────────────────────────────────────────
+
+    def _on_scan(self, msg: LaserScan) -> None:
+        self._latest_scan = msg
+
+    # ── 10 Hz tracker tick ────────────────────────────────────────────────────
+
+    def _track_tick(self) -> None:
+        t0 = time.monotonic()
+
+        now_mono = t0
+        dt = now_mono - self._last_track_t
+        dt = max(1e-3, min(dt, 0.5))
+        self._last_track_t = now_mono
+
+        scan = self._latest_scan
+        detections = []
+
+        if scan is not None:
+            stamp_sec = scan.header.stamp.sec + scan.header.stamp.nanosec * 1e-9
+            if stamp_sec != self._scan_processed_stamp:
+                self._scan_processed_stamp = stamp_sec
+                ranges = np.asarray(scan.ranges, dtype=np.float32)
+                detections = self._detector.update(
+                    ranges,
+                    scan.angle_min,
+                    scan.angle_increment,
+                    min(scan.range_max, self._max_rng),
+                )
+
+        confirmed = self._tracker.update(detections, dt)
+
+        latency_ms = (time.monotonic() - t0) * 1000.0
+        stamp = self.get_clock().now().to_msg()
+
+        if _MSGS_AVAILABLE and self._obj_pub is not None:
+            self._publish_objects(confirmed, stamp, latency_ms)
+        self._publish_viz(confirmed, stamp)
+
+    # ── Publish helpers ───────────────────────────────────────────────────────
+
+    def _publish_objects(self, confirmed: list, stamp, latency_ms: float) -> None:
+        arr = MovingObjectArray()
+        arr.header.stamp    = stamp
+        arr.header.frame_id = self._frame
+        arr.active_count    = len(confirmed)
+        arr.tentative_count = self._tracker.tentative_count
+        arr.detector_latency_ms = float(latency_ms)
+
+        for tr in confirmed:
+            px, py   = tr.kalman.position
+            vx, vy   = tr.kalman.velocity
+            speed    = tr.kalman.speed
+            if speed < self._min_spd:
+                continue
+
+            obj = TrackedObject()
+            obj.header          = arr.header
+            obj.object_id       = tr.track_id
+            obj.pose.pose.position.x = px
+            obj.pose.pose.position.y = py
+            obj.pose.pose.orientation = _yaw_quat(math.atan2(vy, vx))
+            cov = tr.kalman.covariance_2x2
+            obj.pose.covariance[0]  = float(cov[0, 0])
+            obj.pose.covariance[1]  = float(cov[0, 1])
+            obj.pose.covariance[6]  = float(cov[1, 0])
+            obj.pose.covariance[7]  = float(cov[1, 1])
+            obj.velocity.x  = vx
+            obj.velocity.y  = vy
+            obj.speed_mps   = speed
+            obj.confidence  = min(1.0, tr.hits / (self._tracker._confirm_frames * 3))
+            obj.age_frames  = tr.age
+            obj.hits        = tr.hits
+            obj.is_valid    = True
+
+            # Predicted path
+            for px_f, py_f, t_f in tr.kalman.predict_horizon(
+                self._horizon_s, self._pred_step
+            ):
+                p = Pose()
+                p.position.x = px_f
+                p.position.y = py_f
+                p.orientation.w = 1.0
+                obj.predicted_path.append(p)
+                obj.predicted_times.append(float(t_f))
+
+            arr.objects.append(obj)
+
+        self._obj_pub.publish(arr)
+
+    def _publish_viz(self, confirmed: list, stamp) -> None:
+        markers = MarkerArray()
+
+        # Delete old markers
+        del_marker = Marker()
+        del_marker.header.stamp    = stamp
+        del_marker.header.frame_id = self._frame
+        del_marker.action = Marker.DELETEALL
+        markers.markers.append(del_marker)
+
+        for tr in confirmed:
+            px, py = tr.kalman.position
+            vx, vy = tr.kalman.velocity
+            speed  = tr.kalman.speed
+            if speed < self._min_spd:
+                continue
+
+            # Cylinder at current position
+            m = Marker()
+            m.header.stamp    = stamp
+            m.header.frame_id = self._frame
+            m.ns     = 'dyn_obs'
+            m.id     = tr.track_id
+            m.type   = Marker.CYLINDER
+            m.action = Marker.ADD
+            m.pose.position.x = px
+            m.pose.position.y = py
+            m.pose.position.z = 0.5
+            m.pose.orientation.w = 1.0
+            m.scale.x = 0.4
+            m.scale.y = 0.4
+            m.scale.z = 1.0
+            m.color   = ColorRGBA(r=1.0, g=0.2, b=0.0, a=0.7)
+            m.lifetime.sec = 1
+            markers.markers.append(m)
+
+            # Arrow for velocity
+            vel_m = Marker()
+            vel_m.header  = m.header
+            vel_m.ns      = 'dyn_obs_vel'
+            vel_m.id      = tr.track_id
+            vel_m.type    = Marker.ARROW
+            vel_m.action  = Marker.ADD
+            from geometry_msgs.msg import Point as GPoint
+            p_start = GPoint(x=px, y=py, z=1.0)
+            p_end   = GPoint(x=px + vx, y=py + vy, z=1.0)
+            vel_m.points  = [p_start, p_end]
+            vel_m.scale.x = 0.05
+            vel_m.scale.y = 0.10
+            vel_m.color   = ColorRGBA(r=1.0, g=1.0, b=0.0, a=0.9)
+            vel_m.lifetime.sec = 1
+            markers.markers.append(vel_m)
+
+            # Line strip for predicted path
+            path_m = Marker()
+            path_m.header  = m.header
+            path_m.ns      = 'dyn_obs_path'
+            path_m.id      = tr.track_id
+            path_m.type    = Marker.LINE_STRIP
+            path_m.action  = Marker.ADD
+            path_m.scale.x = 0.04
+            path_m.color   = ColorRGBA(r=1.0, g=0.5, b=0.0, a=0.5)
+            path_m.lifetime.sec = 1
+            path_m.points.append(GPoint(x=px, y=py, z=0.5))
+            for fx, fy, _ in tr.kalman.predict_horizon(self._horizon_s, self._pred_step):
+                path_m.points.append(GPoint(x=fx, y=fy, z=0.5))
+            markers.markers.append(path_m)
+
+        self._viz_pub.publish(markers)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = DynamicObsNode()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_dynamic_obstacles/saltybot_dynamic_obstacles/kalman_tracker.py

@@ -0,0 +1,132 @@
+"""
+kalman_tracker.py — Single-object Kalman filter for 2-D ground-plane tracking.
+
+State vector:  x = [px, py, vx, vy]   (position + velocity)
+Motion model:  constant-velocity (CV) with process noise on acceleration
+
+Predict step:
+    F = | 1 0 dt  0 |      x_{k|k-1} = F @ x_{k-1|k-1}
+        | 0 1  0 dt |      P_{k|k-1} = F @ P @ F^T + Q
+        | 0 0  1  0 |
+        | 0 0  0  1 |
+
+Update step (position observation only):
+    H = | 1 0 0 0 |      y   = z - H @ x
+        | 0 1 0 0 |      S   = H @ P @ H^T + R
+                          K   = P @ H^T @ inv(S)
+                          x   = x + K @ y
+                          P   = (I - K @ H) @ P   (Joseph form for stability)
+
+Trajectory prediction: unrolls the CV model forward at fixed time steps.
+"""
+
+from __future__ import annotations
+
+from typing import List, Tuple
+
+import numpy as np
+
+
+# ── Default noise matrices ────────────────────────────────────────────────────
+# Process noise: models uncertainty in acceleration between frames
+_Q_BASE = np.diag([0.02, 0.02, 0.8, 0.8]).astype(np.float64)
+
+# Measurement noise: LIDAR centroid uncertainty (~0.15 m std)
+_R = np.diag([0.025, 0.025]).astype(np.float64)   # 0.16 m sigma each axis
+
+# Observation matrix
+_H = np.array([[1, 0, 0, 0],
+               [0, 1, 0, 0]], dtype=np.float64)
+_I4 = np.eye(4, dtype=np.float64)
+
+
+class KalmanTracker:
+    """
+    Kalman filter tracking one object.
+
+    Parameters
+    ----------
+    x0, y0         : initial position (metres, odom frame)
+    process_noise  : scalar multiplier on _Q_BASE
+    """
+
+    def __init__(self, x0: float, y0: float, process_noise: float = 1.0):
+        self._x = np.array([x0, y0, 0.0, 0.0], dtype=np.float64)
+        self._P = np.eye(4, dtype=np.float64) * 0.5
+        self._Q = _Q_BASE * process_noise
+
+    # ── Core filter ───────────────────────────────────────────────────────────
+
+    def predict(self, dt: float) -> None:
+        """Propagate state by dt seconds."""
+        F = np.array([
+            [1, 0, dt, 0],
+            [0, 1,  0, dt],
+            [0, 0,  1,  0],
+            [0, 0,  0,  1],
+        ], dtype=np.float64)
+        self._x = F @ self._x
+        self._P = F @ self._P @ F.T + self._Q
+
+    def update(self, z: np.ndarray) -> None:
+        """
+        Incorporate a position measurement z = [x, y].
+        Uses Joseph-form covariance update for numerical stability.
+        """
+        y = z.astype(np.float64) - _H @ self._x
+        S = _H @ self._P @ _H.T + _R
+        K = self._P @ _H.T @ np.linalg.inv(S)
+        self._x = self._x + K @ y
+        IKH = _I4 - K @ _H
+        # Joseph form: (I-KH) P (I-KH)^T + K R K^T
+        self._P = IKH @ self._P @ IKH.T + K @ _R @ K.T
+
+    # ── Prediction horizon ────────────────────────────────────────────────────
+
+    def predict_horizon(
+        self,
+        horizon_s: float = 2.5,
+        step_s: float = 0.5,
+    ) -> List[Tuple[float, float, float]]:
+        """
+        Return [(x, y, t), ...] at equally-spaced future times.
+
+        Does NOT modify internal filter state.
+        """
+        predictions: List[Tuple[float, float, float]] = []
+        state = self._x.copy()
+        t = 0.0
+        F_step = np.array([
+            [1, 0, step_s,      0],
+            [0, 1,      0, step_s],
+            [0, 0,      1,      0],
+            [0, 0,      0,      1],
+        ], dtype=np.float64)
+        while t < horizon_s - 1e-6:
+            state = F_step @ state
+            t += step_s
+            predictions.append((float(state[0]), float(state[1]), t))
+        return predictions
+
+    # ── Properties ────────────────────────────────────────────────────────────
+
+    @property
+    def position(self) -> Tuple[float, float]:
+        return float(self._x[0]), float(self._x[1])
+
+    @property
+    def velocity(self) -> Tuple[float, float]:
+        return float(self._x[2]), float(self._x[3])
+
+    @property
+    def speed(self) -> float:
+        return float(np.hypot(self._x[2], self._x[3]))
+
+    @property
+    def covariance_2x2(self) -> np.ndarray:
+        """Position covariance (top-left 2×2 of P)."""
+        return self._P[:2, :2].copy()
+
+    @property
+    def state(self) -> np.ndarray:
+        return self._x.copy()

jetson/ros2_ws/src/saltybot_dynamic_obstacles/saltybot_dynamic_obstacles/object_detector.py

@@ -0,0 +1,168 @@
+"""
+object_detector.py — LIDAR-based moving object detector.
+
+Algorithm:
+  1. Convert each LaserScan to a 2-D occupancy grid (robot-centred, fixed size).
+  2. Maintain a background model via exponential moving average (EMA):
+       bg_t = α * current + (1-α) * bg_{t-1}   (only for non-moving cells)
+  3. Foreground = cells whose occupancy significantly exceeds the background.
+  4. Cluster foreground cells with scipy connected-components → Detection list.
+
+The grid is robot-relative (origin at robot centre) so it naturally tracks
+the robot's motion without needing TF at this stage.  The caller is responsible
+for transforming detections into a stable frame (odom) before passing to the
+tracker.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from typing import List, Optional
+
+import numpy as np
+from scipy import ndimage
+
+
+@dataclass
+class Detection:
+    """A clustered moving foreground blob from one scan."""
+    x: float          # centroid x in sensor frame (m)
+    y: float          # centroid y in sensor frame (m)
+    size_m2: float    # approximate area of the cluster (m²)
+    range_m: float    # distance from robot (m)
+
+
+class ObjectDetector:
+    """
+    Detects moving objects in consecutive 2-D LIDAR scans.
+
+    Parameters
+    ----------
+    grid_radius_m : half-size of the occupancy grid (grid covers ±radius)
+    resolution    : metres per cell
+    bg_alpha      : EMA update rate for background (small = slow forgetting)
+    motion_thr    : occupancy delta above background to count as moving
+    min_cluster   : minimum cells to keep a cluster
+    max_cluster   : maximum cells before a cluster is considered static wall
+    """
+
+    def __init__(
+        self,
+        grid_radius_m: float = 10.0,
+        resolution: float = 0.10,
+        bg_alpha: float = 0.04,
+        motion_thr: float = 0.45,
+        min_cluster: int = 3,
+        max_cluster: int = 200,
+    ):
+        cells = int(2 * grid_radius_m / resolution)
+        self._cells     = cells
+        self._res       = resolution
+        self._origin    = -grid_radius_m        # world x/y at grid index 0
+        self._bg_alpha  = bg_alpha
+        self._motion_thr = motion_thr
+        self._min_clust = min_cluster
+        self._max_clust = max_cluster
+
+        self._bg = np.zeros((cells, cells), dtype=np.float32)
+        self._initialized = False
+
+    # ── Public API ────────────────────────────────────────────────────────────
+
+    def update(
+        self,
+        ranges: np.ndarray,
+        angle_min: float,
+        angle_increment: float,
+        range_max: float,
+    ) -> List[Detection]:
+        """
+        Process one LaserScan and return detected moving blobs.
+
+        Parameters
+        ----------
+        ranges          : 1-D array of range readings (metres)
+        angle_min       : angle of first beam (radians)
+        angle_increment : angular step between beams (radians)
+        range_max       : maximum valid range (metres)
+        """
+        curr = self._scan_to_grid(ranges, angle_min, angle_increment, range_max)
+
+        if not self._initialized:
+            self._bg = curr.copy()
+            self._initialized = True
+            return []
+
+        # Foreground mask
+        motion_mask = (curr - self._bg) > self._motion_thr
+
+        # Update background only on non-moving cells
+        static = ~motion_mask
+        self._bg[static] = (
+            self._bg[static] * (1.0 - self._bg_alpha)
+            + curr[static] * self._bg_alpha
+        )
+
+        return self._cluster(motion_mask)
+
+    def reset(self) -> None:
+        self._bg[:] = 0.0
+        self._initialized = False
+
+    # ── Internals ─────────────────────────────────────────────────────────────
+
+    def _scan_to_grid(
+        self,
+        ranges: np.ndarray,
+        angle_min: float,
+        angle_increment: float,
+        range_max: float,
+    ) -> np.ndarray:
+        grid = np.zeros((self._cells, self._cells), dtype=np.float32)
+        n = len(ranges)
+        angles = angle_min + np.arange(n) * angle_increment
+        r = np.asarray(ranges, dtype=np.float32)
+
+        valid = (r > 0.05) & (r < range_max) & np.isfinite(r)
+        r, a = r[valid], angles[valid]
+
+        x = r * np.cos(a)
+        y = r * np.sin(a)
+
+        ix = np.clip(
+            ((x - self._origin) / self._res).astype(np.int32), 0, self._cells - 1
+        )
+        iy = np.clip(
+            ((y - self._origin) / self._res).astype(np.int32), 0, self._cells - 1
+        )
+        grid[iy, ix] = 1.0
+        return grid
+
+    def _cluster(self, mask: np.ndarray) -> List[Detection]:
+        # Dilate slightly to connect nearby hit cells into one blob
+        struct = ndimage.generate_binary_structure(2, 2)
+        dilated = ndimage.binary_dilation(mask, structure=struct, iterations=1)
+
+        labeled, n_labels = ndimage.label(dilated)
+        detections: List[Detection] = []
+
+        for label_id in range(1, n_labels + 1):
+            coords = np.argwhere(labeled == label_id)
+            n_cells = len(coords)
+            if n_cells < self._min_clust or n_cells > self._max_clust:
+                continue
+
+            ys, xs = coords[:, 0], coords[:, 1]
+            cx_grid = float(np.mean(xs))
+            cy_grid = float(np.mean(ys))
+            cx = cx_grid * self._res + self._origin
+            cy = cy_grid * self._res + self._origin
+
+            detections.append(Detection(
+                x=cx,
+                y=cy,
+                size_m2=n_cells * self._res ** 2,
+                range_m=float(np.hypot(cx, cy)),
+            ))
+
+        return detections

jetson/ros2_ws/src/saltybot_dynamic_obstacles/saltybot_dynamic_obstacles/tracker_manager.py

@@ -0,0 +1,206 @@
+"""
+tracker_manager.py — Multi-object tracker with Hungarian data association.
+
+Track lifecycle:
+  TENTATIVE → confirmed after `confirm_frames` consecutive hits
+  CONFIRMED → normal tracked state
+  LOST      → missed for 1..max_missed frames (still predicts, not published)
+  DEAD      → missed > max_missed → removed
+
+Association:
+  Uses scipy.optimize.linear_sum_assignment (Hungarian algorithm) on a cost
+  matrix of Euclidean distances between predicted track positions and new
+  detections.  Assignments with cost > assoc_dist_m are rejected.
+
+Up to `max_tracks` simultaneous live tracks (tentative + confirmed).
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from enum import IntEnum
+from typing import Dict, List, Optional, Tuple
+
+import numpy as np
+from scipy.optimize import linear_sum_assignment
+
+from .kalman_tracker import KalmanTracker
+from .object_detector import Detection
+
+
+class TrackState(IntEnum):
+    TENTATIVE = 0
+    CONFIRMED = 1
+    LOST      = 2
+
+
+@dataclass
+class Track:
+    track_id:   int
+    kalman:     KalmanTracker
+    state:      TrackState = TrackState.TENTATIVE
+    hits:       int = 1
+    age:        int = 1      # frames since creation
+    missed:     int = 0      # consecutive missed frames
+
+
+class TrackerManager:
+    """
+    Manages a pool of Kalman tracks.
+
+    Parameters
+    ----------
+    max_tracks      : hard cap on simultaneously alive tracks
+    confirm_frames  : hits needed before a track is CONFIRMED
+    max_missed      : consecutive missed frames before a track is DEAD
+    assoc_dist_m    : max allowed distance (m) for a valid assignment
+    horizon_s       : prediction horizon for trajectory output (seconds)
+    pred_step_s     : time step between predicted waypoints
+    process_noise   : KalmanTracker process-noise multiplier
+    """
+
+    def __init__(
+        self,
+        max_tracks:     int   = 20,
+        confirm_frames: int   = 3,
+        max_missed:     int   = 6,
+        assoc_dist_m:   float = 1.5,
+        horizon_s:      float = 2.5,
+        pred_step_s:    float = 0.5,
+        process_noise:  float = 1.0,
+    ):
+        self._max_tracks     = max_tracks
+        self._confirm_frames = confirm_frames
+        self._max_missed     = max_missed
+        self._assoc_dist     = assoc_dist_m
+        self._horizon_s      = horizon_s
+        self._pred_step      = pred_step_s
+        self._proc_noise     = process_noise
+
+        self._tracks:  Dict[int, Track] = {}
+        self._next_id: int = 1
+
+    # ── Public API ────────────────────────────────────────────────────────────
+
+    def update(self, detections: List[Detection], dt: float) -> List[Track]:
+        """
+        Process one frame of detections.
+
+        1. Predict all tracks by dt.
+        2. Hungarian assignment of predictions → detections.
+        3. Update matched tracks; mark unmatched tracks as LOST.
+        4. Promote tracks crossing `confirm_frames`.
+        5. Create new tracks for unmatched detections (if room).
+        6. Remove DEAD tracks.
+
+        Returns confirmed tracks only.
+        """
+        # 1. Predict
+        for tr in self._tracks.values():
+            tr.kalman.predict(dt)
+            tr.age += 1
+
+        # 2. Assign
+        matched, unmatched_tracks, unmatched_dets = self._assign(detections)
+
+        # 3a. Update matched
+        for tid, did in matched:
+            tr = self._tracks[tid]
+            det = detections[did]
+            tr.kalman.update(np.array([det.x, det.y]))
+            tr.hits   += 1
+            tr.missed  = 0
+            if tr.state == TrackState.LOST:
+                tr.state = TrackState.CONFIRMED
+            elif tr.state == TrackState.TENTATIVE and tr.hits >= self._confirm_frames:
+                tr.state = TrackState.CONFIRMED
+
+        # 3b. Unmatched tracks
+        for tid in unmatched_tracks:
+            tr = self._tracks[tid]
+            tr.missed += 1
+            if tr.missed > 1:
+                tr.state = TrackState.LOST
+
+        # 4. New tracks for unmatched detections
+        live = sum(1 for t in self._tracks.values() if t.state != TrackState.LOST
+                   or t.missed <= self._max_missed)
+        for did in unmatched_dets:
+            if live >= self._max_tracks:
+                break
+            det = detections[did]
+            init_state = (TrackState.CONFIRMED
+                          if self._confirm_frames <= 1
+                          else TrackState.TENTATIVE)
+            new_tr = Track(
+                track_id=self._next_id,
+                kalman=KalmanTracker(det.x, det.y, self._proc_noise),
+                state=init_state,
+            )
+            self._tracks[self._next_id] = new_tr
+            self._next_id += 1
+            live += 1
+
+        # 5. Prune dead
+        dead = [tid for tid, t in self._tracks.items() if t.missed > self._max_missed]
+        for tid in dead:
+            del self._tracks[tid]
+
+        return [t for t in self._tracks.values() if t.state == TrackState.CONFIRMED]
+
+    @property
+    def all_tracks(self) -> List[Track]:
+        return list(self._tracks.values())
+
+    @property
+    def tentative_count(self) -> int:
+        return sum(1 for t in self._tracks.values()
+                   if t.state == TrackState.TENTATIVE)
+
+    def reset(self) -> None:
+        self._tracks.clear()
+        self._next_id = 1
+
+    # ── Hungarian assignment ──────────────────────────────────────────────────
+
+    def _assign(
+        self,
+        detections: List[Detection],
+    ) -> Tuple[List[Tuple[int, int]], List[int], List[int]]:
+        """
+        Returns:
+          matched        — list of (track_id, det_index)
+          unmatched_tids — track IDs with no detection assigned
+          unmatched_dids — detection indices with no track assigned
+        """
+        track_ids = list(self._tracks.keys())
+
+        if not track_ids or not detections:
+            return [], track_ids, list(range(len(detections)))
+
+        # Build cost matrix: rows=tracks, cols=detections
+        cost = np.full((len(track_ids), len(detections)), fill_value=np.inf)
+        for r, tid in enumerate(track_ids):
+            tx, ty = self._tracks[tid].kalman.position
+            for c, det in enumerate(detections):
+                cost[r, c] = np.hypot(tx - det.x, ty - det.y)
+
+        row_ind, col_ind = linear_sum_assignment(cost)
+
+        matched:          List[Tuple[int, int]] = []
+        matched_track_idx: set = set()
+        matched_det_idx:   set = set()
+
+        for r, c in zip(row_ind, col_ind):
+            if cost[r, c] > self._assoc_dist:
+                continue
+            matched.append((track_ids[r], c))
+            matched_track_idx.add(r)
+            matched_det_idx.add(c)
+
+        unmatched_tids = [track_ids[r] for r in range(len(track_ids))
+                          if r not in matched_track_idx]
+        unmatched_dids = [c for c in range(len(detections))
+                          if c not in matched_det_idx]
+
+        return matched, unmatched_tids, unmatched_dids

jetson/ros2_ws/src/saltybot_dynamic_obstacles/setup.cfg

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

jetson/ros2_ws/src/saltybot_dynamic_obstacles/setup.py

@@ -0,0 +1,32 @@
+from setuptools import setup, find_packages
+from glob import glob
+
+package_name = 'saltybot_dynamic_obstacles'
+
+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 + '/launch',
+            glob('launch/*.launch.py')),
+        ('share/' + package_name + '/config',
+            glob('config/*.yaml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='SaltyLab',
+    maintainer_email='robot@saltylab.local',
+    description='Dynamic obstacle tracking: LIDAR motion detection, Kalman tracking, Nav2 costmap',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'dynamic_obs_tracker = saltybot_dynamic_obstacles.dynamic_obs_node:main',
+            'dynamic_obs_costmap = saltybot_dynamic_obstacles.costmap_layer_node:main',
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_dynamic_obstacles/test/test_dynamic_obstacles.py

@@ -0,0 +1,262 @@
+"""
+test_dynamic_obstacles.py — Unit tests for KalmanTracker, TrackerManager,
+and ObjectDetector.
+
+Runs without ROS2 / hardware (no rclpy imports).
+"""
+
+from __future__ import annotations
+
+import math
+import sys
+import os
+
+import numpy as np
+import pytest
+
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
+
+from saltybot_dynamic_obstacles.kalman_tracker import KalmanTracker
+from saltybot_dynamic_obstacles.tracker_manager import TrackerManager, TrackState
+from saltybot_dynamic_obstacles.object_detector import ObjectDetector, Detection
+
+
+# ── KalmanTracker ─────────────────────────────────────────────────────────────
+
+class TestKalmanTracker:
+
+    def test_initial_position(self):
+        kt = KalmanTracker(3.0, 4.0)
+        px, py = kt.position
+        assert px == pytest.approx(3.0)
+        assert py == pytest.approx(4.0)
+
+    def test_initial_velocity_zero(self):
+        kt = KalmanTracker(0.0, 0.0)
+        vx, vy = kt.velocity
+        assert vx == pytest.approx(0.0)
+        assert vy == pytest.approx(0.0)
+
+    def test_predict_moves_position(self):
+        kt = KalmanTracker(0.0, 0.0)
+        # Give it some velocity via update sequence
+        kt.update(np.array([0.1, 0.0]))
+        kt.update(np.array([0.2, 0.0]))
+        kt.predict(0.1)
+        px, _ = kt.position
+        assert px > 0.0   # should have moved forward
+
+    def test_pure_predict_constant_velocity(self):
+        """After velocity is established, predict() should move linearly."""
+        kt = KalmanTracker(0.0, 0.0)
+        # Force velocity by repeated updates
+        for i in range(10):
+            kt.update(np.array([i * 0.1, 0.0]))
+            kt.predict(0.1)
+        vx, _ = kt.velocity
+        px0, _ = kt.position
+        kt.predict(1.0)
+        px1, _ = kt.position
+        # Should advance roughly vx * 1.0 metres
+        assert px1 == pytest.approx(px0 + vx * 1.0, abs=0.3)
+
+    def test_update_corrects_position(self):
+        kt = KalmanTracker(0.0, 0.0)
+        # Predict way off
+        kt.predict(10.0)
+        # Then update to ground truth
+        kt.update(np.array([1.0, 2.0]))
+        px, py = kt.position
+        # Should move toward (1, 2)
+        assert px == pytest.approx(1.0, abs=0.5)
+        assert py == pytest.approx(2.0, abs=0.5)
+
+    def test_predict_horizon_length(self):
+        kt = KalmanTracker(0.0, 0.0)
+        preds = kt.predict_horizon(horizon_s=2.5, step_s=0.5)
+        assert len(preds) == 5   # 0.5, 1.0, 1.5, 2.0, 2.5
+
+    def test_predict_horizon_times(self):
+        kt = KalmanTracker(0.0, 0.0)
+        preds = kt.predict_horizon(horizon_s=2.0, step_s=0.5)
+        times = [t for _, _, t in preds]
+        assert times == pytest.approx([0.5, 1.0, 1.5, 2.0], abs=0.01)
+
+    def test_predict_horizon_does_not_mutate_state(self):
+        kt = KalmanTracker(1.0, 2.0)
+        kt.predict_horizon(horizon_s=3.0, step_s=0.5)
+        px, py = kt.position
+        assert px == pytest.approx(1.0)
+        assert py == pytest.approx(2.0)
+
+    def test_speed_zero_at_init(self):
+        kt = KalmanTracker(5.0, 5.0)
+        assert kt.speed == pytest.approx(0.0)
+
+    def test_covariance_shape(self):
+        kt = KalmanTracker(0.0, 0.0)
+        cov = kt.covariance_2x2
+        assert cov.shape == (2, 2)
+
+    def test_covariance_positive_definite(self):
+        kt = KalmanTracker(0.0, 0.0)
+        for _ in range(5):
+            kt.predict(0.1)
+            kt.update(np.array([0.1, 0.0]))
+        eigvals = np.linalg.eigvalsh(kt.covariance_2x2)
+        assert np.all(eigvals > 0)
+
+    def test_joseph_form_stays_symmetric(self):
+        """Covariance should remain symmetric after many updates."""
+        kt = KalmanTracker(0.0, 0.0)
+        for i in range(50):
+            kt.predict(0.1)
+            kt.update(np.array([i * 0.01, 0.0]))
+        P = kt._P
+        assert np.allclose(P, P.T, atol=1e-10)
+
+
+# ── TrackerManager ────────────────────────────────────────────────────────────
+
+class TestTrackerManager:
+
+    def _det(self, x, y):
+        return Detection(x=x, y=y, size_m2=0.1, range_m=math.hypot(x, y))
+
+    def test_empty_detections_no_tracks(self):
+        tm = TrackerManager()
+        confirmed = tm.update([], 0.1)
+        assert confirmed == []
+
+    def test_track_created_on_detection(self):
+        tm = TrackerManager(confirm_frames=1)
+        confirmed = tm.update([self._det(1.0, 0.0)], 0.1)
+        assert len(confirmed) == 1
+
+    def test_track_tentative_before_confirm(self):
+        tm = TrackerManager(confirm_frames=3)
+        tm.update([self._det(1.0, 0.0)], 0.1)
+        # Only 1 hit — should still be tentative
+        assert tm.tentative_count == 1
+
+    def test_track_confirmed_after_N_hits(self):
+        tm = TrackerManager(confirm_frames=3, assoc_dist_m=2.0)
+        for _ in range(4):
+            confirmed = tm.update([self._det(1.0, 0.0)], 0.1)
+        assert len(confirmed) == 1
+
+    def test_track_deleted_after_max_missed(self):
+        tm = TrackerManager(confirm_frames=1, max_missed=3, assoc_dist_m=2.0)
+        tm.update([self._det(1.0, 0.0)], 0.1)   # create
+        for _ in range(5):
+            tm.update([], 0.1)   # no detections → missed++
+        assert len(tm.all_tracks) == 0
+
+    def test_max_tracks_cap(self):
+        tm = TrackerManager(max_tracks=5, confirm_frames=1)
+        dets = [self._det(float(i), 0.0) for i in range(10)]
+        tm.update(dets, 0.1)
+        assert len(tm.all_tracks) <= 5
+
+    def test_consistent_track_id(self):
+        tm = TrackerManager(confirm_frames=3, assoc_dist_m=1.5)
+        for i in range(5):
+            confirmed = tm.update([self._det(1.0 + i * 0.01, 0.0)], 0.1)
+        assert len(confirmed) == 1
+        track_id = confirmed[0].track_id
+        # One more tick — ID should be stable
+        confirmed2 = tm.update([self._det(1.06, 0.0)], 0.1)
+        assert confirmed2[0].track_id == track_id
+
+    def test_two_independent_tracks(self):
+        tm = TrackerManager(confirm_frames=3, assoc_dist_m=0.8)
+        for _ in range(5):
+            confirmed = tm.update([self._det(1.0, 0.0), self._det(5.0, 0.0)], 0.1)
+        assert len(confirmed) == 2
+
+    def test_reset_clears_all(self):
+        tm = TrackerManager(confirm_frames=1)
+        tm.update([self._det(1.0, 0.0)], 0.1)
+        tm.reset()
+        assert len(tm.all_tracks) == 0
+
+    def test_far_detection_not_assigned(self):
+        tm = TrackerManager(confirm_frames=1, assoc_dist_m=0.5)
+        tm.update([self._det(1.0, 0.0)], 0.1)   # create track at (1,0)
+        # Detection 3 m away → new track, not update
+        tm.update([self._det(4.0, 0.0)], 0.1)
+        assert len(tm.all_tracks) == 2
+
+
+# ── ObjectDetector ────────────────────────────────────────────────────────────
+
+class TestObjectDetector:
+
+    def _empty_scan(self, n=360, rmax=8.0) -> tuple:
+        """All readings at max range (static background)."""
+        ranges = np.full(n, rmax - 0.1, dtype=np.float32)
+        return ranges, -math.pi, 2 * math.pi / n, rmax
+
+    def _scan_with_blob(self, blob_r=2.0, blob_theta=0.0, n=360, rmax=8.0) -> tuple:
+        """Background scan + a short-range cluster at blob_theta."""
+        ranges = np.full(n, rmax - 0.1, dtype=np.float32)
+        angle_inc = 2 * math.pi / n
+        angle_min = -math.pi
+        # Put a cluster of ~10 beams at blob_r
+        center_idx = int((blob_theta - angle_min) / angle_inc) % n
+        for di in range(-5, 6):
+            idx = (center_idx + di) % n
+            ranges[idx] = blob_r
+        return ranges, angle_min, angle_inc, rmax
+
+    def test_empty_scan_no_detections_after_warmup(self):
+        od = ObjectDetector()
+        r, a_min, a_inc, rmax = self._empty_scan()
+        od.update(r, a_min, a_inc, rmax)   # init background
+        for _ in range(3):
+            dets = od.update(r, a_min, a_inc, rmax)
+        assert len(dets) == 0
+
+    def test_moving_blob_detected(self):
+        od = ObjectDetector()
+        bg_r, a_min, a_inc, rmax = self._empty_scan()
+        od.update(bg_r, a_min, a_inc, rmax)   # seed background
+        for _ in range(5):
+            od.update(bg_r, a_min, a_inc, rmax)
+        # Now inject a foreground blob
+        fg_r, _, _, _ = self._scan_with_blob(blob_r=2.0, blob_theta=0.0)
+        dets = od.update(fg_r, a_min, a_inc, rmax)
+        assert len(dets) >= 1
+
+    def test_detection_centroid_approximate(self):
+        od = ObjectDetector()
+        bg_r, a_min, a_inc, rmax = self._empty_scan()
+        for _ in range(8):
+            od.update(bg_r, a_min, a_inc, rmax)
+        fg_r, _, _, _ = self._scan_with_blob(blob_r=3.0, blob_theta=0.0)
+        dets = od.update(fg_r, a_min, a_inc, rmax)
+        assert len(dets) >= 1
+        # Blob is at ~3 m along x-axis (theta=0)
+        cx = dets[0].x
+        cy = dets[0].y
+        assert abs(cx - 3.0) < 0.5
+        assert abs(cy) < 0.5
+
+    def test_reset_clears_background(self):
+        od = ObjectDetector()
+        bg_r, a_min, a_inc, rmax = self._empty_scan()
+        for _ in range(5):
+            od.update(bg_r, a_min, a_inc, rmax)
+        od.reset()
+        assert not od._initialized
+
+    def test_no_inf_nan_ranges(self):
+        od = ObjectDetector()
+        r = np.array([np.inf, np.nan, 5.0, -1.0, 0.0] * 72, dtype=np.float32)
+        a_min = -math.pi
+        a_inc = 2 * math.pi / len(r)
+        od.update(r, a_min, a_inc, 8.0)   # should not raise
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])

jetson/ros2_ws/src/saltybot_emergency/config/emergency_params.yaml

@@ -0,0 +1,43 @@
+/**:
+  ros__parameters:
+    # Control loop rate (Hz)
+    control_rate: 20.0
+
+    # Odometry topic for stuck detection
+    odom_topic: "/saltybot/rover_odom"
+
+    # ── LaserScan forward sector ───────────────────────────────────────────────
+    forward_scan_angle_rad: 0.785   # ±45° forward sector
+
+    # ── Obstacle proximity ────────────────────────────────────────────────────
+    stop_distance_m:     0.30       # MAJOR threshold (spec: <30 cm)
+    critical_distance_m: 0.10       # CRITICAL threshold
+    min_cmd_speed_ms:    0.05       # ignore obstacle when nearly stopped
+
+    # ── Fall detection (IMU tilt) ─────────────────────────────────────────────
+    minor_tilt_rad:    0.20         # advisory
+    major_tilt_rad:    0.35         # stop + recover
+    critical_tilt_rad: 0.52         # ~30° — full shutdown
+    floor_drop_m:      0.15         # depth discontinuity triggering MAJOR
+
+    # ── Stuck detection ───────────────────────────────────────────────────────
+    stuck_timeout_s: 3.0            # (spec: 3 s wheel stall)
+
+    # ── Bump / jerk detection ─────────────────────────────────────────────────
+    jerk_threshold_ms3:          8.0
+    critical_jerk_threshold_ms3: 25.0
+
+    # ── FSM / recovery ────────────────────────────────────────────────────────
+    stopped_ms:            0.03     # speed below which robot is "stopped" (m/s)
+    major_count_threshold: 3        # MAJOR alerts before escalation to CRITICAL
+    escalation_window_s:   10.0     # sliding window for escalation counter (s)
+    suppression_s:         1.0      # de-bounce period for duplicate alerts (s)
+
+    # Recovery sequence
+    reverse_speed_ms:   -0.15       # back-up speed (m/s; must be negative)
+    reverse_distance_m:  0.30       # distance to reverse each cycle (m)
+    angular_speed_rads:  0.60       # turn speed (rad/s)
+    turn_angle_rad:      1.5708     # ~90° turn (rad)
+    retry_timeout_s:     3.0        # time in RETRYING per attempt (s)
+    clear_hold_s:        0.50       # consecutive clear time to declare success (s)
+    max_retries:         3          # maximum reverse+turn attempts before GAVE_UP

jetson/ros2_ws/src/saltybot_emergency/launch/emergency.launch.py

@@ -0,0 +1,53 @@
+"""
+emergency.launch.py — Launch the emergency behavior system (Issue #169).
+
+Usage
+-----
+  ros2 launch saltybot_emergency emergency.launch.py
+  ros2 launch saltybot_emergency emergency.launch.py \
+      stop_distance_m:=0.30 max_retries:=3
+"""
+
+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_share = get_package_share_directory("saltybot_emergency")
+    default_params = os.path.join(pkg_share, "config", "emergency_params.yaml")
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            "params_file",
+            default_value=default_params,
+            description="Path to emergency_params.yaml",
+        ),
+        DeclareLaunchArgument(
+            "stop_distance_m",
+            default_value="0.30",
+            description="Obstacle distance triggering MAJOR stop (m)",
+        ),
+        DeclareLaunchArgument(
+            "max_retries",
+            default_value="3",
+            description="Maximum recovery cycles before ESCALATED",
+        ),
+        Node(
+            package="saltybot_emergency",
+            executable="emergency_node",
+            name="emergency",
+            output="screen",
+            parameters=[
+                LaunchConfiguration("params_file"),
+                {
+                    "stop_distance_m": LaunchConfiguration("stop_distance_m"),
+                    "max_retries":     LaunchConfiguration("max_retries"),
+                },
+            ],
+        ),
+    ])

jetson/ros2_ws/src/saltybot_emergency/package.xml

@@ -0,0 +1,24 @@
+<?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_emergency</name>
+  <version>0.1.0</version>
+  <description>Emergency behavior system — collision avoidance, fall prevention, stuck detection, recovery (Issue #169)</description>
+  <maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_emergency/saltybot_emergency/alert_manager.py

@@ -0,0 +1,139 @@
+"""
+alert_manager.py — Alert severity escalation for emergency behavior (Issue #169).
+
+Alert levels
+────────────
+  NONE     : no action
+  MINOR    : advisory beep  → publish to /saltybot/alert_beep
+  MAJOR    : stop + LED flash → publish to /saltybot/alert_flash; cmd_vel override
+  CRITICAL : full shutdown + MQTT → publish to /saltybot/e_stop + /saltybot/critical_alert
+
+Escalation
+──────────
+  If major_count_threshold MAJOR alerts occur within escalation_window_s, the
+  next MAJOR is promoted to CRITICAL.  This catches persistent stuck / repeated
+  collision scenarios.
+
+Suppression
+───────────
+  Identical (type, level) alerts are suppressed within suppression_s to avoid
+  flooding downstream topics.
+
+Pure module — no ROS2 dependency.
+"""
+
+from __future__ import annotations
+
+from collections import deque
+from dataclasses import dataclass
+from enum import Enum
+from typing import Optional
+
+from saltybot_emergency.threat_detector import ThreatEvent, ThreatLevel, ThreatType
+
+
+# ── Alert level ───────────────────────────────────────────────────────────────
+
+class AlertLevel(Enum):
+    NONE     = 0
+    MINOR    = 1   # beep
+    MAJOR    = 2   # stop + flash
+    CRITICAL = 3   # shutdown + MQTT
+
+
+# ── Alert ─────────────────────────────────────────────────────────────────────
+
+@dataclass
+class Alert:
+    level:       AlertLevel
+    source:      str     # ThreatType value string
+    message:     str
+    timestamp_s: float
+
+
+# ── AlertManager ─────────────────────────────────────────────────────────────
+
+class AlertManager:
+    """
+    Converts ThreatEvents to Alerts with escalation and suppression logic.
+
+    Parameters
+    ----------
+    major_count_threshold : number of MAJOR alerts within window to escalate
+    escalation_window_s   : sliding window for escalation counting (s)
+    suppression_s         : suppress duplicate (type, level) alerts within this period
+    """
+
+    def __init__(
+        self,
+        major_count_threshold: int   = 3,
+        escalation_window_s:   float = 10.0,
+        suppression_s:         float = 1.0,
+    ):
+        self._major_threshold  = max(1, int(major_count_threshold))
+        self._esc_window       = float(escalation_window_s)
+        self._suppress         = float(suppression_s)
+        self._major_times:     deque  = deque()            # timestamps of MAJOR alerts
+        self._last_seen:       dict   = {}                 # (type, level) → timestamp
+
+    # ── Update ────────────────────────────────────────────────────────────────
+
+    def update(self, threat: ThreatEvent) -> Optional[Alert]:
+        """
+        Convert one ThreatEvent to an Alert, applying escalation and suppression.
+
+        Returns None if threat is CLEAR or the alert is suppressed.
+        """
+        if threat.level == ThreatLevel.CLEAR:
+            return None
+
+        now = threat.timestamp_s
+        alert_level = _threat_to_alert(threat.level)
+
+        # ── Suppression ───────────────────────────────────────────────────────
+        key = (threat.threat_type, threat.level)
+        last = self._last_seen.get(key)
+        if last is not None and (now - last) < self._suppress:
+            return None
+        self._last_seen[key] = now
+
+        # ── Escalation ────────────────────────────────────────────────────────
+        if alert_level == AlertLevel.MAJOR:
+            # Prune old timestamps
+            while self._major_times and (now - self._major_times[0]) > self._esc_window:
+                self._major_times.popleft()
+            self._major_times.append(now)
+            if len(self._major_times) >= self._major_threshold:
+                alert_level = AlertLevel.CRITICAL
+
+        msg = _build_message(alert_level, threat)
+        return Alert(
+            level=alert_level,
+            source=threat.threat_type.value,
+            message=msg,
+            timestamp_s=now,
+        )
+
+    def reset(self) -> None:
+        """Clear escalation history and suppression state."""
+        self._major_times.clear()
+        self._last_seen.clear()
+
+
+# ── Helpers ───────────────────────────────────────────────────────────────────
+
+def _threat_to_alert(level: ThreatLevel) -> AlertLevel:
+    return {
+        ThreatLevel.MINOR:    AlertLevel.MINOR,
+        ThreatLevel.MAJOR:    AlertLevel.MAJOR,
+        ThreatLevel.CRITICAL: AlertLevel.CRITICAL,
+    }.get(level, AlertLevel.NONE)
+
+
+def _build_message(level: AlertLevel, threat: ThreatEvent) -> str:
+    prefix = {
+        AlertLevel.MINOR:    "[MINOR]",
+        AlertLevel.MAJOR:    "[MAJOR]",
+        AlertLevel.CRITICAL: "[CRITICAL]",
+    }.get(level, "[?]")
+    return f"{prefix} {threat.threat_type.value}: {threat.detail}"

jetson/ros2_ws/src/saltybot_emergency/saltybot_emergency/emergency_fsm.py

@@ -0,0 +1,232 @@
+"""
+emergency_fsm.py — Master emergency FSM integrating all detectors (Issue #169).
+
+States
+──────
+  NOMINAL    : normal operation; minor alerts pass through; major/critical → STOPPING.
+  STOPPING   : commanding zero velocity until robot speed drops below stopped_ms.
+               Critical threats skip RECOVERING → ESCALATED immediately.
+  RECOVERING : RecoverySequencer executing reverse+turn sequence.
+               Success → NOMINAL; gave-up → ESCALATED.
+  ESCALATED  : full stop; critical alert emitted once; stays until acknowledge.
+
+Alert actions produced by state
+────────────────────────────────
+  NOMINAL    : emit MINOR alert (beep only); no velocity override.
+  STOPPING   : suppress nav, publish zero; emit MAJOR alert once.
+  RECOVERING : suppress nav, publish recovery cmds; no new alerts.
+  ESCALATED  : suppress nav, publish zero; emit CRITICAL alert once per entry.
+
+Pure module — no ROS2 dependency.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from enum import Enum
+from typing import Optional
+
+from saltybot_emergency.alert_manager import Alert, AlertLevel, AlertManager
+from saltybot_emergency.recovery_sequencer import RecoveryInputs, RecoverySequencer
+from saltybot_emergency.threat_detector import ThreatEvent, ThreatLevel
+
+
+# ── States ────────────────────────────────────────────────────────────────────
+
+class EmergencyState(Enum):
+    NOMINAL    = "NOMINAL"
+    STOPPING   = "STOPPING"
+    RECOVERING = "RECOVERING"
+    ESCALATED  = "ESCALATED"
+
+
+# ── I/O ───────────────────────────────────────────────────────────────────────
+
+@dataclass
+class EmergencyInputs:
+    threat:         ThreatEvent  # highest-severity threat this tick
+    robot_speed_ms: float = 0.0  # actual speed from odometry (m/s)
+    acknowledge:    bool  = False # operator cleared the escalation
+
+
+@dataclass
+class EmergencyOutputs:
+    state:        EmergencyState    = EmergencyState.NOMINAL
+    cmd_override: bool              = False   # True = emergency owns cmd_vel
+    cmd_linear:   float             = 0.0
+    cmd_angular:  float             = 0.0
+    alert:        Optional[Alert]   = None
+    e_stop:       bool              = False   # assert /saltybot/e_stop
+    state_changed: bool             = False
+    recovery_progress: float        = 0.0
+    recovery_retry_count: int       = 0
+
+
+# ── EmergencyFSM ──────────────────────────────────────────────────────────────
+
+class EmergencyFSM:
+    """
+    Master emergency FSM.
+
+    Owns an AlertManager and a RecoverySequencer; coordinates them each tick.
+
+    Parameters
+    ----------
+    stopped_ms              : speed below which robot is considered stopped (m/s)
+    major_count_threshold   : MAJOR events within window before escalation
+    escalation_window_s     : sliding window for escalation (s)
+    suppression_s           : alert de-bounce period (s)
+    reverse_speed_ms        : reverse speed during recovery (m/s)
+    reverse_distance_m      : reverse travel per cycle (m)
+    angular_speed_rads      : turn speed during recovery (rad/s)
+    turn_angle_rad          : turn per cycle (rad)
+    retry_timeout_s         : time in RETRYING before next cycle (s)
+    clear_hold_s            : clear duration required to declare success (s)
+    max_retries             : recovery cycles before GAVE_UP
+    """
+
+    def __init__(
+        self,
+        stopped_ms:            float = 0.03,
+        major_count_threshold: int   = 3,
+        escalation_window_s:   float = 10.0,
+        suppression_s:         float = 1.0,
+        reverse_speed_ms:      float = -0.15,
+        reverse_distance_m:    float = 0.30,
+        angular_speed_rads:    float = 0.60,
+        turn_angle_rad:        float = 1.5708,
+        retry_timeout_s:       float = 3.0,
+        clear_hold_s:          float = 0.5,
+        max_retries:           int   = 3,
+    ):
+        self._stopped_ms = max(0.0, stopped_ms)
+        self._alert_mgr  = AlertManager(
+            major_count_threshold=major_count_threshold,
+            escalation_window_s=escalation_window_s,
+            suppression_s=suppression_s,
+        )
+        self._recovery = RecoverySequencer(
+            reverse_speed_ms=reverse_speed_ms,
+            reverse_distance_m=reverse_distance_m,
+            angular_speed_rads=angular_speed_rads,
+            turn_angle_rad=turn_angle_rad,
+            retry_timeout_s=retry_timeout_s,
+            clear_hold_s=clear_hold_s,
+            max_retries=max_retries,
+        )
+        self._state            = EmergencyState.NOMINAL
+        self._critical_pending = False   # STOPPING → ESCALATED (not RECOVERING)
+        self._escalation_alerted = False # CRITICAL alert emitted once per ESCALATED entry
+
+    # ── Public API ────────────────────────────────────────────────────────────
+
+    @property
+    def state(self) -> EmergencyState:
+        return self._state
+
+    def reset(self) -> None:
+        self._state              = EmergencyState.NOMINAL
+        self._critical_pending   = False
+        self._escalation_alerted = False
+        self._alert_mgr.reset()
+        self._recovery.reset()
+
+    def tick(self, inputs: EmergencyInputs) -> EmergencyOutputs:
+        prev = self._state
+        out  = self._step(inputs)
+        out.state         = self._state
+        out.state_changed = (self._state != prev)
+        return out
+
+    # ── Step ─────────────────────────────────────────────────────────────────
+
+    def _step(self, inp: EmergencyInputs) -> EmergencyOutputs:
+        out = EmergencyOutputs(state=self._state)
+
+        # Run alert manager for this threat
+        alert = self._alert_mgr.update(inp.threat)
+
+        # ── NOMINAL ───────────────────────────────────────────────────────────
+        if self._state == EmergencyState.NOMINAL:
+            if inp.threat.level == ThreatLevel.CRITICAL:
+                self._state            = EmergencyState.STOPPING
+                self._critical_pending = True
+                out.alert              = alert
+                out.cmd_override       = True   # start overriding on entry tick
+            elif inp.threat.level == ThreatLevel.MAJOR:
+                self._state            = EmergencyState.STOPPING
+                self._critical_pending = False
+                out.alert              = alert
+                out.cmd_override       = True   # start overriding on entry tick
+            elif inp.threat.level == ThreatLevel.MINOR:
+                # Advisory only — no override
+                out.alert = alert
+
+        # ── STOPPING ──────────────────────────────────────────────────────────
+        elif self._state == EmergencyState.STOPPING:
+            out.cmd_override = True
+            out.cmd_linear   = 0.0
+            out.cmd_angular  = 0.0
+            # Upgrade to critical if new critical arrives
+            if inp.threat.level == ThreatLevel.CRITICAL:
+                self._critical_pending = True
+            if abs(inp.robot_speed_ms) <= self._stopped_ms:
+                if self._critical_pending:
+                    self._state              = EmergencyState.ESCALATED
+                    self._escalation_alerted = False
+                else:
+                    self._state = EmergencyState.RECOVERING
+                    self._recovery.reset()
+                    self._recovery.tick(RecoveryInputs(trigger=True, dt=0.0))
+
+        # ── RECOVERING ────────────────────────────────────────────────────────
+        elif self._state == EmergencyState.RECOVERING:
+            threat_cleared = inp.threat.level == ThreatLevel.CLEAR
+            rec_inp = RecoveryInputs(
+                trigger=False,
+                threat_cleared=threat_cleared,
+                dt=0.02,   # nominal dt; node should pass actual dt
+            )
+            rec_out = self._recovery.tick(rec_inp)
+
+            out.cmd_override          = True
+            out.cmd_linear            = rec_out.cmd_linear
+            out.cmd_angular           = rec_out.cmd_angular
+            out.recovery_progress     = rec_out.progress
+            out.recovery_retry_count  = rec_out.retry_count
+
+            if rec_out.gave_up:
+                self._state              = EmergencyState.ESCALATED
+                self._escalation_alerted = False
+            elif rec_out.state.value == "IDLE" and not inp.trigger if hasattr(inp, "trigger") else True:
+                # RecoverySequencer returned to IDLE = success
+                from saltybot_emergency.recovery_sequencer import RecoveryState
+                if self._recovery.state == RecoveryState.IDLE and not rec_out.gave_up:
+                    self._state = EmergencyState.NOMINAL
+                    self._recovery.reset()
+
+        # ── ESCALATED ─────────────────────────────────────────────────────────
+        elif self._state == EmergencyState.ESCALATED:
+            out.cmd_override = True
+            out.cmd_linear   = 0.0
+            out.cmd_angular  = 0.0
+            out.e_stop       = True
+            if not self._escalation_alerted:
+                # Force a CRITICAL alert regardless of suppression
+                from saltybot_emergency.alert_manager import Alert, AlertLevel
+                out.alert = Alert(
+                    level=AlertLevel.CRITICAL,
+                    source=inp.threat.threat_type.value,
+                    message=f"[CRITICAL] ESCALATED: {inp.threat.detail or 'Recovery gave up'}",
+                    timestamp_s=inp.threat.timestamp_s,
+                )
+                self._escalation_alerted = True
+            if inp.acknowledge:
+                self._state              = EmergencyState.NOMINAL
+                self._critical_pending   = False
+                self._escalation_alerted = False
+                out.e_stop               = False
+                self._alert_mgr.reset()
+                self._recovery.reset()
+
+        return out

jetson/ros2_ws/src/saltybot_emergency/saltybot_emergency/emergency_node.py

@@ -0,0 +1,383 @@
+"""
+emergency_node.py — Emergency behavior system orchestration (Issue #169).
+
+Overview
+────────
+  Aggregates threats from four independent detectors and drives the
+  EmergencyFSM.  Overrides /cmd_vel when an emergency is active.  Escalates
+  via /saltybot/e_stop and /saltybot/critical_alert for CRITICAL events.
+
+Pipeline (20 Hz)
+────────────────
+  1. LaserScan callback → ObstacleDetector → ThreatEvent
+  2. IMU callback       → FallDetector + BumpDetector → ThreatEvent (×2)
+  3. Odom callback      → StuckDetector (fed in timer) → ThreatEvent
+  4. 20 Hz timer        → highest_threat() → EmergencyFSM.tick()
+                        → publish overriding cmd_vel or pass-through
+                        → publish /saltybot/emergency + /saltybot/recovery_action
+
+Subscribes
+──────────
+  /scan                    sensor_msgs/LaserScan  — obstacle detection
+  /saltybot/imu            sensor_msgs/Imu        — fall + bump detection
+  <odom_topic>             nav_msgs/Odometry      — stuck + speed tracking
+  /cmd_vel                 geometry_msgs/Twist    — nav commands (pass-through)
+
+Publishes
+─────────
+  /saltybot/cmd_vel_out    geometry_msgs/Twist    — muxed cmd_vel (to drive nodes)
+  /saltybot/e_stop         std_msgs/Bool          — emergency stop flag
+  /saltybot/alert_beep     std_msgs/Empty         — beep trigger (MINOR)
+  /saltybot/alert_flash    std_msgs/Empty         — LED flash trigger (MAJOR)
+  /saltybot/critical_alert std_msgs/String (JSON) — CRITICAL event for MQTT bridge
+  /saltybot/emergency      saltybot_emergency_msgs/EmergencyEvent
+  /saltybot/recovery_action saltybot_emergency_msgs/RecoveryAction
+
+Parameters
+──────────
+  control_rate             20.0    Hz
+  odom_topic               /saltybot/rover_odom
+  forward_scan_angle_rad   0.785   rad  (±45° forward sector for obstacle check)
+  stop_distance_m          0.30    m
+  critical_distance_m      0.10    m
+  min_cmd_speed_ms         0.05    m/s
+  minor_tilt_rad           0.20    rad
+  major_tilt_rad           0.35    rad
+  critical_tilt_rad        0.52    rad
+  floor_drop_m             0.15    m
+  stuck_timeout_s          3.0     s
+  jerk_threshold_ms3       8.0     m/s³
+  critical_jerk_threshold_ms3  25.0 m/s³
+  stopped_ms               0.03    m/s
+  major_count_threshold    3
+  escalation_window_s      10.0    s
+  suppression_s            1.0     s
+  reverse_speed_ms        -0.15    m/s
+  reverse_distance_m       0.30    m
+  angular_speed_rads       0.60    rad/s
+  turn_angle_rad           1.5708  rad
+  retry_timeout_s          3.0     s
+  clear_hold_s             0.50    s
+  max_retries              3
+"""
+
+import json
+import math
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import Imu, LaserScan
+from std_msgs.msg import Bool, Empty, String
+
+from saltybot_emergency.alert_manager import AlertLevel
+from saltybot_emergency.emergency_fsm import EmergencyFSM, EmergencyInputs, EmergencyState
+from saltybot_emergency.threat_detector import (
+    BumpDetector,
+    FallDetector,
+    ObstacleDetector,
+    StuckDetector,
+    ThreatEvent,
+    ThreatType,
+    highest_threat,
+)
+
+try:
+    from saltybot_emergency_msgs.msg import EmergencyEvent, RecoveryAction
+    _MSGS_OK = True
+except ImportError:
+    _MSGS_OK = False
+
+
+def _quaternion_to_pitch_roll(qx, qy, qz, qw):
+    pitch = math.asin(max(-1.0, min(1.0, 2.0 * (qw * qy - qz * qx))))
+    roll  = math.atan2(2.0 * (qw * qx + qy * qz), 1.0 - 2.0 * (qx * qx + qy * qy))
+    return pitch, roll
+
+
+class EmergencyNode(Node):
+
+    def __init__(self):
+        super().__init__("emergency")
+
+        self._declare_params()
+        p = self._load_params()
+
+        # ── Detectors ────────────────────────────────────────────────────────
+        self._obstacle = ObstacleDetector(
+            stop_distance_m=p["stop_distance_m"],
+            critical_distance_m=p["critical_distance_m"],
+            min_speed_ms=p["min_cmd_speed_ms"],
+        )
+        self._fall = FallDetector(
+            minor_tilt_rad=p["minor_tilt_rad"],
+            major_tilt_rad=p["major_tilt_rad"],
+            critical_tilt_rad=p["critical_tilt_rad"],
+            floor_drop_m=p["floor_drop_m"],
+        )
+        self._stuck = StuckDetector(
+            stuck_timeout_s=p["stuck_timeout_s"],
+            min_cmd_ms=p["min_cmd_speed_ms"],
+        )
+        self._bump = BumpDetector(
+            jerk_threshold_ms3=p["jerk_threshold_ms3"],
+            critical_jerk_threshold_ms3=p["critical_jerk_threshold_ms3"],
+        )
+        self._fsm = EmergencyFSM(
+            stopped_ms=p["stopped_ms"],
+            major_count_threshold=p["major_count_threshold"],
+            escalation_window_s=p["escalation_window_s"],
+            suppression_s=p["suppression_s"],
+            reverse_speed_ms=p["reverse_speed_ms"],
+            reverse_distance_m=p["reverse_distance_m"],
+            angular_speed_rads=p["angular_speed_rads"],
+            turn_angle_rad=p["turn_angle_rad"],
+            retry_timeout_s=p["retry_timeout_s"],
+            clear_hold_s=p["clear_hold_s"],
+            max_retries=p["max_retries"],
+        )
+
+        # ── State ────────────────────────────────────────────────────────────
+        self._latest_obstacle_threat = ThreatEvent()
+        self._latest_fall_threat     = ThreatEvent()
+        self._latest_bump_threat     = ThreatEvent()
+        self._cmd_speed_ms           = 0.0
+        self._actual_speed_ms        = 0.0
+        self._last_ctrl_t            = time.monotonic()
+        self._scan_forward_angle     = p["forward_scan_angle_rad"]
+        self._acknowledge_flag       = False
+
+        # ── QoS ──────────────────────────────────────────────────────────────
+        reliable = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=10,
+        )
+        best_effort = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=1,
+        )
+
+        # ── Subscriptions ────────────────────────────────────────────────────
+        self.create_subscription(LaserScan, "/scan",              self._scan_cb,   best_effort)
+        self.create_subscription(Imu,       "/saltybot/imu",      self._imu_cb,    best_effort)
+        self.create_subscription(Odometry,  p["odom_topic"],      self._odom_cb,   reliable)
+        self.create_subscription(Twist,     "/cmd_vel",           self._cmd_vel_cb, reliable)
+        self.create_subscription(Bool,      "/saltybot/emergency_ack", self._ack_cb, reliable)
+
+        # ── Publishers ───────────────────────────────────────────────────────
+        self._cmd_out_pub    = self.create_publisher(Twist,  "/saltybot/cmd_vel_out", reliable)
+        self._estop_pub      = self.create_publisher(Bool,   "/saltybot/e_stop",      reliable)
+        self._beep_pub       = self.create_publisher(Empty,  "/saltybot/alert_beep",  reliable)
+        self._flash_pub      = self.create_publisher(Empty,  "/saltybot/alert_flash", reliable)
+        self._critical_pub   = self.create_publisher(String, "/saltybot/critical_alert", reliable)
+
+        self._event_pub    = None
+        self._recovery_pub = None
+        if _MSGS_OK:
+            self._event_pub    = self.create_publisher(EmergencyEvent,  "/saltybot/emergency",       reliable)
+            self._recovery_pub = self.create_publisher(RecoveryAction, "/saltybot/recovery_action", reliable)
+
+        # ── Timer ────────────────────────────────────────────────────────────
+        rate = p["control_rate"]
+        self._timer = self.create_timer(1.0 / rate, self._control_cb)
+        self.get_logger().info(f"EmergencyNode ready  rate={rate}Hz")
+
+    # ── Parameters ────────────────────────────────────────────────────────────
+
+    def _declare_params(self) -> None:
+        self.declare_parameter("control_rate",               20.0)
+        self.declare_parameter("odom_topic",                 "/saltybot/rover_odom")
+        self.declare_parameter("forward_scan_angle_rad",     0.785)
+        self.declare_parameter("stop_distance_m",            0.30)
+        self.declare_parameter("critical_distance_m",        0.10)
+        self.declare_parameter("min_cmd_speed_ms",           0.05)
+        self.declare_parameter("minor_tilt_rad",             0.20)
+        self.declare_parameter("major_tilt_rad",             0.35)
+        self.declare_parameter("critical_tilt_rad",          0.52)
+        self.declare_parameter("floor_drop_m",               0.15)
+        self.declare_parameter("stuck_timeout_s",            3.0)
+        self.declare_parameter("jerk_threshold_ms3",         8.0)
+        self.declare_parameter("critical_jerk_threshold_ms3", 25.0)
+        self.declare_parameter("stopped_ms",                 0.03)
+        self.declare_parameter("major_count_threshold",      3)
+        self.declare_parameter("escalation_window_s",        10.0)
+        self.declare_parameter("suppression_s",              1.0)
+        self.declare_parameter("reverse_speed_ms",          -0.15)
+        self.declare_parameter("reverse_distance_m",         0.30)
+        self.declare_parameter("angular_speed_rads",         0.60)
+        self.declare_parameter("turn_angle_rad",             1.5708)
+        self.declare_parameter("retry_timeout_s",            3.0)
+        self.declare_parameter("clear_hold_s",               0.50)
+        self.declare_parameter("max_retries",                3)
+
+    def _load_params(self) -> dict:
+        g = self.get_parameter
+        return {k: g(k).value for k in [
+            "control_rate", "odom_topic",
+            "forward_scan_angle_rad",
+            "stop_distance_m", "critical_distance_m", "min_cmd_speed_ms",
+            "minor_tilt_rad", "major_tilt_rad", "critical_tilt_rad", "floor_drop_m",
+            "stuck_timeout_s", "jerk_threshold_ms3", "critical_jerk_threshold_ms3",
+            "stopped_ms",
+            "major_count_threshold", "escalation_window_s", "suppression_s",
+            "reverse_speed_ms", "reverse_distance_m",
+            "angular_speed_rads", "turn_angle_rad",
+            "retry_timeout_s", "clear_hold_s", "max_retries",
+        ]}
+
+    # ── Callbacks ─────────────────────────────────────────────────────────────
+
+    def _scan_cb(self, msg: LaserScan) -> None:
+        # Extract minimum range within forward sector (±forward_scan_angle_rad)
+        half = self._scan_forward_angle
+        ranges = []
+        for i, r in enumerate(msg.ranges):
+            angle = msg.angle_min + i * msg.angle_increment
+            if abs(angle) <= half and msg.range_min < r < msg.range_max:
+                ranges.append(r)
+        min_r = min(ranges) if ranges else float("inf")
+        self._latest_obstacle_threat = self._obstacle.update(
+            min_r, self._cmd_speed_ms, time.monotonic()
+        )
+
+    def _imu_cb(self, msg: Imu) -> None:
+        now = time.monotonic()
+        ax = msg.linear_acceleration.x
+        ay = msg.linear_acceleration.y
+        az = msg.linear_acceleration.z
+        pitch, roll = _quaternion_to_pitch_roll(
+            msg.orientation.x, msg.orientation.y,
+            msg.orientation.z, msg.orientation.w,
+        )
+        # dt for jerk is approximated; bump detector handles None on first call
+        dt = 0.02  # nominal 20 Hz
+        self._latest_fall_threat = self._fall.update(pitch, roll, 0.0, now)
+        self._latest_bump_threat = self._bump.update(ax, ay, az, dt, now)
+
+    def _odom_cb(self, msg: Odometry) -> None:
+        self._actual_speed_ms = msg.twist.twist.linear.x
+
+    def _cmd_vel_cb(self, msg: Twist) -> None:
+        self._cmd_speed_ms = msg.linear.x
+
+    def _ack_cb(self, msg: Bool) -> None:
+        if msg.data:
+            self._acknowledge_flag = True
+
+    # ── 20 Hz control loop ────────────────────────────────────────────────────
+
+    def _control_cb(self) -> None:
+        now = time.monotonic()
+        dt  = now - self._last_ctrl_t
+        self._last_ctrl_t = now
+
+        stuck_threat = self._stuck.update(
+            self._cmd_speed_ms, self._actual_speed_ms, dt, now
+        )
+
+        threat = highest_threat([
+            self._latest_obstacle_threat,
+            self._latest_fall_threat,
+            self._latest_bump_threat,
+            stuck_threat,
+        ])
+
+        inp = EmergencyInputs(
+            threat=threat,
+            robot_speed_ms=self._actual_speed_ms,
+            acknowledge=self._acknowledge_flag,
+        )
+        self._acknowledge_flag = False
+
+        out = self._fsm.tick(inp)
+
+        if out.state_changed:
+            self.get_logger().info(f"Emergency FSM → {out.state.value}")
+
+        # ── Alert dispatch ────────────────────────────────────────────────────
+        if out.alert is not None:
+            lvl = out.alert.level
+            self.get_logger().warn(out.alert.message)
+            if lvl == AlertLevel.MINOR:
+                self._beep_pub.publish(Empty())
+            elif lvl == AlertLevel.MAJOR:
+                self._flash_pub.publish(Empty())
+            elif lvl == AlertLevel.CRITICAL:
+                self._flash_pub.publish(Empty())
+                self._publish_critical_alert(out.alert.message, threat)
+
+        # ── E-stop ───────────────────────────────────────────────────────────
+        estop_msg = Bool()
+        estop_msg.data = out.e_stop
+        self._estop_pub.publish(estop_msg)
+
+        # ── cmd_vel mux ───────────────────────────────────────────────────────
+        twist = Twist()
+        if out.cmd_override:
+            twist.linear.x  = out.cmd_linear
+            twist.angular.z = out.cmd_angular
+        else:
+            twist.linear.x  = self._cmd_speed_ms
+        self._cmd_out_pub.publish(twist)
+
+        # ── Status topics ─────────────────────────────────────────────────────
+        if self._event_pub is not None:
+            self._publish_event(out, threat)
+        if self._recovery_pub is not None:
+            self._publish_recovery(out)
+
+    # ── Publishers ────────────────────────────────────────────────────────────
+
+    def _publish_critical_alert(self, message: str, threat: ThreatEvent) -> None:
+        msg = String()
+        msg.data = json.dumps({
+            "severity":   "CRITICAL",
+            "threat":     threat.threat_type.value,
+            "value":      round(threat.value, 3),
+            "detail":     threat.detail,
+            "message":    message,
+        })
+        self._critical_pub.publish(msg)
+
+    def _publish_event(self, out, threat: ThreatEvent) -> None:
+        msg = EmergencyEvent()
+        msg.stamp        = self.get_clock().now().to_msg()
+        msg.state        = out.state.value
+        msg.threat_type  = threat.threat_type.value
+        msg.severity     = threat.level.name
+        msg.threat_value = float(threat.value)
+        msg.detail       = threat.detail
+        msg.cmd_override = out.cmd_override
+        self._event_pub.publish(msg)
+
+    def _publish_recovery(self, out) -> None:
+        msg = RecoveryAction()
+        msg.stamp        = self.get_clock().now().to_msg()
+        msg.action       = self._fsm._recovery.state.value
+        msg.retry_count  = out.recovery_retry_count
+        msg.progress     = float(out.recovery_progress)
+        self._recovery_pub.publish(msg)
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = EmergencyNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_emergency/saltybot_emergency/recovery_sequencer.py

@@ -0,0 +1,193 @@
+"""
+recovery_sequencer.py — Reverse + turn recovery FSM for emergency behavior (Issue #169).
+
+Sequence
+────────
+  IDLE → REVERSING → TURNING → RETRYING → (IDLE on success)
+                                         → (REVERSING on re-threat, retry loop)
+                                         → (GAVE_UP after max_retries)
+
+  REVERSING : command reverse at reverse_speed_ms until reverse_distance_m covered.
+  TURNING   : command angular_speed_rads until turn_angle_rad covered (90°).
+  RETRYING  : zero velocity; wait up to retry_timeout_s for threat to clear.
+              If threat clears within clear_hold_s → back to IDLE (success).
+              If timeout without clearance → start another REVERSING cycle.
+              If retry_count >= max_retries → GAVE_UP.
+
+Pure module — no ROS2 dependency.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from enum import Enum
+
+
+# ── States ────────────────────────────────────────────────────────────────────
+
+class RecoveryState(Enum):
+    IDLE      = "IDLE"
+    REVERSING = "REVERSING"
+    TURNING   = "TURNING"
+    RETRYING  = "RETRYING"
+    GAVE_UP   = "GAVE_UP"
+
+
+# ── I/O ───────────────────────────────────────────────────────────────────────
+
+@dataclass
+class RecoveryInputs:
+    trigger:        bool  = False   # True to start (or restart) recovery
+    threat_cleared: bool  = False   # True when all threats are CLEAR
+    dt:             float = 0.02    # time step (s)
+
+
+@dataclass
+class RecoveryOutputs:
+    state:         RecoveryState = RecoveryState.IDLE
+    cmd_linear:    float         = 0.0   # m/s
+    cmd_angular:   float         = 0.0   # rad/s
+    progress:      float         = 0.0   # [0, 1] completion of current phase
+    retry_count:   int           = 0
+    gave_up:       bool          = False
+    state_changed: bool          = False
+
+
+# ── RecoverySequencer ────────────────────────────────────────────────────────
+
+class RecoverySequencer:
+    """
+    Tick-based FSM for executing reverse + turn recovery sequences.
+
+    Parameters
+    ----------
+    reverse_speed_ms  : backward speed during REVERSING (m/s; stored as negative)
+    reverse_distance_m: total reverse travel before turning (m)
+    angular_speed_rads: yaw rate during TURNING (rad/s; positive = left)
+    turn_angle_rad    : total turn angle before RETRYING (rad; default π/2)
+    retry_timeout_s   : max RETRYING time per attempt before next reverse cycle
+    clear_hold_s      : consecutive clear time needed to declare success
+    max_retries       : maximum reverse+turn attempts before GAVE_UP
+    """
+
+    def __init__(
+        self,
+        reverse_speed_ms:   float = -0.15,
+        reverse_distance_m: float = 0.30,
+        angular_speed_rads: float = 0.60,
+        turn_angle_rad:     float = 1.5708,    # π/2
+        retry_timeout_s:    float = 3.0,
+        clear_hold_s:       float = 0.5,
+        max_retries:        int   = 3,
+    ):
+        self._rev_speed  = min(0.0, float(reverse_speed_ms))   # ensure negative
+        self._rev_dist   = max(0.01, float(reverse_distance_m))
+        self._ang_speed  = abs(float(angular_speed_rads))
+        self._turn_angle = max(0.01, float(turn_angle_rad))
+        self._retry_tout = max(0.1, float(retry_timeout_s))
+        self._clear_hold = max(0.0, float(clear_hold_s))
+        self._max_retry  = max(1, int(max_retries))
+
+        self._state        = RecoveryState.IDLE
+        self._rev_done     = 0.0   # distance reversed so far
+        self._turn_done    = 0.0   # angle turned so far
+        self._retry_count  = 0
+        self._retry_timer  = 0.0   # time spent in RETRYING
+        self._clear_timer  = 0.0   # consecutive clear time in RETRYING
+
+    # ── Public API ────────────────────────────────────────────────────────────
+
+    @property
+    def state(self) -> RecoveryState:
+        return self._state
+
+    @property
+    def retry_count(self) -> int:
+        return self._retry_count
+
+    def reset(self) -> None:
+        """Return to IDLE and clear all counters."""
+        self._state       = RecoveryState.IDLE
+        self._rev_done    = 0.0
+        self._turn_done   = 0.0
+        self._retry_count = 0
+        self._retry_timer = 0.0
+        self._clear_timer = 0.0
+
+    def tick(self, inputs: RecoveryInputs) -> RecoveryOutputs:
+        prev = self._state
+        out  = self._step(inputs)
+        out.state         = self._state
+        out.retry_count   = self._retry_count
+        out.state_changed = (self._state != prev)
+        if out.state_changed:
+            self._on_enter(self._state)
+        return out
+
+    # ── Internal step ─────────────────────────────────────────────────────────
+
+    def _step(self, inp: RecoveryInputs) -> RecoveryOutputs:
+        out = RecoveryOutputs(state=self._state)
+        dt  = max(0.0, inp.dt)
+
+        # ── IDLE ──────────────────────────────────────────────────────────────
+        if self._state == RecoveryState.IDLE:
+            if inp.trigger:
+                self._state = RecoveryState.REVERSING
+
+        # ── REVERSING ─────────────────────────────────────────────────────────
+        elif self._state == RecoveryState.REVERSING:
+            step = abs(self._rev_speed) * dt
+            self._rev_done += step
+            out.cmd_linear = self._rev_speed
+            out.progress   = min(1.0, self._rev_done / self._rev_dist)
+            if self._rev_done >= self._rev_dist:
+                self._state = RecoveryState.TURNING
+
+        # ── TURNING ───────────────────────────────────────────────────────────
+        elif self._state == RecoveryState.TURNING:
+            step = self._ang_speed * dt
+            self._turn_done += step
+            out.cmd_angular = self._ang_speed
+            out.progress    = min(1.0, self._turn_done / self._turn_angle)
+            if self._turn_done >= self._turn_angle:
+                self._retry_count += 1
+                self._state = RecoveryState.RETRYING
+
+        # ── RETRYING ──────────────────────────────────────────────────────────
+        elif self._state == RecoveryState.RETRYING:
+            self._retry_timer += dt
+
+            if inp.threat_cleared:
+                self._clear_timer += dt
+                if self._clear_timer >= self._clear_hold:
+                    # Success — threat has cleared
+                    self._state = RecoveryState.IDLE
+                    return out
+            else:
+                self._clear_timer = 0.0
+
+            if self._retry_timer >= self._retry_tout:
+                if self._retry_count >= self._max_retry:
+                    self._state  = RecoveryState.GAVE_UP
+                    out.gave_up  = True
+                else:
+                    self._state = RecoveryState.REVERSING
+
+        # ── GAVE_UP ───────────────────────────────────────────────────────────
+        elif self._state == RecoveryState.GAVE_UP:
+            # Stay in GAVE_UP until external reset()
+            out.gave_up = True
+
+        return out
+
+    # ── Entry actions ─────────────────────────────────────────────────────────
+
+    def _on_enter(self, state: RecoveryState) -> None:
+        if state == RecoveryState.REVERSING:
+            self._rev_done  = 0.0
+        elif state == RecoveryState.TURNING:
+            self._turn_done = 0.0
+        elif state == RecoveryState.RETRYING:
+            self._retry_timer = 0.0
+            self._clear_timer = 0.0

jetson/ros2_ws/src/saltybot_emergency/saltybot_emergency/threat_detector.py

@@ -0,0 +1,354 @@
+"""
+threat_detector.py — Multi-source threat detection for emergency behavior (Issue #169).
+
+Detectors
+─────────
+  ObstacleDetector  : Forward-sector minimum range < stop thresholds at speed.
+                      Inputs: min_range_m (pre-filtered from LaserScan forward
+                      sector), cmd_speed_ms.
+
+  FallDetector      : Extreme pitch/roll from IMU, or depth floor-drop ahead.
+                      Inputs: pitch_rad, roll_rad, floor_drop_m (depth-derived;
+                      0.0 if depth unavailable).
+
+  StuckDetector     : Commanded speed vs actual speed mismatch sustained for
+                      stuck_timeout_s.  Tracks elapsed time with dt argument.
+
+  BumpDetector      : IMU acceleration jerk (|Δ|a||/dt) above threshold.
+                      MAJOR at jerk_threshold_ms3, CRITICAL at
+                      critical_jerk_threshold_ms3.
+
+ThreatLevel
+───────────
+  CLEAR    : no threat; normal operation
+  MINOR    : advisory; log/beep only
+  MAJOR    : stop and execute recovery
+  CRITICAL : full shutdown + MQTT escalation
+
+Pure module — no ROS2 dependency.
+"""
+
+from __future__ import annotations
+
+import math
+import time
+from dataclasses import dataclass
+from enum import Enum
+from typing import Optional
+
+
+# ── Enumerations ──────────────────────────────────────────────────────────────
+
+class ThreatLevel(Enum):
+    CLEAR    = 0
+    MINOR    = 1
+    MAJOR    = 2
+    CRITICAL = 3
+
+
+class ThreatType(Enum):
+    NONE                = "NONE"
+    OBSTACLE_PROXIMITY  = "OBSTACLE_PROXIMITY"
+    FALL_RISK           = "FALL_RISK"
+    WHEEL_STUCK         = "WHEEL_STUCK"
+    BUMP                = "BUMP"
+
+
+# ── ThreatEvent ───────────────────────────────────────────────────────────────
+
+@dataclass
+class ThreatEvent:
+    """Snapshot of a single detected threat."""
+    threat_type:  ThreatType  = ThreatType.NONE
+    level:        ThreatLevel = ThreatLevel.CLEAR
+    value:        float       = 0.0    # triggering metric
+    detail:       str         = ""
+    timestamp_s:  float       = 0.0
+
+    @staticmethod
+    def clear(timestamp_s: float = 0.0) -> "ThreatEvent":
+        return ThreatEvent(timestamp_s=timestamp_s)
+
+
+_CLEAR = ThreatEvent()
+
+
+# ── ObstacleDetector ─────────────────────────────────────────────────────────
+
+class ObstacleDetector:
+    """
+    Obstacle proximity threat from forward-sector minimum range.
+
+    Parameters
+    ----------
+    stop_distance_m      : range below which MAJOR is raised (default 0.30 m)
+    critical_distance_m  : range below which CRITICAL is raised (default 0.10 m)
+    min_speed_ms         : only active above this commanded speed (default 0.05 m/s)
+    """
+
+    def __init__(
+        self,
+        stop_distance_m:     float = 0.30,
+        critical_distance_m: float = 0.10,
+        min_speed_ms:        float = 0.05,
+    ):
+        self._stop     = max(1e-3, stop_distance_m)
+        self._critical = max(1e-3, min(self._stop, critical_distance_m))
+        self._min_spd  = abs(min_speed_ms)
+
+    def update(
+        self,
+        min_range_m:   float,
+        cmd_speed_ms:  float,
+        timestamp_s:   float = 0.0,
+    ) -> ThreatEvent:
+        """
+        Parameters
+        ----------
+        min_range_m   : minimum obstacle range in forward sector (m)
+        cmd_speed_ms  : signed commanded forward speed (m/s)
+        """
+        if abs(cmd_speed_ms) < self._min_spd:
+            return ThreatEvent.clear(timestamp_s)
+
+        if min_range_m <= self._critical:
+            return ThreatEvent(
+                threat_type=ThreatType.OBSTACLE_PROXIMITY,
+                level=ThreatLevel.CRITICAL,
+                value=min_range_m,
+                detail=f"Obstacle {min_range_m:.2f} m (critical zone)",
+                timestamp_s=timestamp_s,
+            )
+        if min_range_m <= self._stop:
+            return ThreatEvent(
+                threat_type=ThreatType.OBSTACLE_PROXIMITY,
+                level=ThreatLevel.MAJOR,
+                value=min_range_m,
+                detail=f"Obstacle {min_range_m:.2f} m ahead",
+                timestamp_s=timestamp_s,
+            )
+        return ThreatEvent.clear(timestamp_s)
+
+
+# ── FallDetector ──────────────────────────────────────────────────────────────
+
+class FallDetector:
+    """
+    Fall / tipping risk from IMU pitch/roll and optional depth floor-drop.
+
+    Parameters
+    ----------
+    minor_tilt_rad    : |pitch| or |roll| above which MINOR fires (default 0.20 rad)
+    major_tilt_rad    : above which MAJOR fires (default 0.35 rad)
+    critical_tilt_rad : above which CRITICAL fires (default 0.52 rad ≈ 30°)
+    floor_drop_m      : depth discontinuity (m) triggering MAJOR (default 0.15 m)
+    """
+
+    def __init__(
+        self,
+        minor_tilt_rad:    float = 0.20,
+        major_tilt_rad:    float = 0.35,
+        critical_tilt_rad: float = 0.52,
+        floor_drop_m:      float = 0.15,
+    ):
+        self._minor    = float(minor_tilt_rad)
+        self._major    = float(major_tilt_rad)
+        self._critical = float(critical_tilt_rad)
+        self._drop     = float(floor_drop_m)
+
+    def update(
+        self,
+        pitch_rad:    float,
+        roll_rad:     float,
+        floor_drop_m: float = 0.0,
+        timestamp_s:  float = 0.0,
+    ) -> ThreatEvent:
+        """
+        Parameters
+        ----------
+        pitch_rad    : forward tilt (rad); +ve = nose up
+        roll_rad     : lateral tilt (rad); +ve = left side up
+        floor_drop_m : depth discontinuity ahead of robot (m); 0 = not measured
+        """
+        tilt = max(abs(pitch_rad), abs(roll_rad))
+
+        if tilt >= self._critical:
+            return ThreatEvent(
+                threat_type=ThreatType.FALL_RISK,
+                level=ThreatLevel.CRITICAL,
+                value=tilt,
+                detail=f"Critical tilt {math.degrees(tilt):.1f}°",
+                timestamp_s=timestamp_s,
+            )
+        if tilt >= self._major or floor_drop_m >= self._drop:
+            value   = max(tilt, floor_drop_m)
+            detail  = (
+                f"Floor drop {floor_drop_m:.2f} m" if floor_drop_m >= self._drop
+                else f"Major tilt {math.degrees(tilt):.1f}°"
+            )
+            return ThreatEvent(
+                threat_type=ThreatType.FALL_RISK,
+                level=ThreatLevel.MAJOR,
+                value=value,
+                detail=detail,
+                timestamp_s=timestamp_s,
+            )
+        if tilt >= self._minor:
+            return ThreatEvent(
+                threat_type=ThreatType.FALL_RISK,
+                level=ThreatLevel.MINOR,
+                value=tilt,
+                detail=f"Tilt advisory {math.degrees(tilt):.1f}°",
+                timestamp_s=timestamp_s,
+            )
+        return ThreatEvent.clear(timestamp_s)
+
+
+# ── StuckDetector ─────────────────────────────────────────────────────────────
+
+class StuckDetector:
+    """
+    Wheel stall / stuck detection from cmd_vel vs odometry mismatch.
+
+    Accumulates stuck time while |cmd| > min_cmd_ms AND |actual| < moving_ms.
+    Resets when motion resumes or commanded speed drops below min_cmd_ms.
+
+    Parameters
+    ----------
+    stuck_timeout_s  : accumulated stuck time before MAJOR fires (default 3.0 s)
+    min_cmd_ms       : minimum commanded speed to consider stalling (0.05 m/s)
+    moving_threshold_ms : actual speed above which robot is considered moving
+    """
+
+    def __init__(
+        self,
+        stuck_timeout_s:     float = 3.0,
+        min_cmd_ms:          float = 0.05,
+        moving_threshold_ms: float = 0.05,
+    ):
+        self._timeout   = max(0.1, stuck_timeout_s)
+        self._min_cmd   = abs(min_cmd_ms)
+        self._moving    = abs(moving_threshold_ms)
+        self._stuck_time: float = 0.0
+
+    @property
+    def stuck_time(self) -> float:
+        """Accumulated stuck duration (s)."""
+        return self._stuck_time
+
+    def update(
+        self,
+        cmd_speed_ms:    float,
+        actual_speed_ms: float,
+        dt:              float,
+        timestamp_s:     float = 0.0,
+    ) -> ThreatEvent:
+        """
+        Parameters
+        ----------
+        cmd_speed_ms    : commanded forward speed (m/s)
+        actual_speed_ms : measured forward speed from odometry (m/s)
+        dt              : elapsed time since last call (s)
+        """
+        commanding = abs(cmd_speed_ms) >= self._min_cmd
+        moving     = abs(actual_speed_ms) >= self._moving
+
+        if not commanding or moving:
+            self._stuck_time = 0.0
+            return ThreatEvent.clear(timestamp_s)
+
+        self._stuck_time += max(0.0, dt)
+
+        if self._stuck_time >= self._timeout:
+            return ThreatEvent(
+                threat_type=ThreatType.WHEEL_STUCK,
+                level=ThreatLevel.MAJOR,
+                value=self._stuck_time,
+                detail=f"Wheels stuck for {self._stuck_time:.1f} s",
+                timestamp_s=timestamp_s,
+            )
+        return ThreatEvent.clear(timestamp_s)
+
+    def reset(self) -> None:
+        self._stuck_time = 0.0
+
+
+# ── BumpDetector ─────────────────────────────────────────────────────────────
+
+class BumpDetector:
+    """
+    Collision / bump detection via IMU acceleration jerk.
+
+    Jerk = |Δ|a|| / dt   where |a| = sqrt(ax²+ay²+az²) − g  (gravity removed)
+
+    Parameters
+    ----------
+    jerk_threshold_ms3          : MAJOR at jerk above this (m/s³, default 8.0)
+    critical_jerk_threshold_ms3 : CRITICAL at jerk above this (m/s³, default 25.0)
+    gravity_ms2                 : gravity magnitude to subtract (default 9.81)
+    """
+
+    def __init__(
+        self,
+        jerk_threshold_ms3:          float = 8.0,
+        critical_jerk_threshold_ms3: float = 25.0,
+        gravity_ms2:                 float = 9.81,
+    ):
+        self._jerk_major    = float(jerk_threshold_ms3)
+        self._jerk_critical = float(critical_jerk_threshold_ms3)
+        self._gravity       = float(gravity_ms2)
+        self._prev_dyn_mag: Optional[float] = None   # previous |a_dynamic|
+
+    def update(
+        self,
+        ax: float,
+        ay: float,
+        az: float,
+        dt: float,
+        timestamp_s: float = 0.0,
+    ) -> ThreatEvent:
+        """
+        Parameters
+        ----------
+        ax, ay, az : IMU linear acceleration (m/s²)
+        dt         : elapsed time since last call (s)
+        """
+        raw_mag = math.sqrt(ax * ax + ay * ay + az * az)
+        dyn_mag = abs(raw_mag - self._gravity)   # remove gravity component
+
+        if self._prev_dyn_mag is None or dt <= 0.0:
+            self._prev_dyn_mag = dyn_mag
+            return ThreatEvent.clear(timestamp_s)
+
+        jerk = abs(dyn_mag - self._prev_dyn_mag) / dt
+        self._prev_dyn_mag = dyn_mag
+
+        if jerk >= self._jerk_critical:
+            return ThreatEvent(
+                threat_type=ThreatType.BUMP,
+                level=ThreatLevel.CRITICAL,
+                value=jerk,
+                detail=f"Critical impact: jerk {jerk:.1f} m/s³",
+                timestamp_s=timestamp_s,
+            )
+        if jerk >= self._jerk_major:
+            return ThreatEvent(
+                threat_type=ThreatType.BUMP,
+                level=ThreatLevel.MAJOR,
+                value=jerk,
+                detail=f"Bump detected: jerk {jerk:.1f} m/s³",
+                timestamp_s=timestamp_s,
+            )
+        return ThreatEvent.clear(timestamp_s)
+
+    def reset(self) -> None:
+        self._prev_dyn_mag = None
+
+
+# ── Utility: pick highest-severity threat ─────────────────────────────────────
+
+def highest_threat(events: list[ThreatEvent]) -> ThreatEvent:
+    """Return the ThreatEvent with the highest ThreatLevel from a list."""
+    if not events:
+        return _CLEAR
+    return max(events, key=lambda e: e.level.value)

jetson/ros2_ws/src/saltybot_emergency/setup.cfg

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

jetson/ros2_ws/src/saltybot_emergency/setup.py

@@ -0,0 +1,32 @@
+from setuptools import setup, find_packages
+import os
+from glob import glob
+
+package_name = "saltybot_emergency"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages",
+         [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (os.path.join("share", package_name, "config"),
+         glob("config/*.yaml")),
+        (os.path.join("share", package_name, "launch"),
+         glob("launch/*.py")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="sl-controls",
+    maintainer_email="sl-controls@saltylab.local",
+    description="Emergency behavior system — collision avoidance, fall prevention, stuck detection, recovery",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            f"emergency_node = {package_name}.emergency_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_emergency/test/test_emergency.py

@@ -0,0 +1,560 @@
+"""
+test_emergency.py — Unit tests for Issue #169 emergency behavior modules.
+
+Covers:
+  ObstacleDetector   — proximity thresholds, speed gate
+  FallDetector       — tilt levels, floor drop
+  StuckDetector      — timeout accumulation, reset on motion
+  BumpDetector       — jerk thresholds, first-call safety
+  AlertManager       — severity mapping, escalation, suppression
+  RecoverySequencer  — full sequence, retry, give-up
+  EmergencyFSM       — all state transitions and guard conditions
+"""
+
+import math
+import sys
+import os
+
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
+
+import pytest
+
+from saltybot_emergency.threat_detector import (
+    BumpDetector,
+    FallDetector,
+    ObstacleDetector,
+    StuckDetector,
+    ThreatEvent,
+    ThreatLevel,
+    ThreatType,
+    highest_threat,
+)
+from saltybot_emergency.alert_manager import Alert, AlertLevel, AlertManager
+from saltybot_emergency.recovery_sequencer import (
+    RecoveryInputs,
+    RecoverySequencer,
+    RecoveryState,
+)
+from saltybot_emergency.emergency_fsm import (
+    EmergencyFSM,
+    EmergencyInputs,
+    EmergencyState,
+)
+
+
+# ── Helpers ───────────────────────────────────────────────────────────────────
+
+def _obs(**kw):
+    d = dict(stop_distance_m=0.30, critical_distance_m=0.10, min_speed_ms=0.05)
+    d.update(kw)
+    return ObstacleDetector(**d)
+
+def _fall(**kw):
+    d = dict(minor_tilt_rad=0.20, major_tilt_rad=0.35,
+             critical_tilt_rad=0.52, floor_drop_m=0.15)
+    d.update(kw)
+    return FallDetector(**d)
+
+def _stuck(**kw):
+    d = dict(stuck_timeout_s=3.0, min_cmd_ms=0.05, moving_threshold_ms=0.05)
+    d.update(kw)
+    return StuckDetector(**d)
+
+def _bump(**kw):
+    d = dict(jerk_threshold_ms3=8.0, critical_jerk_threshold_ms3=25.0)
+    d.update(kw)
+    return BumpDetector(**d)
+
+def _alert_mgr(**kw):
+    d = dict(major_count_threshold=3, escalation_window_s=10.0, suppression_s=1.0)
+    d.update(kw)
+    return AlertManager(**d)
+
+def _seq(**kw):
+    d = dict(
+        reverse_speed_ms=-0.15, reverse_distance_m=0.30,
+        angular_speed_rads=0.60, turn_angle_rad=1.5708,
+        retry_timeout_s=3.0, clear_hold_s=0.5, max_retries=3,
+    )
+    d.update(kw)
+    return RecoverySequencer(**d)
+
+def _fsm(**kw):
+    d = dict(
+        stopped_ms=0.03, major_count_threshold=3, escalation_window_s=10.0,
+        suppression_s=0.0,  # disable suppression for cleaner tests
+        reverse_speed_ms=-0.15, reverse_distance_m=0.30,
+        angular_speed_rads=0.60, turn_angle_rad=1.5708,
+        retry_timeout_s=3.0, clear_hold_s=0.5, max_retries=3,
+    )
+    d.update(kw)
+    return EmergencyFSM(**d)
+
+def _major_threat(threat_type=ThreatType.OBSTACLE_PROXIMITY, ts=0.0):
+    return ThreatEvent(threat_type=threat_type, level=ThreatLevel.MAJOR,
+                       value=1.0, detail="test", timestamp_s=ts)
+
+def _critical_threat(ts=0.0):
+    return ThreatEvent(threat_type=ThreatType.OBSTACLE_PROXIMITY,
+                       level=ThreatLevel.CRITICAL, value=0.05,
+                       detail="critical test", timestamp_s=ts)
+
+def _minor_threat(ts=0.0):
+    return ThreatEvent(threat_type=ThreatType.FALL_RISK, level=ThreatLevel.MINOR,
+                       value=0.21, detail="tilt", timestamp_s=ts)
+
+def _clear_threat():
+    return ThreatEvent.clear()
+
+def _inp(threat=None, speed=0.0, ack=False):
+    return EmergencyInputs(
+        threat=threat or _clear_threat(),
+        robot_speed_ms=speed,
+        acknowledge=ack,
+    )
+
+
+# ══════════════════════════════════════════════════════════════════════════════
+# ObstacleDetector
+# ══════════════════════════════════════════════════════════════════════════════
+
+class TestObstacleDetector:
+
+    def test_clear_when_far(self):
+        ev = _obs().update(0.5, 0.3)
+        assert ev.level == ThreatLevel.CLEAR
+
+    def test_major_within_stop_distance(self):
+        ev = _obs(stop_distance_m=0.30).update(0.25, 0.3)
+        assert ev.level == ThreatLevel.MAJOR
+        assert ev.threat_type == ThreatType.OBSTACLE_PROXIMITY
+
+    def test_critical_within_critical_distance(self):
+        ev = _obs(critical_distance_m=0.10).update(0.05, 0.3)
+        assert ev.level == ThreatLevel.CRITICAL
+
+    def test_clear_when_stopped(self):
+        """Obstacle detection suppressed when not moving."""
+        ev = _obs(min_speed_ms=0.05).update(0.05, 0.01)
+        assert ev.level == ThreatLevel.CLEAR
+
+    def test_active_at_min_speed(self):
+        ev = _obs(min_speed_ms=0.05).update(0.20, 0.06)
+        assert ev.level == ThreatLevel.MAJOR
+
+    def test_value_is_distance(self):
+        ev = _obs().update(0.20, 0.3)
+        assert ev.value == pytest.approx(0.20, abs=1e-9)
+
+
+# ══════════════════════════════════════════════════════════════════════════════
+# FallDetector
+# ══════════════════════════════════════════════════════════════════════════════
+
+class TestFallDetector:
+
+    def test_clear_on_flat(self):
+        ev = _fall().update(0.0, 0.0)
+        assert ev.level == ThreatLevel.CLEAR
+
+    def test_minor_moderate_tilt(self):
+        ev = _fall(minor_tilt_rad=0.20, major_tilt_rad=0.35).update(0.25, 0.0)
+        assert ev.level == ThreatLevel.MINOR
+
+    def test_major_high_tilt(self):
+        ev = _fall(major_tilt_rad=0.35, critical_tilt_rad=0.52).update(0.40, 0.0)
+        assert ev.level == ThreatLevel.MAJOR
+
+    def test_critical_extreme_tilt(self):
+        ev = _fall(critical_tilt_rad=0.52).update(0.60, 0.0)
+        assert ev.level == ThreatLevel.CRITICAL
+
+    def test_major_on_floor_drop(self):
+        ev = _fall(floor_drop_m=0.15).update(0.0, 0.0, floor_drop_m=0.20)
+        assert ev.level == ThreatLevel.MAJOR
+        assert "drop" in ev.detail.lower()
+
+    def test_roll_triggers_same_as_pitch(self):
+        """Roll beyond minor threshold also fires."""
+        ev = _fall(minor_tilt_rad=0.20).update(0.0, 0.25)
+        assert ev.level == ThreatLevel.MINOR
+
+
+# ══════════════════════════════════════════════════════════════════════════════
+# StuckDetector
+# ══════════════════════════════════════════════════════════════════════════════
+
+class TestStuckDetector:
+
+    def test_clear_when_not_commanded(self):
+        s = _stuck(stuck_timeout_s=1.0, min_cmd_ms=0.05)
+        ev = s.update(0.01, 0.0, dt=1.0)  # cmd below threshold
+        assert ev.level == ThreatLevel.CLEAR
+
+    def test_clear_when_moving(self):
+        s = _stuck(stuck_timeout_s=1.0)
+        ev = s.update(0.2, 0.2, dt=1.0)   # actually moving
+        assert ev.level == ThreatLevel.CLEAR
+
+    def test_major_after_timeout(self):
+        s = _stuck(stuck_timeout_s=3.0, min_cmd_ms=0.05, moving_threshold_ms=0.05)
+        for _ in range(6):
+            ev = s.update(0.2, 0.0, dt=0.5)   # cmd=0.2, actual=0 → stuck
+        assert ev.level == ThreatLevel.MAJOR
+
+    def test_no_major_before_timeout(self):
+        s = _stuck(stuck_timeout_s=3.0)
+        ev = s.update(0.2, 0.0, dt=1.0)   # only 1s — not yet
+        assert ev.level == ThreatLevel.CLEAR
+
+    def test_reset_on_motion_resume(self):
+        s = _stuck(stuck_timeout_s=1.0)
+        s.update(0.2, 0.0, dt=0.8)        # accumulate stuck time
+        s.update(0.2, 0.3, dt=0.1)        # motion resumes → reset
+        ev = s.update(0.2, 0.0, dt=0.3)   # only 0.3s since reset → still clear
+        assert ev.level == ThreatLevel.CLEAR
+
+    def test_stuck_time_property(self):
+        s = _stuck(stuck_timeout_s=5.0)
+        s.update(0.2, 0.0, dt=1.0)
+        s.update(0.2, 0.0, dt=1.0)
+        assert s.stuck_time == pytest.approx(2.0, abs=1e-6)
+
+
+# ══════════════════════════════════════════════════════════════════════════════
+# BumpDetector
+# ══════════════════════════════════════════════════════════════════════════════
+
+class TestBumpDetector:
+
+    def test_clear_on_first_call(self):
+        """No jerk on first sample (no previous value)."""
+        ev = _bump().update(0.0, 0.0, 9.81, dt=0.05)
+        assert ev.level == ThreatLevel.CLEAR
+
+    def test_major_on_jerk(self):
+        b = _bump(jerk_threshold_ms3=5.0, critical_jerk_threshold_ms3=20.0)
+        b.update(0.0, 0.0, 9.81, dt=0.05)    # seed → dyn_mag = 0
+        # ax=4.5: raw≈10.79, dyn≈0.98, jerk≈9.8 m/s³ → MAJOR (5.0 < 9.8 < 20.0)
+        ev = b.update(4.5, 0.0, 9.81, dt=0.1)
+        assert ev.level == ThreatLevel.MAJOR
+
+    def test_critical_on_severe_jerk(self):
+        b = _bump(jerk_threshold_ms3=5.0, critical_jerk_threshold_ms3=20.0)
+        b.update(0.0, 0.0, 9.81, dt=0.05)
+        # Very large spike
+        ev = b.update(50.0, 0.0, 9.81, dt=0.1)
+        assert ev.level == ThreatLevel.CRITICAL
+
+    def test_clear_on_gentle_acceleration(self):
+        b = _bump(jerk_threshold_ms3=8.0)
+        b.update(0.0, 0.0, 9.81, dt=0.05)
+        ev = b.update(0.1, 0.0, 9.81, dt=0.05)   # tiny change
+        assert ev.level == ThreatLevel.CLEAR
+
+
+# ══════════════════════════════════════════════════════════════════════════════
+# highest_threat
+# ══════════════════════════════════════════════════════════════════════════════
+
+class TestHighestThreat:
+
+    def test_empty_returns_clear(self):
+        assert highest_threat([]).level == ThreatLevel.CLEAR
+
+    def test_picks_highest(self):
+        a = ThreatEvent(level=ThreatLevel.MINOR)
+        b = ThreatEvent(level=ThreatLevel.CRITICAL)
+        c = ThreatEvent(level=ThreatLevel.MAJOR)
+        assert highest_threat([a, b, c]).level == ThreatLevel.CRITICAL
+
+    def test_single_item(self):
+        ev = ThreatEvent(level=ThreatLevel.MAJOR)
+        assert highest_threat([ev]) is ev
+
+
+# ══════════════════════════════════════════════════════════════════════════════
+# AlertManager
+# ══════════════════════════════════════════════════════════════════════════════
+
+class TestAlertManager:
+
+    def test_clear_returns_none(self):
+        am = _alert_mgr()
+        assert am.update(_clear_threat()) is None
+
+    def test_minor_threat_gives_minor_alert(self):
+        am = _alert_mgr(suppression_s=0.0)
+        alert = am.update(_minor_threat(ts=0.0))
+        assert alert is not None
+        assert alert.level == AlertLevel.MINOR
+
+    def test_major_threat_gives_major_alert(self):
+        am = _alert_mgr(suppression_s=0.0)
+        alert = am.update(_major_threat(ts=0.0))
+        assert alert is not None
+        assert alert.level == AlertLevel.MAJOR
+
+    def test_critical_threat_gives_critical_alert(self):
+        am = _alert_mgr(suppression_s=0.0)
+        alert = am.update(_critical_threat(ts=0.0))
+        assert alert is not None
+        assert alert.level == AlertLevel.CRITICAL
+
+    def test_suppression_blocks_duplicate(self):
+        am = _alert_mgr(suppression_s=5.0)
+        am.update(_major_threat(ts=0.0))
+        alert = am.update(_major_threat(ts=1.0))   # within 5s window
+        assert alert is None
+
+    def test_suppression_expires(self):
+        am = _alert_mgr(suppression_s=2.0)
+        am.update(_major_threat(ts=0.0))
+        alert = am.update(_major_threat(ts=3.0))   # after 2s window
+        assert alert is not None
+
+    def test_escalation_major_to_critical(self):
+        """After major_count_threshold major alerts, next one becomes CRITICAL."""
+        am = _alert_mgr(major_count_threshold=3, escalation_window_s=60.0,
+                        suppression_s=0.0)
+        for i in range(3):
+            am.update(_major_threat(ts=float(i)))
+        # 4th should be escalated
+        alert = am.update(_major_threat(ts=4.0))
+        assert alert is not None
+        assert alert.level == AlertLevel.CRITICAL
+
+    def test_escalation_resets_after_window(self):
+        """Major alerts outside the window don't contribute to escalation."""
+        am = _alert_mgr(major_count_threshold=3, escalation_window_s=5.0,
+                        suppression_s=0.0)
+        am.update(_major_threat(ts=0.0))
+        am.update(_major_threat(ts=1.0))
+        am.update(_major_threat(ts=2.0))
+        # All 3 are old; new one at t=10 is outside window
+        alert = am.update(_major_threat(ts=10.0))
+        assert alert is not None
+        assert alert.level == AlertLevel.MAJOR   # not escalated
+
+    def test_reset_clears_escalation_state(self):
+        am = _alert_mgr(major_count_threshold=2, suppression_s=0.0)
+        am.update(_major_threat(ts=0.0))
+        am.update(_major_threat(ts=1.0))  # now at threshold
+        am.reset()
+        alert = am.update(_major_threat(ts=2.0))
+        assert alert.level == AlertLevel.MAJOR  # back to major after reset
+
+
+# ══════════════════════════════════════════════════════════════════════════════
+# RecoverySequencer
+# ══════════════════════════════════════════════════════════════════════════════
+
+class TestRecoverySequencer:
+
+    def _trigger(self, seq):
+        return seq.tick(RecoveryInputs(trigger=True, dt=0.02))
+
+    def test_idle_on_init(self):
+        seq = _seq()
+        assert seq.state == RecoveryState.IDLE
+
+    def test_trigger_starts_reversing(self):
+        seq = _seq()
+        out = self._trigger(seq)
+        assert seq.state == RecoveryState.REVERSING
+
+    def test_reversing_backward_velocity(self):
+        seq = _seq(reverse_speed_ms=-0.15)
+        self._trigger(seq)
+        out = seq.tick(RecoveryInputs(dt=0.02))
+        assert out.cmd_linear < 0.0
+
+    def test_reversing_completes_to_turning(self):
+        seq = _seq(reverse_speed_ms=-1.0, reverse_distance_m=0.5)
+        self._trigger(seq)
+        for _ in range(30):
+            out = seq.tick(RecoveryInputs(dt=0.02))
+        assert seq.state == RecoveryState.TURNING
+
+    def test_turning_positive_angular(self):
+        seq = _seq(reverse_speed_ms=-1.0, reverse_distance_m=0.1,
+                   angular_speed_rads=1.0)
+        self._trigger(seq)
+        # Skip through reversing quickly
+        for _ in range(20):
+            seq.tick(RecoveryInputs(dt=0.02))
+        if seq.state == RecoveryState.TURNING:
+            out = seq.tick(RecoveryInputs(dt=0.02))
+            assert out.cmd_angular > 0.0
+
+    def test_retrying_increments_count(self):
+        seq = _seq(reverse_speed_ms=-1.0, reverse_distance_m=0.05,
+                   angular_speed_rads=10.0, turn_angle_rad=0.1)
+        self._trigger(seq)
+        for _ in range(100):
+            seq.tick(RecoveryInputs(dt=0.02))
+        assert seq.state == RecoveryState.RETRYING
+        assert seq.retry_count == 1
+
+    def test_threat_cleared_returns_idle(self):
+        seq = _seq(reverse_speed_ms=-1.0, reverse_distance_m=0.05,
+                   angular_speed_rads=10.0, turn_angle_rad=0.1,
+                   clear_hold_s=0.1)
+        self._trigger(seq)
+        # Fast-forward to RETRYING
+        for _ in range(100):
+            seq.tick(RecoveryInputs(dt=0.02))
+        assert seq.state == RecoveryState.RETRYING
+        # Feed cleared ticks until clear_hold met
+        for _ in range(20):
+            seq.tick(RecoveryInputs(threat_cleared=True, dt=0.02))
+        assert seq.state == RecoveryState.IDLE
+
+    def test_max_retries_gives_up(self):
+        seq = _seq(reverse_speed_ms=-1.0, reverse_distance_m=0.05,
+                   angular_speed_rads=10.0, turn_angle_rad=0.1,
+                   retry_timeout_s=0.1, max_retries=2)
+        self._trigger(seq)
+        for _ in range(500):
+            out = seq.tick(RecoveryInputs(threat_cleared=False, dt=0.05))
+            if seq.state == RecoveryState.GAVE_UP:
+                break
+        assert seq.state == RecoveryState.GAVE_UP
+
+    def test_reset_returns_to_idle(self):
+        seq = _seq()
+        self._trigger(seq)
+        seq.reset()
+        assert seq.state == RecoveryState.IDLE
+        assert seq.retry_count == 0
+
+
+# ══════════════════════════════════════════════════════════════════════════════
+# EmergencyFSM
+# ══════════════════════════════════════════════════════════════════════════════
+
+class TestEmergencyFSMBasic:
+
+    def test_initial_state_nominal(self):
+        fsm = _fsm()
+        assert fsm.state == EmergencyState.NOMINAL
+
+    def test_nominal_stays_on_clear(self):
+        fsm = _fsm()
+        out = fsm.tick(_inp())
+        assert fsm.state == EmergencyState.NOMINAL
+        assert out.cmd_override is False
+
+    def test_minor_alert_no_override(self):
+        fsm = _fsm()
+        out = fsm.tick(_inp(_minor_threat(ts=0.0)))
+        assert fsm.state == EmergencyState.NOMINAL
+        assert out.cmd_override is False
+        assert out.alert is not None
+        assert out.alert.level == AlertLevel.MINOR
+
+    def test_major_threat_enters_stopping(self):
+        fsm = _fsm()
+        out = fsm.tick(_inp(_major_threat()))
+        assert fsm.state == EmergencyState.STOPPING
+        assert out.cmd_override is True
+
+    def test_critical_threat_enters_stopping_critical_pending(self):
+        fsm = _fsm()
+        fsm.tick(_inp(_critical_threat()))
+        assert fsm.state == EmergencyState.STOPPING
+        assert fsm._critical_pending is True
+
+
+class TestEmergencyFSMStopping:
+
+    def test_stopping_commands_zero(self):
+        fsm = _fsm()
+        fsm.tick(_inp(_major_threat()))
+        out = fsm.tick(_inp(_major_threat(), speed=0.5))
+        assert out.cmd_linear  == pytest.approx(0.0, abs=1e-9)
+        assert out.cmd_angular == pytest.approx(0.0, abs=1e-9)
+
+    def test_stopped_enters_recovering(self):
+        fsm = _fsm(stopped_ms=0.03)
+        fsm.tick(_inp(_major_threat()))
+        out = fsm.tick(_inp(_major_threat(), speed=0.01))  # below stopped_ms
+        assert fsm.state == EmergencyState.RECOVERING
+
+    def test_critical_pending_enters_escalated(self):
+        fsm = _fsm(stopped_ms=0.03)
+        fsm.tick(_inp(_critical_threat()))
+        fsm.tick(_inp(_critical_threat(), speed=0.01))  # stopped → ESCALATED
+        assert fsm.state == EmergencyState.ESCALATED
+
+
+class TestEmergencyFSMRecovering:
+
+    def _reach_recovering(self, fsm):
+        fsm.tick(_inp(_major_threat()))
+        fsm.tick(_inp(_major_threat(), speed=0.0))  # stopped → RECOVERING
+        assert fsm.state == EmergencyState.RECOVERING
+
+    def test_recovering_has_cmd_override(self):
+        fsm = _fsm()
+        self._reach_recovering(fsm)
+        out = fsm.tick(_inp(_clear_threat()))
+        assert out.cmd_override is True
+
+    def test_recovering_gave_up_escalates(self):
+        fsm = _fsm(max_retries=1, retry_timeout_s=0.05)
+        self._reach_recovering(fsm)
+        # Drive recovery to GAVE_UP by feeding many non-clearing ticks
+        for _ in range(500):
+            out = fsm.tick(_inp(_major_threat()))
+            if fsm.state == EmergencyState.ESCALATED:
+                break
+        assert fsm.state == EmergencyState.ESCALATED
+
+
+class TestEmergencyFSMEscalated:
+
+    def _reach_escalated(self, fsm):
+        fsm.tick(_inp(_critical_threat()))
+        fsm.tick(_inp(_critical_threat(), speed=0.0))
+        assert fsm.state == EmergencyState.ESCALATED
+
+    def test_escalated_emits_critical_alert_once(self):
+        fsm = _fsm()
+        self._reach_escalated(fsm)
+        out1 = fsm.tick(_inp())
+        out2 = fsm.tick(_inp())
+        assert out1.alert is not None
+        assert out1.alert.level == AlertLevel.CRITICAL
+        assert out2.alert is None   # suppressed after first emission
+
+    def test_escalated_e_stop_asserted(self):
+        fsm = _fsm()
+        self._reach_escalated(fsm)
+        out = fsm.tick(_inp())
+        assert out.e_stop is True
+
+    def test_escalated_stays_without_ack(self):
+        fsm = _fsm()
+        self._reach_escalated(fsm)
+        for _ in range(5):
+            fsm.tick(_inp())
+        assert fsm.state == EmergencyState.ESCALATED
+
+    def test_acknowledge_returns_to_nominal(self):
+        fsm = _fsm()
+        self._reach_escalated(fsm)
+        fsm.tick(_inp(ack=True))
+        assert fsm.state == EmergencyState.NOMINAL
+
+    def test_reset_returns_to_nominal(self):
+        fsm = _fsm()
+        self._reach_escalated(fsm)
+        fsm.reset()
+        assert fsm.state == EmergencyState.NOMINAL
+
+    def test_e_stop_cleared_on_ack(self):
+        fsm = _fsm()
+        self._reach_escalated(fsm)
+        out = fsm.tick(_inp(ack=True))
+        assert out.e_stop is False

jetson/ros2_ws/src/saltybot_emergency_msgs/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.8)
+project(saltybot_emergency_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/EmergencyEvent.msg"
+  "msg/RecoveryAction.msg"
+  DEPENDENCIES builtin_interfaces
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

jetson/ros2_ws/src/saltybot_emergency_msgs/msg/EmergencyEvent.msg

@@ -0,0 +1,25 @@
+# EmergencyEvent.msg — Real-time emergency system state snapshot (Issue #169)
+# Published by: /saltybot/emergency_node
+# Topic: /saltybot/emergency
+
+builtin_interfaces/Time stamp
+
+# Overall FSM state
+# Values: "NOMINAL" | "STOPPING" | "RECOVERING" | "ESCALATED"
+string state
+
+# Active threat (highest severity across all detectors)
+# threat_type values: "NONE" | "OBSTACLE_PROXIMITY" | "FALL_RISK" | "WHEEL_STUCK" | "BUMP"
+string threat_type
+
+# Severity: "CLEAR" | "MINOR" | "MAJOR" | "CRITICAL"
+string severity
+
+# Triggering metric value (e.g. distance in m, jerk in m/s³, stuck seconds)
+float32 threat_value
+
+# Human-readable description of the active threat
+string detail
+
+# True when emergency system is overriding normal cmd_vel with its own commands
+bool cmd_override

jetson/ros2_ws/src/saltybot_emergency_msgs/msg/RecoveryAction.msg

@@ -0,0 +1,15 @@
+# RecoveryAction.msg — Recovery sequencer state (Issue #169)
+# Published by: /saltybot/emergency_node
+# Topic: /saltybot/recovery_action
+
+builtin_interfaces/Time stamp
+
+# Current recovery action
+# Values: "IDLE" | "REVERSING" | "TURNING" | "RETRYING" | "GAVE_UP"
+string action
+
+# Number of reverse+turn attempts completed so far
+int32 retry_count
+
+# Progress through current phase [0.0 – 1.0]
+float32 progress

jetson/ros2_ws/src/saltybot_emergency_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_emergency_msgs</name>
+  <version>0.1.0</version>
+  <description>Emergency behavior message definitions for SaltyBot (Issue #169)</description>
+  <maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <depend>builtin_interfaces</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>

ui/social-bot/src/App.jsx

@@ -5,13 +5,16 @@
  *   Status | Faces | Conversation | Personality | Navigation
  *
  * Telemetry tabs (issue #126):
- *   IMU | Battery | Motors | Map | Control | Health
+ *   IMU | Battery | Motors | Map | Control | Health | Cameras
  *
  * Fleet tabs (issue #139):
  *   Fleet (self-contained via useFleet)
  *
  * Mission tabs (issue #145):
  *   Missions (waypoint editor, route builder, geofence, schedule, execute)
+ *
+ * Camera viewer (issue #177):
+ *   CSI × 4 + D435i RGB/depth + panoramic, detection overlays, recording
  */
 
 import { useState, useCallback } from 'react';
@@ -41,6 +44,9 @@ import { MissionPlanner } from './components/MissionPlanner.jsx';
 // Settings panel (issue #160)
 import { SettingsPanel } from './components/SettingsPanel.jsx';
 
+// Camera viewer (issue #177)
+import { CameraViewer } from './components/CameraViewer.jsx';
+
 const TAB_GROUPS = [
   {
     label: 'SOCIAL',
@@ -63,6 +69,7 @@ const TAB_GROUPS = [
       { id: 'map',     label: 'Map',     },
       { id: 'control', label: 'Control', },
       { id: 'health',  label: 'Health',  },
+      { id: 'cameras', label: 'Cameras', },
     ],
   },
   {
@@ -206,6 +213,7 @@ export default function App() {
         {activeTab === 'map'     && <MapViewer    subscribe={subscribe} />}
         {activeTab === 'control' && <ControlMode  subscribe={subscribe} />}
         {activeTab === 'health'  && <SystemHealth subscribe={subscribe} />}
+        {activeTab === 'cameras' && <CameraViewer subscribe={subscribe} />}
 
         {activeTab === 'fleet'    && <FleetPanel />}
         {activeTab === 'missions' && <MissionPlanner />}

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

@@ -0,0 +1,671 @@
+/**
+ * CameraViewer.jsx — Live camera stream viewer (Issue #177).
+ *
+ * Features:
+ *   - 7 cameras: front/left/rear/right (CSI), D435i RGB/depth, panoramic
+ *   - Detection overlays: face boxes + names, gesture icons, scene object labels
+ *   - 360° panoramic equirect viewer with mouse drag pan
+ *   - One-click recording (MP4/WebM) + download
+ *   - Snapshot to PNG with annotations + timestamp
+ *   - Picture-in-picture (up to 3 pinned cameras)
+ *   - Per-camera FPS indicator + adaptive quality badge
+ *
+ * Topics consumed:
+ *   /camera/<name>/image_raw/compressed    sensor_msgs/CompressedImage
+ *   /camera/color/image_raw/compressed     sensor_msgs/CompressedImage  (D435i)
+ *   /camera/depth/image_rect_raw/compressed sensor_msgs/CompressedImage  (D435i)
+ *   /camera/panoramic/compressed            sensor_msgs/CompressedImage
+ *   /social/faces/detections               saltybot_social_msgs/FaceDetectionArray
+ *   /social/gestures                       saltybot_social_msgs/GestureArray
+ *   /social/scene/objects                  saltybot_scene_msgs/SceneObjectArray
+ */
+
+import { useEffect, useRef, useState, useCallback } from 'react';
+import { useCamera, CAMERAS, CAMERA_BY_ID, CAMERA_BY_ROS_ID } from '../hooks/useCamera.js';
+
+// ── Constants ─────────────────────────────────────────────────────────────────
+
+const GESTURE_ICONS = {
+  wave:         '👋',
+  point:        '👆',
+  stop_palm:    '✋',
+  thumbs_up:    '👍',
+  thumbs_down:  '👎',
+  come_here:    '🤏',
+  follow:       '☞',
+  arms_up:      '🙌',
+  crouch:       '⬇',
+  arms_spread:  '↔',
+};
+
+const HAZARD_COLORS = {
+  1: '#f59e0b',  // stairs — amber
+  2: '#ef4444',  // drop — red
+  3: '#60a5fa',  // wet floor — blue
+  4: '#a855f7',  // glass door — purple
+  5: '#f97316',  // pet — orange
+};
+
+// ── Detection overlay drawing helpers ─────────────────────────────────────────
+
+function drawFaceBoxes(ctx, faces, scaleX, scaleY) {
+  for (const face of faces) {
+    const x = face.bbox_x * scaleX;
+    const y = face.bbox_y * scaleY;
+    const w = face.bbox_w * scaleX;
+    const h = face.bbox_h * scaleY;
+
+    const isKnown = face.person_name && face.person_name !== 'unknown';
+    ctx.strokeStyle = isKnown ? '#06b6d4' : '#f59e0b';
+    ctx.lineWidth   = 2;
+    ctx.shadowBlur  = 6;
+    ctx.shadowColor = ctx.strokeStyle;
+    ctx.strokeRect(x, y, w, h);
+    ctx.shadowBlur  = 0;
+
+    // Corner accent marks
+    const cLen = 8;
+    ctx.lineWidth = 3;
+    [[x,y,1,1],[x+w,y,-1,1],[x,y+h,1,-1],[x+w,y+h,-1,-1]].forEach(([cx,cy,dx,dy]) => {
+      ctx.beginPath();
+      ctx.moveTo(cx, cy + dy * cLen);
+      ctx.lineTo(cx, cy);
+      ctx.lineTo(cx + dx * cLen, cy);
+      ctx.stroke();
+    });
+
+    // Label
+    const label = isKnown
+      ? `${face.person_name} ${(face.recognition_score * 100).toFixed(0)}%`
+      : `face #${face.face_id}`;
+    ctx.font      = 'bold 11px monospace';
+    const tw      = ctx.measureText(label).width;
+    ctx.fillStyle = isKnown ? 'rgba(6,182,212,0.8)' : 'rgba(245,158,11,0.8)';
+    ctx.fillRect(x, y - 16, tw + 6, 16);
+    ctx.fillStyle = '#000';
+    ctx.fillText(label, x + 3, y - 4);
+  }
+}
+
+function drawGestureIcons(ctx, gestures, activeCamId, scaleX, scaleY) {
+  for (const g of gestures) {
+    // Only show gestures from the currently viewed camera
+    const cam = CAMERA_BY_ROS_ID[g.camera_id];
+    if (!cam || cam.cameraId !== activeCamId) continue;
+
+    const x = g.hand_x * ctx.canvas.width;
+    const y = g.hand_y * ctx.canvas.height;
+    const icon = GESTURE_ICONS[g.gesture_type] ?? '?';
+
+    ctx.font      = '24px serif';
+    ctx.shadowBlur  = 8;
+    ctx.shadowColor = '#f97316';
+    ctx.fillText(icon, x - 12, y + 8);
+    ctx.shadowBlur = 0;
+
+    ctx.font      = 'bold 10px monospace';
+    ctx.fillStyle = '#f97316';
+    const label   = g.gesture_type;
+    ctx.fillText(label, x - ctx.measureText(label).width / 2, y + 22);
+  }
+}
+
+function drawSceneObjects(ctx, objects, scaleX, scaleY) {
+  for (const obj of objects) {
+    // vision_msgs/BoundingBox2D: center_x, center_y, size_x, size_y
+    const bb  = obj.bbox;
+    const cx  = bb?.center?.x ?? bb?.center_x;
+    const cy  = bb?.center?.y ?? bb?.center_y;
+    const sw  = bb?.size_x ?? 0;
+    const sh  = bb?.size_y ?? 0;
+    if (cx == null) continue;
+
+    const x = (cx - sw / 2) * scaleX;
+    const y = (cy - sh / 2) * scaleY;
+    const w = sw * scaleX;
+    const h = sh * scaleY;
+
+    const color = HAZARD_COLORS[obj.hazard_type] ?? '#22c55e';
+    ctx.strokeStyle = color;
+    ctx.lineWidth   = 1.5;
+    ctx.setLineDash([4, 3]);
+    ctx.strokeRect(x, y, w, h);
+    ctx.setLineDash([]);
+
+    const dist  = obj.distance_m > 0 ? ` ${obj.distance_m.toFixed(1)}m` : '';
+    const label = `${obj.class_name}${dist}`;
+    ctx.font      = '10px monospace';
+    const tw      = ctx.measureText(label).width;
+    ctx.fillStyle = `${color}cc`;
+    ctx.fillRect(x, y + h, tw + 4, 14);
+    ctx.fillStyle = '#000';
+    ctx.fillText(label, x + 2, y + h + 11);
+  }
+}
+
+// ── Overlay canvas ─────────────────────────────────────────────────────────────
+
+function OverlayCanvas({ faces, gestures, sceneObjects, activeCam, containerW, containerH }) {
+  const canvasRef = useRef(null);
+
+  useEffect(() => {
+    const canvas = canvasRef.current;
+    if (!canvas) return;
+    const ctx = canvas.getContext('2d');
+    ctx.clearRect(0, 0, canvas.width, canvas.height);
+
+    if (!activeCam) return;
+
+    const scaleX = canvas.width  / (activeCam.width  || 640);
+    const scaleY = canvas.height / (activeCam.height || 480);
+
+    // Draw overlays: only for front camera (face + gesture source)
+    if (activeCam.id === 'front') {
+      drawFaceBoxes(ctx, faces, scaleX, scaleY);
+    }
+    if (!activeCam.isPanoramic) {
+      drawGestureIcons(ctx, gestures, activeCam.cameraId, scaleX, scaleY);
+    }
+    if (activeCam.id === 'color') {
+      drawSceneObjects(ctx, sceneObjects, scaleX, scaleY);
+    }
+  }, [faces, gestures, sceneObjects, activeCam]);
+
+  return (
+    <canvas
+      ref={canvasRef}
+      width={containerW || 640}
+      height={containerH || 480}
+      className="absolute inset-0 w-full h-full pointer-events-none"
+    />
+  );
+}
+
+// ── Panoramic equirect viewer ──────────────────────────────────────────────────
+
+function PanoViewer({ frameUrl }) {
+  const canvasRef = useRef(null);
+  const azRef     = useRef(0);    // 0–1920px offset
+  const dragRef   = useRef(null);
+  const imgRef    = useRef(null);
+
+  const draw = useCallback(() => {
+    const canvas = canvasRef.current;
+    const img    = imgRef.current;
+    if (!canvas || !img || !img.complete) return;
+
+    const ctx  = canvas.getContext('2d');
+    const W    = canvas.width;
+    const H    = canvas.height;
+    const iW   = img.naturalWidth;   // 1920
+    const iH   = img.naturalHeight;  // 960
+    const vW   = iW / 2;             // viewport = 50% of equirect width
+    const vH   = Math.round((H / W) * vW);
+    const vY   = Math.round((iH - vH) / 2);
+    const off  = Math.round(azRef.current) % iW;
+
+    ctx.clearRect(0, 0, W, H);
+
+    // Draw left segment
+    const srcX1 = off;
+    const srcW1 = Math.min(vW, iW - off);
+    const dstW1 = Math.round((srcW1 / vW) * W);
+    if (dstW1 > 0) {
+      ctx.drawImage(img, srcX1, vY, srcW1, vH, 0, 0, dstW1, H);
+    }
+
+    // Draw wrapped right segment (if viewport crosses 0°)
+    if (srcW1 < vW) {
+      const srcX2 = 0;
+      const srcW2 = vW - srcW1;
+      const dstX2 = dstW1;
+      const dstW2 = W - dstW1;
+      ctx.drawImage(img, srcX2, vY, srcW2, vH, dstX2, 0, dstW2, H);
+    }
+
+    // Compass badge
+    const azDeg = Math.round((azRef.current / iW) * 360);
+    ctx.fillStyle = 'rgba(0,0,0,0.5)';
+    ctx.fillRect(W - 58, 6, 52, 18);
+    ctx.fillStyle = '#06b6d4';
+    ctx.font      = 'bold 11px monospace';
+    ctx.fillText(`${azDeg}°`, W - 52, 19);
+  }, []);
+
+  // Load image when URL changes
+  useEffect(() => {
+    if (!frameUrl) return;
+    const img = new Image();
+    img.onload = draw;
+    img.src    = frameUrl;
+    imgRef.current = img;
+  }, [frameUrl, draw]);
+
+  // Re-draw when azimuth changes
+  const onMouseDown = e => { dragRef.current = e.clientX; };
+  const onMouseMove = e => {
+    if (dragRef.current == null) return;
+    const dx = e.clientX - dragRef.current;
+    dragRef.current = e.clientX;
+    azRef.current = ((azRef.current - dx * 2) % 1920 + 1920) % 1920;
+    draw();
+  };
+  const onMouseUp = () => { dragRef.current = null; };
+
+  const onTouchStart = e => { dragRef.current = e.touches[0].clientX; };
+  const onTouchMove  = e => {
+    if (dragRef.current == null) return;
+    const dx = e.touches[0].clientX - dragRef.current;
+    dragRef.current = e.touches[0].clientX;
+    azRef.current = ((azRef.current - dx * 2) % 1920 + 1920) % 1920;
+    draw();
+  };
+
+  return (
+    <canvas
+      ref={canvasRef}
+      width={960}
+      height={240}
+      className="w-full object-contain bg-black cursor-ew-resize rounded"
+      onMouseDown={onMouseDown}
+      onMouseMove={onMouseMove}
+      onMouseUp={onMouseUp}
+      onMouseLeave={onMouseUp}
+      onTouchStart={onTouchStart}
+      onTouchMove={onTouchMove}
+      onTouchEnd={() => { dragRef.current = null; }}
+    />
+  );
+}
+
+// ── PiP mini window ────────────────────────────────────────────────────────────
+
+function PiPWindow({ cam, frameUrl, fps, onClose, index }) {
+  const positions = [
+    'bottom-2 left-2',
+    'bottom-2 left-40',
+    'bottom-2 left-[18rem]',
+  ];
+  return (
+    <div className={`absolute ${positions[index] ?? 'bottom-2 left-2'} w-36 rounded border border-cyan-900 overflow-hidden bg-black shadow-lg shadow-black z-10`}>
+      <div className="flex items-center justify-between px-1.5 py-0.5 bg-gray-950 text-xs">
+        <span className="text-cyan-700 font-bold">{cam.label}</span>
+        <div className="flex items-center gap-1">
+          <span className="text-gray-700">{fps}fps</span>
+          <button onClick={onClose} className="text-gray-600 hover:text-red-400">✕</button>
+        </div>
+      </div>
+      {frameUrl ? (
+        <img src={frameUrl} alt={cam.label} className="w-full aspect-video object-cover block" />
+      ) : (
+        <div className="w-full aspect-video flex items-center justify-center text-gray-800 text-xs">
+          no signal
+        </div>
+      )}
+    </div>
+  );
+}
+
+// ── Camera selector strip ──────────────────────────────────────────────────────
+
+function CameraStrip({ cameras, activeId, pipList, frames, fps, onSelect, onTogglePip }) {
+  return (
+    <div className="flex gap-1.5 flex-wrap">
+      {cameras.map(cam => {
+        const hasFrame = !!frames[cam.id];
+        const camFps   = fps[cam.id] ?? 0;
+        const isActive = activeId === cam.id;
+        const isPip    = pipList.includes(cam.id);
+
+        return (
+          <div key={cam.id} className="relative">
+            <button
+              onClick={() => onSelect(cam.id)}
+              className={`flex flex-col items-start rounded border px-2.5 py-1.5 text-xs font-bold transition-colors ${
+                isActive
+                  ? 'border-cyan-500 bg-cyan-950 bg-opacity-50 text-cyan-300'
+                  : hasFrame
+                  ? 'border-gray-700 bg-gray-900 text-gray-400 hover:border-cyan-800 hover:text-gray-200'
+                  : 'border-gray-800 bg-gray-950 text-gray-700 hover:border-gray-700'
+              }`}
+            >
+              <span>{cam.label.toUpperCase()}</span>
+              <span className={`text-xs font-normal mt-0.5 ${
+                camFps >= 12 ? 'text-green-600' :
+                camFps > 0   ? 'text-amber-600' :
+                               'text-gray-700'
+              }`}>
+                {camFps > 0 ? `${camFps}fps` : 'no signal'}
+              </span>
+            </button>
+
+            {/* PiP pin button — only when NOT the active camera */}
+            {!isActive && (
+              <button
+                onClick={() => onTogglePip(cam.id)}
+                title={isPip ? 'Unpin PiP' : 'Pin PiP'}
+                className={`absolute -top-1.5 -right-1.5 w-4 h-4 rounded-full text-[9px] flex items-center justify-center border transition-colors ${
+                  isPip
+                    ? 'bg-cyan-600 border-cyan-400 text-white'
+                    : 'bg-gray-800 border-gray-700 text-gray-600 hover:border-cyan-700 hover:text-cyan-500'
+                }`}
+              >
+                {isPip ? '×' : '⊕'}
+              </button>
+            )}
+          </div>
+        );
+      })}
+    </div>
+  );
+}
+
+// ── Recording bar ──────────────────────────────────────────────────────────────
+
+function RecordingBar({ recording, recSeconds, onStart, onStop, onSnapshot, overlayRef }) {
+  const fmtTime = s => `${String(Math.floor(s / 60)).padStart(2, '0')}:${String(s % 60).padStart(2, '0')}`;
+
+  return (
+    <div className="flex items-center gap-2 flex-wrap">
+      {!recording ? (
+        <button
+          onClick={onStart}
+          className="flex items-center gap-1.5 px-3 py-1.5 rounded border border-red-900 bg-red-950 text-red-400 hover:bg-red-900 text-xs font-bold transition-colors"
+        >
+          <span className="w-2 h-2 rounded-full bg-red-500" />
+          REC
+        </button>
+      ) : (
+        <button
+          onClick={onStop}
+          className="flex items-center gap-1.5 px-3 py-1.5 rounded border border-red-600 bg-red-900 text-red-300 hover:bg-red-800 text-xs font-bold animate-pulse"
+        >
+          <span className="w-2 h-2 rounded bg-red-400" />
+          STOP  {fmtTime(recSeconds)}
+        </button>
+      )}
+
+      <button
+        onClick={() => onSnapshot(overlayRef?.current)}
+        className="flex items-center gap-1 px-3 py-1.5 rounded border border-gray-700 bg-gray-900 text-gray-400 hover:border-cyan-700 hover:text-cyan-400 text-xs font-bold transition-colors"
+      >
+        📷 SNAP
+      </button>
+
+      {recording && (
+        <span className="text-xs text-red-500 animate-pulse font-mono">
+          ● RECORDING {fmtTime(recSeconds)}
+        </span>
+      )}
+    </div>
+  );
+}
+
+// ── Main component ─────────────────────────────────────────────────────────────
+
+export function CameraViewer({ subscribe }) {
+  const {
+    cameras, frames, fps,
+    activeId, setActiveId,
+    pipList, togglePip,
+    recording, recSeconds,
+    startRecording, stopRecording,
+    takeSnapshot,
+  } = useCamera({ subscribe });
+
+  // ── Detection state ─────────────────────────────────────────────────────────
+  const [faces,        setFaces]        = useState([]);
+  const [gestures,     setGestures]     = useState([]);
+  const [sceneObjects, setSceneObjects] = useState([]);
+
+  const [showOverlay, setShowOverlay] = useState(true);
+  const [overlayMode, setOverlayMode] = useState('all'); // 'all' | 'faces' | 'gestures' | 'objects' | 'off'
+
+  const overlayCanvasRef = useRef(null);
+
+  // Subscribe to detection topics
+  useEffect(() => {
+    if (!subscribe) return;
+    const u1 = subscribe('/social/faces/detections', 'saltybot_social_msgs/FaceDetectionArray', msg => {
+      setFaces(msg.faces ?? []);
+    });
+    const u2 = subscribe('/social/gestures', 'saltybot_social_msgs/GestureArray', msg => {
+      setGestures(msg.gestures ?? []);
+    });
+    const u3 = subscribe('/social/scene/objects', 'saltybot_scene_msgs/SceneObjectArray', msg => {
+      setSceneObjects(msg.objects ?? []);
+    });
+    return () => { u1?.(); u2?.(); u3?.(); };
+  }, [subscribe]);
+
+  const activeCam  = CAMERA_BY_ID[activeId];
+  const activeFrame = frames[activeId];
+
+  // Filter overlay data based on mode
+  const visibleFaces   = (overlayMode === 'all' || overlayMode === 'faces')   ? faces        : [];
+  const visibleGestures = (overlayMode === 'all' || overlayMode === 'gestures') ? gestures    : [];
+  const visibleObjects  = (overlayMode === 'all' || overlayMode === 'objects')  ? sceneObjects : [];
+
+  // ── Container size tracking (for overlay canvas sizing) ────────────────────
+  const containerRef = useRef(null);
+  const [containerSize, setContainerSize] = useState({ w: 640, h: 480 });
+
+  useEffect(() => {
+    if (!containerRef.current) return;
+    const ro = new ResizeObserver(entries => {
+      const e = entries[0];
+      setContainerSize({ w: Math.round(e.contentRect.width), h: Math.round(e.contentRect.height) });
+    });
+    ro.observe(containerRef.current);
+    return () => ro.disconnect();
+  }, []);
+
+  // ── Quality badge ──────────────────────────────────────────────────────────
+  const camFps    = fps[activeId] ?? 0;
+  const quality   = camFps >= 13 ? 'FULL' : camFps >= 8 ? 'GOOD' : camFps > 0 ? 'LOW' : 'NO SIGNAL';
+  const qualColor = camFps >= 13 ? 'text-green-500' : camFps >= 8 ? 'text-amber-500' : camFps > 0 ? 'text-red-500' : 'text-gray-700';
+
+  return (
+    <div className="space-y-3">
+
+      {/* ── Camera strip ── */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 space-y-2">
+        <div className="flex items-center justify-between">
+          <div className="text-cyan-700 text-xs font-bold tracking-widest">CAMERA SELECT</div>
+          <span className={`text-xs font-bold ${qualColor}`}>{quality}  {camFps > 0 ? `${camFps}fps` : ''}</span>
+        </div>
+        <CameraStrip
+          cameras={cameras}
+          activeId={activeId}
+          pipList={pipList}
+          frames={frames}
+          fps={fps}
+          onSelect={setActiveId}
+          onTogglePip={togglePip}
+        />
+      </div>
+
+      {/* ── Main viewer ── */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 overflow-hidden">
+
+        {/* Viewer toolbar */}
+        <div className="flex items-center justify-between px-3 py-2 border-b border-cyan-950">
+          <div className="flex items-center gap-2">
+            <span className="text-cyan-400 text-xs font-bold">{activeCam?.label ?? '—'}</span>
+            {activeCam?.isDepth && (
+              <span className="text-xs text-gray-600 border border-gray-800 rounded px-1">DEPTH · greyscale</span>
+            )}
+            {activeCam?.isPanoramic && (
+              <span className="text-xs text-gray-600 border border-gray-800 rounded px-1">360° · drag to pan</span>
+            )}
+          </div>
+
+          {/* Overlay mode selector */}
+          <div className="flex items-center gap-1">
+            {['off','faces','gestures','objects','all'].map(mode => (
+              <button
+                key={mode}
+                onClick={() => setOverlayMode(mode)}
+                className={`px-2 py-0.5 rounded text-xs border transition-colors ${
+                  overlayMode === mode
+                    ? 'border-cyan-600 bg-cyan-950 text-cyan-400'
+                    : 'border-gray-800 text-gray-600 hover:border-gray-700 hover:text-gray-400'
+                }`}
+              >
+                {mode === 'all' ? 'ALL' : mode === 'off' ? 'OFF' : mode.slice(0,3).toUpperCase()}
+              </button>
+            ))}
+          </div>
+        </div>
+
+        {/* Image + overlay */}
+        <div className="relative" ref={containerRef}>
+          {activeCam?.isPanoramic ? (
+            <PanoViewer frameUrl={activeFrame} />
+          ) : activeFrame ? (
+            <img
+              src={activeFrame}
+              alt={activeCam?.label ?? 'camera'}
+              className="w-full object-contain block bg-black"
+              style={{ maxHeight: '480px' }}
+            />
+          ) : (
+            <div className="w-full bg-black flex items-center justify-center text-gray-800 text-sm font-mono"
+                 style={{ height: '360px' }}>
+              <div className="text-center space-y-2">
+                <div className="text-2xl">📷</div>
+                <div>Waiting for {activeCam?.label ?? '—'}…</div>
+                <div className="text-xs text-gray-700">{activeCam?.topic}</div>
+              </div>
+            </div>
+          )}
+
+          {/* Detection overlay canvas */}
+          {overlayMode !== 'off' && !activeCam?.isPanoramic && (
+            <OverlayCanvas
+              ref={overlayCanvasRef}
+              faces={visibleFaces}
+              gestures={visibleGestures}
+              sceneObjects={visibleObjects}
+              activeCam={activeCam}
+              containerW={containerSize.w}
+              containerH={containerSize.h}
+            />
+          )}
+
+          {/* PiP windows */}
+          {pipList.map((id, idx) => {
+            const cam = CAMERA_BY_ID[id];
+            if (!cam) return null;
+            return (
+              <PiPWindow
+                key={id}
+                cam={cam}
+                frameUrl={frames[id]}
+                fps={fps[id] ?? 0}
+                index={idx}
+                onClose={() => togglePip(id)}
+              />
+            );
+          })}
+        </div>
+      </div>
+
+      {/* ── Recording controls ── */}
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3">
+        <div className="flex items-center justify-between mb-2">
+          <div className="text-cyan-700 text-xs font-bold tracking-widest">CAPTURE</div>
+        </div>
+        <RecordingBar
+          recording={recording}
+          recSeconds={recSeconds}
+          onStart={startRecording}
+          onStop={stopRecording}
+          onSnapshot={takeSnapshot}
+          overlayRef={overlayCanvasRef}
+        />
+        <div className="mt-2 text-xs text-gray-700">
+          Recording saves as MP4/WebM to your Downloads.
+          Snapshot includes detection overlay + timestamp.
+        </div>
+      </div>
+
+      {/* ── Detection status ── */}
+      <div className="grid grid-cols-3 gap-2 text-xs">
+        <div className="bg-gray-950 rounded border border-gray-800 p-2">
+          <div className="text-gray-600 mb-1">FACES</div>
+          <div className={`font-bold ${faces.length > 0 ? 'text-cyan-400' : 'text-gray-700'}`}>
+            {faces.length > 0 ? `${faces.length} detected` : 'none'}
+          </div>
+          {faces.slice(0, 2).map((f, i) => (
+            <div key={i} className="text-gray-600 truncate">
+              {f.person_name && f.person_name !== 'unknown'
+                ? `↳ ${f.person_name}`
+                : `↳ unknown #${f.face_id}`}
+            </div>
+          ))}
+        </div>
+
+        <div className="bg-gray-950 rounded border border-gray-800 p-2">
+          <div className="text-gray-600 mb-1">GESTURES</div>
+          <div className={`font-bold ${gestures.length > 0 ? 'text-amber-400' : 'text-gray-700'}`}>
+            {gestures.length > 0 ? `${gestures.length} active` : 'none'}
+          </div>
+          {gestures.slice(0, 2).map((g, i) => {
+            const icon = GESTURE_ICONS[g.gesture_type] ?? '?';
+            return (
+              <div key={i} className="text-gray-600 truncate">
+                {icon} {g.gesture_type} cam{g.camera_id}
+              </div>
+            );
+          })}
+        </div>
+
+        <div className="bg-gray-950 rounded border border-gray-800 p-2">
+          <div className="text-gray-600 mb-1">OBJECTS</div>
+          <div className={`font-bold ${sceneObjects.length > 0 ? 'text-green-400' : 'text-gray-700'}`}>
+            {sceneObjects.length > 0 ? `${sceneObjects.length} objects` : 'none'}
+          </div>
+          {sceneObjects
+            .filter(o => o.hazard_type > 0)
+            .slice(0, 2)
+            .map((o, i) => (
+              <div key={i} className="text-amber-700 truncate">⚠ {o.class_name}</div>
+            ))
+          }
+          {sceneObjects.filter(o => o.hazard_type === 0).slice(0, 2).map((o, i) => (
+            <div key={`ok${i}`} className="text-gray-600 truncate">
+              {o.class_name} {o.distance_m > 0 ? `${o.distance_m.toFixed(1)}m` : ''}
+            </div>
+          ))}
+        </div>
+      </div>
+
+      {/* ── Legend ── */}
+      <div className="flex gap-4 text-xs text-gray-700 flex-wrap">
+        <div className="flex items-center gap-1">
+          <div className="w-3 h-3 rounded-sm border border-cyan-600" />
+          Known face
+        </div>
+        <div className="flex items-center gap-1">
+          <div className="w-3 h-3 rounded-sm border border-amber-600" />
+          Unknown face
+        </div>
+        <div className="flex items-center gap-1">
+          <span>👆</span> Gesture
+        </div>
+        <div className="flex items-center gap-1">
+          <div className="w-3 h-3 rounded-sm border border-green-700 border-dashed" />
+          Object
+        </div>
+        <div className="flex items-center gap-1">
+          <div className="w-3 h-3 rounded-sm border border-amber-600 border-dashed" />
+          Hazard
+        </div>
+        <div className="ml-auto text-gray-800 italic">
+          ⊕ pin = PiP  ·  overlay: {overlayMode}
+        </div>
+      </div>
+    </div>
+  );
+}

ui/social-bot/src/hooks/useCamera.js

@@ -0,0 +1,325 @@
+/**
+ * useCamera.js — Multi-camera stream manager (Issue #177).
+ *
+ * Subscribes to sensor_msgs/CompressedImage topics via rosbridge.
+ * Decodes base64 JPEG/PNG → data URL for <img>/<canvas> display.
+ * Tracks per-camera FPS. Manages MediaRecorder for recording + snapshots.
+ *
+ * Camera sources:
+ *   front / left / rear / right  — 4× CSI IMX219, 640×480
+ *                                  topic: /camera/<name>/image_raw/compressed
+ *   color                        — D435i RGB, 640×480
+ *                                  topic: /camera/color/image_raw/compressed
+ *   depth                        — D435i depth, 640×480 greyscale (PNG16)
+ *                                  topic: /camera/depth/image_rect_raw/compressed
+ *   panoramic                    — equirect stitch 1920×960
+ *                                  topic: /camera/panoramic/compressed
+ */
+
+import { useState, useEffect, useRef, useCallback } from 'react';
+
+// ── Camera catalogue ──────────────────────────────────────────────────────────
+
+export const CAMERAS = [
+  {
+    id: 'front',
+    label: 'Front',
+    shortLabel: 'F',
+    topic: '/camera/front/image_raw/compressed',
+    msgType: 'sensor_msgs/CompressedImage',
+    cameraId: 0,           // matches gesture_node camera_id
+    width: 640, height: 480,
+  },
+  {
+    id: 'left',
+    label: 'Left',
+    shortLabel: 'L',
+    topic: '/camera/left/image_raw/compressed',
+    msgType: 'sensor_msgs/CompressedImage',
+    cameraId: 1,
+    width: 640, height: 480,
+  },
+  {
+    id: 'rear',
+    label: 'Rear',
+    shortLabel: 'R',
+    topic: '/camera/rear/image_raw/compressed',
+    msgType: 'sensor_msgs/CompressedImage',
+    cameraId: 2,
+    width: 640, height: 480,
+  },
+  {
+    id: 'right',
+    label: 'Right',
+    shortLabel: 'Rt',
+    topic: '/camera/right/image_raw/compressed',
+    msgType: 'sensor_msgs/CompressedImage',
+    cameraId: 3,
+    width: 640, height: 480,
+  },
+  {
+    id: 'color',
+    label: 'D435i RGB',
+    shortLabel: 'D',
+    topic: '/camera/color/image_raw/compressed',
+    msgType: 'sensor_msgs/CompressedImage',
+    cameraId: 4,
+    width: 640, height: 480,
+  },
+  {
+    id: 'depth',
+    label: 'Depth',
+    shortLabel: '≋',
+    topic: '/camera/depth/image_rect_raw/compressed',
+    msgType: 'sensor_msgs/CompressedImage',
+    cameraId: 5,
+    width: 640, height: 480,
+    isDepth: true,
+  },
+  {
+    id: 'panoramic',
+    label: 'Panoramic',
+    shortLabel: '360',
+    topic: '/camera/panoramic/compressed',
+    msgType: 'sensor_msgs/CompressedImage',
+    cameraId: -1,
+    width: 1920, height: 960,
+    isPanoramic: true,
+  },
+];
+
+export const CAMERA_BY_ID = Object.fromEntries(CAMERAS.map(c => [c.id, c]));
+export const CAMERA_BY_ROS_ID = Object.fromEntries(
+  CAMERAS.filter(c => c.cameraId >= 0).map(c => [c.cameraId, c])
+);
+
+const TARGET_FPS     = 15;
+const FPS_INTERVAL   = 1000; // ms between FPS counter resets
+
+// ── Hook ──────────────────────────────────────────────────────────────────────
+
+export function useCamera({ subscribe } = {}) {
+  const [frames, setFrames]         = useState(() =>
+    Object.fromEntries(CAMERAS.map(c => [c.id, null]))
+  );
+  const [fps, setFps]               = useState(() =>
+    Object.fromEntries(CAMERAS.map(c => [c.id, 0]))
+  );
+  const [activeId, setActiveId]     = useState('front');
+  const [pipList, setPipList]       = useState([]);   // up to 3 extra camera ids
+  const [recording, setRecording]   = useState(false);
+  const [recSeconds, setRecSeconds] = useState(0);
+
+  // ── Refs (not state — no re-render needed) ─────────────────────────────────
+  const countRef      = useRef(Object.fromEntries(CAMERAS.map(c => [c.id, 0])));
+  const mediaRecRef   = useRef(null);
+  const chunksRef     = useRef([]);
+  const recTimerRef   = useRef(null);
+  const recordCanvas  = useRef(null); // hidden canvas used for recording
+  const recAnimRef    = useRef(null); // rAF handle for record-canvas loop
+  const latestFrameRef = useRef(Object.fromEntries(CAMERAS.map(c => [c.id, null])));
+  const latestTsRef   = useRef(Object.fromEntries(CAMERAS.map(c => [c.id, 0])));
+
+  // ── FPS counter ────────────────────────────────────────────────────────────
+  useEffect(() => {
+    const timer = setInterval(() => {
+      setFps({ ...countRef.current });
+      const reset = Object.fromEntries(CAMERAS.map(c => [c.id, 0]));
+      countRef.current = reset;
+    }, FPS_INTERVAL);
+    return () => clearInterval(timer);
+  }, []);
+
+  // ── Subscribe all camera topics ────────────────────────────────────────────
+  useEffect(() => {
+    if (!subscribe) return;
+
+    const unsubs = CAMERAS.map(cam => {
+      let lastTs = 0;
+      const interval = Math.floor(1000 / TARGET_FPS); // client-side 15fps gate
+
+      return subscribe(cam.topic, cam.msgType, (msg) => {
+        const now = Date.now();
+        if (now - lastTs < interval) return; // drop frames > 15fps
+        lastTs = now;
+
+        const fmt    = msg.format || 'jpeg';
+        const mime   = fmt.includes('png') || fmt.includes('16UC') ? 'image/png' : 'image/jpeg';
+        const dataUrl = `data:${mime};base64,${msg.data}`;
+
+        latestFrameRef.current[cam.id] = dataUrl;
+        latestTsRef.current[cam.id]    = now;
+        countRef.current[cam.id]       = (countRef.current[cam.id] ?? 0) + 1;
+
+        setFrames(prev => ({ ...prev, [cam.id]: dataUrl }));
+      });
+    });
+
+    return () => unsubs.forEach(fn => fn?.());
+  }, [subscribe]);
+
+  // ── Create hidden record canvas ────────────────────────────────────────────
+  useEffect(() => {
+    const c = document.createElement('canvas');
+    c.width  = 640;
+    c.height = 480;
+    c.style.display = 'none';
+    document.body.appendChild(c);
+    recordCanvas.current = c;
+    return () => { c.remove(); };
+  }, []);
+
+  // ── Draw loop for record canvas ────────────────────────────────────────────
+  // Runs at TARGET_FPS when recording — draws active frame to hidden canvas
+  const startRecordLoop = useCallback(() => {
+    const canvas = recordCanvas.current;
+    if (!canvas) return;
+
+    const step = () => {
+      const cam    = CAMERA_BY_ID[activeId];
+      const src    = latestFrameRef.current[activeId];
+      const ctx    = canvas.getContext('2d');
+
+      if (!cam || !src) {
+        recAnimRef.current = requestAnimationFrame(step);
+        return;
+      }
+
+      // Resize canvas to match source
+      if (canvas.width !== cam.width || canvas.height !== cam.height) {
+        canvas.width  = cam.width;
+        canvas.height = cam.height;
+      }
+
+      const img = new Image();
+      img.onload = () => {
+        ctx.drawImage(img, 0, 0, canvas.width, canvas.height);
+      };
+      img.src = src;
+
+      recAnimRef.current = setTimeout(step, Math.floor(1000 / TARGET_FPS));
+    };
+
+    recAnimRef.current = setTimeout(step, 0);
+  }, [activeId]);
+
+  const stopRecordLoop = useCallback(() => {
+    if (recAnimRef.current) {
+      clearTimeout(recAnimRef.current);
+      cancelAnimationFrame(recAnimRef.current);
+      recAnimRef.current = null;
+    }
+  }, []);
+
+  // ── Recording ──────────────────────────────────────────────────────────────
+
+  const startRecording = useCallback(() => {
+    const canvas = recordCanvas.current;
+    if (!canvas || recording) return;
+
+    startRecordLoop();
+
+    const stream   = canvas.captureStream(TARGET_FPS);
+    const mimeType =
+      MediaRecorder.isTypeSupported('video/mp4')                ? 'video/mp4'            :
+      MediaRecorder.isTypeSupported('video/webm;codecs=vp9')    ? 'video/webm;codecs=vp9' :
+      MediaRecorder.isTypeSupported('video/webm;codecs=vp8')    ? 'video/webm;codecs=vp8' :
+                                                                   'video/webm';
+
+    chunksRef.current = [];
+    const mr = new MediaRecorder(stream, { mimeType, videoBitsPerSecond: 2_500_000 });
+    mr.ondataavailable = e => { if (e.data?.size > 0) chunksRef.current.push(e.data); };
+    mr.start(200);
+    mediaRecRef.current = mr;
+    setRecording(true);
+    setRecSeconds(0);
+    recTimerRef.current = setInterval(() => setRecSeconds(s => s + 1), 1000);
+  }, [recording, startRecordLoop]);
+
+  const stopRecording = useCallback(() => {
+    const mr = mediaRecRef.current;
+    if (!mr || mr.state === 'inactive') return;
+
+    mr.onstop = () => {
+      const ext  = mr.mimeType.includes('mp4') ? 'mp4' : 'webm';
+      const blob = new Blob(chunksRef.current, { type: mr.mimeType });
+      const url  = URL.createObjectURL(blob);
+      const a    = document.createElement('a');
+      a.href     = url;
+      a.download = `saltybot-${activeId}-${Date.now()}.${ext}`;
+      a.click();
+      URL.revokeObjectURL(url);
+    };
+
+    mr.stop();
+    stopRecordLoop();
+    clearInterval(recTimerRef.current);
+    setRecording(false);
+  }, [activeId, stopRecordLoop]);
+
+  // ── Snapshot ───────────────────────────────────────────────────────────────
+
+  const takeSnapshot = useCallback((overlayCanvasEl) => {
+    const src = latestFrameRef.current[activeId];
+    if (!src) return;
+
+    const cam    = CAMERA_BY_ID[activeId];
+    const canvas = document.createElement('canvas');
+    canvas.width  = cam.width;
+    canvas.height = cam.height;
+    const ctx = canvas.getContext('2d');
+
+    const img = new Image();
+    img.onload = () => {
+      ctx.drawImage(img, 0, 0, canvas.width, canvas.height);
+
+      // Composite detection overlay if provided
+      if (overlayCanvasEl) {
+        ctx.drawImage(overlayCanvasEl, 0, 0, canvas.width, canvas.height);
+      }
+
+      // Timestamp watermark
+      ctx.fillStyle = 'rgba(0,0,0,0.5)';
+      ctx.fillRect(0, canvas.height - 20, canvas.width, 20);
+      ctx.fillStyle = '#06b6d4';
+      ctx.font = '11px monospace';
+      ctx.fillText(`SALTYBOT  ${cam.label}  ${new Date().toISOString()}`, 8, canvas.height - 6);
+
+      canvas.toBlob(blob => {
+        const url = URL.createObjectURL(blob);
+        const a   = document.createElement('a');
+        a.href    = url;
+        a.download = `saltybot-snap-${activeId}-${Date.now()}.png`;
+        a.click();
+        URL.revokeObjectURL(url);
+      }, 'image/png');
+    };
+    img.src = src;
+  }, [activeId]);
+
+  // ── PiP management ─────────────────────────────────────────────────────────
+
+  const togglePip = useCallback(id => {
+    setPipList(prev => {
+      if (prev.includes(id)) return prev.filter(x => x !== id);
+      const next = [...prev, id].filter(x => x !== activeId);
+      return next.slice(-3); // max 3 PIPs
+    });
+  }, [activeId]);
+
+  // Remove PiP if it becomes the active camera
+  useEffect(() => {
+    setPipList(prev => prev.filter(id => id !== activeId));
+  }, [activeId]);
+
+  return {
+    cameras:      CAMERAS,
+    frames,
+    fps,
+    activeId,     setActiveId,
+    pipList,      togglePip,
+    recording,    recSeconds,
+    startRecording, stopRecording,
+    takeSnapshot,
+  };
+}