Compare commits
No commits in common. "728d1b0c0eca1b692a7d1a193ced6f98de9550a3" and "50971c0946a30853e1ba9214f908b59191311a32" have entirely different histories.
728d1b0c0e
...
50971c0946
@ -1,165 +0,0 @@
|
||||
# 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
|
||||
```
|
||||
@ -1,462 +0,0 @@
|
||||
// ============================================================
|
||||
// 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.
|
||||
// ============================================================
|
||||
@ -1,429 +0,0 @@
|
||||
// ============================================================
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
@ -40,11 +40,6 @@ 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",
|
||||
|
||||
@ -94,33 +94,4 @@ def generate_launch_description():
|
||||
for name in _CAMERAS
|
||||
]
|
||||
|
||||
# ── 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])
|
||||
return LaunchDescription([rosbridge] + republishers)
|
||||
|
||||
@ -1,16 +0,0 @@
|
||||
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()
|
||||
@ -1,12 +0,0 @@
|
||||
# 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
|
||||
@ -1,21 +0,0 @@
|
||||
# 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
|
||||
@ -1,23 +0,0 @@
|
||||
<?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>
|
||||
@ -1,52 +0,0 @@
|
||||
# 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.
|
||||
@ -1,75 +0,0 @@
|
||||
"""
|
||||
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])
|
||||
@ -1,29 +0,0 @@
|
||||
<?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>
|
||||
@ -1,178 +0,0 @@
|
||||
"""
|
||||
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()
|
||||
@ -1,319 +0,0 @@
|
||||
"""
|
||||
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()
|
||||
@ -1,132 +0,0 @@
|
||||
"""
|
||||
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()
|
||||
@ -1,168 +0,0 @@
|
||||
"""
|
||||
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
|
||||
@ -1,206 +0,0 @@
|
||||
"""
|
||||
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
|
||||
@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_dynamic_obstacles
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_dynamic_obstacles
|
||||
@ -1,32 +0,0 @@
|
||||
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',
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,262 +0,0 @@
|
||||
"""
|
||||
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'])
|
||||
@ -1,43 +0,0 @@
|
||||
/**:
|
||||
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
|
||||
@ -1,53 +0,0 @@
|
||||
"""
|
||||
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"),
|
||||
},
|
||||
],
|
||||
),
|
||||
])
|
||||
@ -1,24 +0,0 @@
|
||||
<?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>
|
||||
@ -1,139 +0,0 @@
|
||||
"""
|
||||
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}"
|
||||
@ -1,232 +0,0 @@
|
||||
"""
|
||||
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
|
||||
@ -1,383 +0,0 @@
|
||||
"""
|
||||
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()
|
||||
@ -1,193 +0,0 @@
|
||||
"""
|
||||
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
|
||||
@ -1,354 +0,0 @@
|
||||
"""
|
||||
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)
|
||||
@ -1,5 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_emergency
|
||||
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_emergency
|
||||
@ -1,32 +0,0 @@
|
||||
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",
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,560 +0,0 @@
|
||||
"""
|
||||
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
|
||||
@ -1,15 +0,0 @@
|
||||
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()
|
||||
@ -1,25 +0,0 @@
|
||||
# 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
|
||||
@ -1,15 +0,0 @@
|
||||
# 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
|
||||
@ -1,22 +0,0 @@
|
||||
<?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>
|
||||
@ -5,16 +5,13 @@
|
||||
* Status | Faces | Conversation | Personality | Navigation
|
||||
*
|
||||
* Telemetry tabs (issue #126):
|
||||
* IMU | Battery | Motors | Map | Control | Health | Cameras
|
||||
* IMU | Battery | Motors | Map | Control | Health
|
||||
*
|
||||
* 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';
|
||||
@ -44,9 +41,6 @@ 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',
|
||||
@ -69,7 +63,6 @@ const TAB_GROUPS = [
|
||||
{ id: 'map', label: 'Map', },
|
||||
{ id: 'control', label: 'Control', },
|
||||
{ id: 'health', label: 'Health', },
|
||||
{ id: 'cameras', label: 'Cameras', },
|
||||
],
|
||||
},
|
||||
{
|
||||
@ -213,7 +206,6 @@ 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 />}
|
||||
|
||||
@ -1,671 +0,0 @@
|
||||
/**
|
||||
* 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>
|
||||
);
|
||||
}
|
||||
@ -1,325 +0,0 @@
|
||||
/**
|
||||
* 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,
|
||||
};
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user