# Next Session: Lidar-Reactive Navigation (V2)

## What

Wire RPLIDAR C1 into the robot car's safety daemon and navigation loop. Replace the unreliable bbox-based distance estimation with real 360-degree lidar distances. Add sector summary to the VLM navigation prompt so Annie can make informed directional choices. **No SLAM** — lidar is used reactively, not for mapping. SLAM is deferred until IMU/encoders are added.

## Key Design Decisions (from adversarial review — 18 findings, all addressed)

1. **No SLAM** — Without IMU or wheel encoders, all SLAM approaches are fragile. Lidar-reactive navigation (sector summary in VLM prompt) delivers 80% of value with 20% of complexity.
2. **No dead reckoning for SLAM** — Mecanum wheel slip on carpet makes motor-based odometry actively worse than no odometry. If SLAM is ever added, use zero odometry (identity transform).
3. **udev rule for stable device names** — `/dev/lidar` symlink via VID/PID (CP2102N: 10c4:ea60). Never hardcode `/dev/ttyUSB1`.
4. **pyrplidar monkey-patch** — Apply byte-order fix at import time (blackwell_patch.py pattern). Pin pyrplidar==0.1.2. Regression test with known raw bytes.
5. **Double-buffer scan protocol** — Accumulate full rotation locally, atomic swap under `_scan_lock`. Consumers always see coherent single-rotation data. Epoch counter for staleness.
6. **Lidar-camera coordinate calibration** — One-time: place object at camera center, read lidar angle, compute `LIDAR_FORWARD_OFFSET_DEG`. Store as constant.
7. **Filter distance==0** — pyrplidar returns 0 for no-return. Use 5th percentile per sector, not min. Require 3+ valid points per sector.
8. **Stall detection** — 10 consecutive empty scans → reconnect. `lidar_healthy` flag in `/health`.
9. **Safety daemon polls `lidar.is_healthy()`** — No asyncio events across threads. Thread-safe read of atomic state.
10. **IMU NOT AVAILABLE** — Verified Session 43. SDK has `board.get_imu()` but STM32 never sends data. Don't waste time trying to enable it.
11. **ESTOP callback guard** — Add `if _board:` guard in `_safety_estop_callback()` for shutdown race.
12. **`/scan` raw_points default off** — Query param `?raw=true` to include. Navigation loop NEVER requests raw points.
13. **Spin-up validation** — SPINNING_UP waits 2s AND validates >10 valid points before transitioning to SCANNING.
14. **CPU budget** — Phase 1 total ~30-43% (comfortable on 4 cores). Threshold: abort if >70%.

## Car Dimensions (verified Session 44)

| Dimension | Value |
|-----------|-------|
| Width | **20cm** |
| Length (front to back) | **25cm** |
| Height | **~25cm** |
| Power | USB-C battery (get_battery() returns None) |
| Min navigable gap | **~25cm** (width + 5cm margin) |

## Hardware Verified (Session 42-43)

| Sensor | Status | Key Specs |
|--------|--------|-----------|
| RPLIDAR C1 | **VERIFIED** | DenseBoost: 10.24m range, ~2000 pts/s, `/dev/lidar` (CP2102N, 460800 baud) |
| Hailo-8 + YOLOv8s | **DEPLOYED** | 30 FPS, auto-ESTOP <30cm |
| IMU | **NOT AVAILABLE** | SDK API exists, STM32 never sends data |
| Wheel encoders | **NOT PRESENT** | TurboPi variant lacks encoders |
| Lidar directions | **VERIFIED** | Left/Right matches physical reality (Session 44) |

## Architecture

```
Pi 5:
  FrameGrabber (camera) ──┐
  LidarDaemon (RPLIDAR)  ─┤── FastAPI :8080 ──── HTTP ──── Annie (Titan)
  HailoSafetyDaemon (YOLO)┘                                  │
       │                                                      ▼
       │ get_sector_min(bearing)                    Gemma 4 26B vLLM
       └─────► fuse lidar distance                  image + sectors + obstacles → action
               into YOLO detections
```

Annie's VLM prompt gets sector summary: `"Forward: clear 3.2m, Right: blocked 0.4m, Rear: wall 1.1m, Left: open 2.8m"`

## Files to Modify

### Phase 0: Pi Setup
1. `/etc/udev/rules.d/99-turbopi.rules` — **NEW**: `SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", SYMLINK+="lidar"`

### Phase 1: LidarDaemon (`services/turbopi-server/`)
2. `lidar.py` — **NEW**: LidarDaemon thread (pyrplidar monkey-patch, double-buffer, stall detection, udev-aware, coordinate offset)
3. `main.py` — **MODIFY**: Wire LidarDaemon into lifespan; add `/scan` endpoint (sector summary, no raw by default); add `lidar_healthy` to `/health`
4. `test_lidar.py` — **NEW**: Unit tests (double-buffer, stall detection, monkey-patch regression, coordinate offset, distance==0 filtering)

