# Next Session: ArUco Visual Homing

## What

Build a closed-loop visual homing system for the TurboPi robot. The robot searches for an ArUco marker (DICT_6X6_50, id=23), drives toward it, stops within 30 cm, then turns 180° so the marker is at its back ("ready to leave home" pose). On arrival, `/slam/reset` re-anchors the SLAM frame to (0,0,0°), making homing a SLAM-drift correction mechanism.

**Why it matters:** Session 69 proved SLAM's home pose drifts 7.9× after rotation-coast-corrupted frame anchors. ArUco is an absolute external reference that bypasses SLAM for homing.

## Plan

**Full plan (with architecture diagrams, adversarial review, state machine, pre-mortem, ADRs):**
```
cat /home/rajesh/.claude/plans/whimsical-sparking-squid.md
```

Read the plan first — it has the full implementation spec, all 24 adversarial review findings addressed, 4 Architecture Decision Records, and 26-row pre-mortem failure analysis.

## Key Design Decisions (from adversarial review)

1. **Bypass `/drive` HTTP layer entirely** — homing.py calls `_drive_sync` directly under `_motor_lock` (same pattern as safety daemon). Avoids the 30/min rate limiter that would brick the homing loop, AND the side-clearance gate that would block centering turns near the table edge.

2. **`_yaw_to_action`: "left" when yaw_error > 0** — positive yaw_error means marker is RIGHT of camera; to center it, rotate LEFT (CCW). The original draft had the sign inverted — would have caused every mission to spin the wrong way and timeout.

3. **Object-point Y-axis convention: +Y down** — OpenCV ArUco marker frame uses +Y down (image plane). Draft had +Y up, which flips the rvec by 180° around X. Correct object points: `top-left=(-s/2, -s/2, 0)`.

4. **Cancel via `asyncio.Event`, not state mutation** — `/home/cancel` sets `_cancel_event` (lock-free). Loop polls at top of every iteration AND before each motor command. Avoids mid-transition race.

5. **`DUTY_TO_MPS > 0` validation at start** — division by zero crash in approach formula if odometry uncalibrated (returns 503 with clear message).

6. **Marker size is a hard Phase 0 gate** — must be physically measured and written to `~/aruco-homing/marker-spec.md` BEFORE Phase 2 deploys. No circular 80 mm assumption.

7. **`/imu` endpoint (already deployed this session)** — exposes raw Pico IMU heading (0-360) for closed-loop rotation deltas. Empirical calibration (session 70): IMU CW = negative heading delta; min controllable rotation step ≈ 25° at speed=40.

8. **Home pose = facing AWAY from marker** — after ARRIVED, robot executes a 180° turn so the marker is at its back. This is the "ready to leave" pose. Added an ALIGNING state between ARRIVED and IDLE.

## Critical Calibration Data (from this session)

| Measurement | Value | Source |
|---|---|---|
| Marker dict | DICT_6X6_50 | Live detection sweep, 11 dicts |
| Marker id | 23 | Consistent across center/down tilt |
| Marker visibility tilt range | PWM 1200–1500 (down to center) | Tilt sweep, 3 captures |
| Marker NOT visible | PWM 1800 (up) | Tilt sweep |
| IMU heading convention | CW = negative delta | Empirical: right turn 102.63° → 77.73° |
| 0.2s right pulse at speed=40 | ~25° actual rotation | IMU delta, single sample |
| 2.0s right turn at speed=40 | ~225° physical (user) / 165° SLAM (drift) | Session 70 rotation test |
| Rotation coast ratio | ~1.53–1.58× (commanded vs observed) | Session 69-70 consensus |
| Min controllable rotation step | ~25° at speed=40 | IMU measurement, 0.2s pulse |
| `/imu` endpoint | DEPLOYED (commit d8ebe6f) | Verified live |

## Files to Modify (ordered by phase)

**Phase 0 (no code, 5 min):**
- NEW `~/aruco-homing/marker-spec.md` — measure marker with ruler, write spec

**Phase 1 (40 min):**
- NEW `scripts/calibrate_camera_intrinsics.py` — runs from laptop, reads `ROBOT_API_TOKEN` from env (NO fallback), captures frames via `/photo`, saves `camera_intrinsics.json`
- MODIFY `services/turbopi-server/main.py` — add `GET /camera/v4l2_state` (~15 lines)
- NEW `services/turbopi-server/camera_intrinsics.json` — committed, <1 KB

**Phase 2 (45 min):**
- NEW `services/turbopi-server/aruco_detect.py` — pure module, `detect_markers()` function. Uses `cv2.solvePnP` with `SOLVEPNP_IPPE_SQUARE` and correct Y-down object points
- MODIFY `services/turbopi-server/main.py` — add `GET /aruco/detect` + intrinsics caching in `lifespan()`

**Phase 3 (75 min, longest):**
- NEW `services/turbopi-server/homing.py` — state machine (IDLE → SEARCHING → CENTERING → APPROACHING → ARRIVED → ALIGNING → IDLE), `_drive_internal()` bypasses HTTP, `_cancel_event` for lock-free cancellation, `homing.init()` for safe lock binding
- MODIFY `services/turbopi-server/main.py` — add `/home/start`, `/home/status`, `/home/cancel`, call `homing.init(...)` in `lifespan()`, extend `/health` with `homing_state`

**Phase 4 (30 min):**
- MODIFY `services/annie-voice/tool_schemas.py` — add `GoHomeInput`
- MODIFY `services/annie-voice/robot_tools.py` — add `handle_go_home` using `_call_robot()` (NOT inline httpx)
- MODIFY `services/annie-voice/text_llm.py` — add ToolSpec to `TOOL_SPECS`

**Tests throughout:**
- NEW `services/turbopi-server/tests/test_aruco_detect.py` — 10 tests incl. sign-convention + Y-axis regression
- NEW `services/turbopi-server/tests/test_homing.py` — 16 tests incl. rate-limit-bypass, side-clearance-bypass, yaw-sign, median-filter, duration-clamp regression
- MODIFY `services/annie-voice/tests/test_robot_tools.py` — 5 `go_home` tests

## Start Command

```bash
# Read the full plan
cat /home/rajesh/.claude/plans/whimsical-sparking-squid.md

# Then implement Phase 0-4 in order. All adversarial findings are already addressed.
```

## Verification

1. **Phase 0:** `~/aruco-homing/marker-spec.md` has measured size (mm)
2. **Phase 1:** `camera_intrinsics.json` with `reprojection_error_px < 0.5`, `frame_count >= 15`
3. **Phase 2:** `curl /aruco/detect` returns `{"detected":true, "markers":[{"id":23,...}]}`; distance within 10% at 0.5/1.0/2.0 m; yaw sign verified (marker right of camera → positive)
4. **Phase 3:** 3 starting poses, all 3 succeed within 180 s; cancel test within 1 s; ALIGNING 180° turn ends with marker behind
5. **Phase 4:** Telegram "Annie, go home" → robot homes → `handle_robot_status` shows ARRIVED
6. **All tests:** `pytest services/turbopi-server/tests/test_aruco_detect.py test_homing.py` + `pytest services/annie-voice/tests/test_robot_tools.py::test_go_home_*`

## Git State at Session 70 End

- `main` at `d8ebe6f` (feat: /imu endpoint deployed)
- Pi deployed via git pull; turbopi-server restarted
- Plan at `/home/rajesh/.claude/plans/whimsical-sparking-squid.md`
- Robot physically oriented approximately 180° from marker (marker behind, ±15° — IMU-measured)