# Next Session: HectorSLAM Implementation

## What
Implement HectorSLAM (numpy port) for the TurboPi robot car. IMU hardware is DONE (Phase 0). This session implements the SLAM core, server endpoints, and Annie navigation integration. The plan has been adversarially reviewed with 12 findings — all addressed.

## Plan
Read the approved plan at `~/.claude/plans/swirling-wondering-trinket.md`.
**Read the ENTIRE plan first** — it has the full implementation code, all 12 adversarial review findings, and their fixes at the bottom. The fixes are MANDATORY — they correct critical coordinate system bugs, lock safety issues, and performance problems found by two independent hostile reviewers.

## Key Design Decisions (from adversarial review)

1. **Coordinate system: `(row, col)` not `(gx, gy)`** — grid[row, col] where row=world_y, col=world_x. All code uses this convention. Heading arrow uses `cos→row`, `sin→col`. (Fix 1, CRITICAL)
2. **Grid snapshot COPY inside lock** — `to_probability()` on live array has torn reads on ARM. Always `grid.copy()` inside `_slam_lock`, render outside. (Fix 2, CRITICAL)
3. **Subsample to 500 points** — `MAX_SLAM_POINTS=500` before scan matching. Python for-loop over 4000+ points blows 100ms budget. (Fix 3, HIGH)
4. **`consume_heading_delta_deg()`** — destructive read renamed from `get_` to make mutation explicit. Only SlamDaemon calls it. (Fix 7, HIGH)
5. **No `_require_no_demo()` on `/map` and `/pose`** — read-only endpoints, safe during demos. (Fix 8, HIGH)
6. **`POST /slam/reset`** — map corruption recovery without service restart. (Fix 6, HIGH)
7. **`_call_robot_bytes()`** — new helper for binary `/map` PNG response. `_call_robot()` always calls `.json()`. (Fix 5, HIGH)
8. **`global _imu_reader, _slam_daemon`** — must be in BOTH `_start_daemons` and `_teardown_daemons`. (Fix 4, HIGH)
9. **Lidar CW/CCW verification** — `LIDAR_CCW=True` constant, verify empirically on Pi. (Fix 10, MEDIUM)
10. **`MIN_TRAVEL_M=0.10, MIN_TRAVEL_RAD=0.5`** — skip SLAM update if robot hasn't moved enough (from HiWonder's slam_toolbox config). (HiWonder research)
11. **udev PID is `0005`** (MicroPython), NOT `0005`. (Gotcha G1)

## Files to Modify (in order)

1. `services/turbopi-server/99-turbopi.rules` — add `/dev/imu` udev rule
2. `services/turbopi-server/imu.py` — CREATE: ImuReader serial thread
3. `services/turbopi-server/test_imu.py` — CREATE: IMU tests
4. `services/turbopi-server/lidar.py` — MODIFY: add `_raw_points` buffer + `get_raw_points()`
5. `services/turbopi-server/test_lidar.py` — MODIFY: add raw points tests
6. `services/turbopi-server/slam.py` — CREATE: OccupancyGrid, MultiResGrid, scan_match, SlamDaemon
7. `services/turbopi-server/test_slam.py` — CREATE: SLAM core tests
8. `services/turbopi-server/main.py` — MODIFY: wire daemons, add /map, /pose, /slam/reset
9. `services/turbopi-server/test_slam_endpoints.py` — CREATE: endpoint tests
10. `services/annie-voice/robot_tools.py` — MODIFY: add map/pose to nav loop
11. `services/annie-voice/tests/test_robot_tools.py` — MODIFY: map-aware nav tests

## Start Command

```
cat ~/.claude/plans/swirling-wondering-trinket.md
```

Then implement the plan task by task (Tasks 1-9). All adversarial findings are already addressed in the "Adversarial Review Findings & Mandatory Fixes" section at the bottom of the plan.

## Verification

1. `cd services/turbopi-server && python -m pytest -v` — all new + existing tests pass
2. `cd services/annie-voice && python -m pytest tests/test_robot_tools.py -v` — Annie tests pass
3. SSH to Pi: `/health` shows `slam_healthy: true`, `imu_healthy: true`
4. `curl /pose` returns valid JSON, `curl /map > map.png` returns valid PNG
5. "Annie, explore this room" — map builds, pose updates
6. `top` shows < 70% CPU on Pi 5
7. ESTOP still works during SLAM
8. Unplug Pico → SLAM transitions to DEGRADED gracefully

## HiWonder Research (reference only)

No reusable SLAM code — they use ROS2 `slam_toolbox`. Key repos for reference:
- `Hiwonder/MentorPi` (branch `MentorPi-M1`) — slam_toolbox + Nav2 config
- `wltjr/turbopi_ros` — TurboPi + RPLidar on ROS2 Jazzy
- `Hiwonder/PuppyPi` — includes hector SLAM option via ROS2
- Config values adopted: `resolution: 0.05`, `min_travel: 0.10m / 0.5rad`