### Phase 2: Safety Fusion (`services/turbopi-server/`)
5. `safety.py` — **MODIFY**: Query `lidar.get_sector_min(bearing_deg)` for each YOLO detection; replace bbox distance with lidar distance when available; fallback to bbox when lidar unhealthy
6. `main.py` — **MODIFY**: Add `if _board:` guard in `_safety_estop_callback()`
7. `test_safety.py` — **MODIFY**: Tests for lidar-fused distance, fallback to bbox

### Phase 3: Nav Brain Upgrade (`services/annie-voice/`)
8. `robot_tools.py` — **MODIFY**: Fetch `/scan` in navigate loop (parallel with photo+obstacles); format sector summary for LLM prompt; fix HTTP client lifecycle (lifespan-managed)
9. `tests/test_robot_tools.py` — **MODIFY**: Tests for lidar-enhanced navigation prompt

## Known Gotchas (MUST READ)

| Gotcha | Fix |
|--------|-----|
| pyrplidar DenseBoost byte-order bug | Monkey-patch at import time. Pin `pyrplidar==0.1.2`. Regression test. |
| `/dev/ttyUSB1` can swap with `/dev/ttyUSB0` after reboot | udev rule → `/dev/lidar` symlink. NEVER hardcode ttyUSB1. |
| Lidar motor needs 2s spin-up | SPINNING_UP state validates >10 valid points before SCANNING. |
| `asyncio.Event.set()` not thread-safe from daemon threads | LidarDaemon uses Lock-protected buffer + `is_healthy()` polling. No asyncio events. |
| Gemma 4 thinking mode eats tokens | `"chat_template_kwargs": {"thinking": False}` already in nav calls. |
| Distance==0 is pyrplidar no-return marker | Filter before aggregation. Use 5th percentile, not min. |
| Lidar bearing ≠ camera bearing | One-time calibration: measure offset, store as `LIDAR_FORWARD_OFFSET_DEG`. |
| ESTOP callback can race during shutdown | Add `if _board:` guard. |
| Dead reckoning is WORSE than no odometry for mecanum | If SLAM is ever added, use identity transform, not motor dead reckoning. |

## SLAM Research Summary (Deferred)

| Library | Verdict | Why |
|---------|---------|-----|
| BreezySLAM | DEFERRED | No odom = drift; Py3.13 C API breakage risk |
| HectorSLAM | DEFERRED | Best algorithm but ROS-only; porting = 2-4 sessions |
| GMapping | REJECTED | Requires wheel encoder odometry (we don't have it) |
| Cartographer | REJECTED | ROS2 + heavy deps, overkill for apartment |
| Custom ICP | FUTURE OPTION | 80 LOC numpy, viable if SLAM needed later |
| **No SLAM** | **CHOSEN** | 80% value, 20% complexity. VLM + lidar sectors = good enough. |

**Revisit SLAM when:** External USB IMU added (~$15) OR wheel encoders added OR persistent mapping genuinely needed.

## Pre-Mortem (Top 5 Risks)

| Risk | Mitigation |
|------|------------|
| pyrplidar patch lost on pip upgrade → garbage distances, robot drives into wall | Monkey-patch at import; pin version; regression test |
| Lidar bearing misaligned with camera → safety fuses wrong distance | One-time calibration procedure |
| Mixed-scan after reconnect → phantom obstacles | Double-buffer with epoch counter; invalidate on reconnect |
| Pi 5 throttle under load → safety daemon slows | CPU budget 30-43%; monitor throttle; abort if >70% |
| Stalled lidar motor → silent data loss | 10-empty-scan detector → reconnect; lidar_healthy in /health |

## Start Command

Read this file, then implement Phases 0→1→2→3→4 in order.

## Verification

1. Reboot Pi → verify `/dev/lidar` exists (udev rule)
2. Run pyrplidar regression test → assert byte order correct
3. Calibrate lidar-camera offset → store constant in `lidar.py`
4. `curl http://pi-car:8080/scan` → verify sector data, `scan_age_ms < 3000`
5. Place obstacle at 25cm → verify ESTOP with `distance_source: "lidar"`
6. Telegram → "Annie, what's around you?" → verify lidar distances in response
7. Telegram → "Annie, explore the room" → multi-cycle lidar-aware navigation
8. Unplug lidar USB → verify `/health` shows `lidar_healthy: false` within 5s
9. 10-minute stress test → verify `vcgencmd get_throttled` stays 0x0