# Next Session: Sonar + Visual Return E2E — Live Car Testing

## What Was Done (Session 46)

Full implementation of sonar integration + visual return-to-start. 6 files, +730 lines, 144 tests pass. Deployed to Pi + Titan. Sonar reading 1132mm on live hardware. Commit `700883d`.

**Hardware verified:**
- I2C enabled, sonar at 0x77 confirmed via `i2cdetect`
- `sonar_healthy: true` in `/health`
- `sonar_cm: 113.2` in `/obstacles`
- All 3 safety layers healthy: YOLO + lidar + sonar

## Architecture Quick Reference

```
┌──────────────────────────────────────────────────────────────┐
│                    TITAN (192.168.68.52)                      │
│  Annie Voice → robot_tools.py → VLM (port 8003, Gemma 4)    │
│  Sense → Think (VLM) → Act loop, max 10 cycles @ ~3.5s each │
│  Visual return: 2-image VLM matching (target vs current)     │
└──────────────────┬───────────────────────────────────────────┘
                   │ HTTP (port 8080, bearer token)
┌──────────────────▼───────────────────────────────────────────┐
│                   PI 5 (192.168.68.61)                        │
│  turbopi-server (systemd unit)                                │
│                                                               │
│  Layer 1: SonarPoller        10 Hz, 20-5000mm, 500ms stale   │
│           ↓ ESTOP if < 30cm (SAFETY_DISTANCE_CM)              │
│  Layer 2: HailoSafetyDaemon  30 FPS YOLOv8s on Hailo-8 NPU  │
│           ↓ ESTOP if object < 30cm (bbox + lidar distance)    │
│  Layer 3: LidarDaemon        RPLIDAR C1 360°, 12 sectors     │
│           ↓ blocked < 15cm, near < 25cm (tuned for 20×25cm)  │
│                                                               │
│  /health → sonar_healthy, lidar_healthy, safety_daemon_healthy│
│  /obstacles → sonar_cm, safe_forward, YOLO detections        │
│  /drive → motor commands, ESTOP clears on action=stop         │
│  /photo → camera snapshot (base64)                            │
│  /scan → lidar sector summary                                │
└──────────────────────────────────────────────────────────────┘
```

## Exact Thresholds (from code)

| Constant | Value | File | Purpose |
|----------|-------|------|---------|
| `SAFETY_DISTANCE_CM` | 30.0 | safety.py:42 | ESTOP trigger distance (YOLO + sonar) |
| `SONAR_MIN_MM` | 20 | safety.py:45 | Min valid range (below = sensor noise) |
| `SONAR_MAX_MM` | 5000 | safety.py:46 | Max valid range (above = garbage/99999) |
| `SONAR_STALE_MS` | 500 | safety.py:47 | Readings older than this → `None` |
| `SONAR_POLL_HZ` | 10 | safety.py:48 | Poll rate (100ms between reads) |
| `BLOCKED_THRESHOLD_MM` | 150 | lidar.py:106 | Lidar: < 15cm = blocked |
| `NEAR_THRESHOLD_MM` | 250 | lidar.py:107 | Lidar: 15-25cm = near |
| `MIN_CONFIDENCE` | 0.3 | safety.py:41 | YOLO detection confidence floor |
| `MAX_RETURN_ATTEMPTS` | 3 | robot_tools.py:42 | VLM retry per waypoint on return |
| `max_cycles` | 10 | tool_schemas.py:410 | Max nav cycles (capped) |
| `STALE_FRAME_LIMIT` | 30 | safety.py | ~1s at 30 FPS → camera stale ESTOP |

## Safety Precautions

**Before powering on:**
- Clear the test area of fragile objects, pets, and cables
- Ensure battery USB-C is accessible for quick disconnect (physical kill switch)
- Keep laptop near car — you are the final safety layer

**During tests:**
- Never reach into the car's path while motors are active
- If car behaves unexpectedly: **unplug USB-C battery** immediately
- ESTOP via software: `curl -X POST -H "Authorization: Bearer $TOKEN" http://192.168.68.61:8080/estop`
- Car speed is capped at 50 (tool schema enforces 0-50 range)
- Each movement command is clamped to max 5s duration (`MAX_DURATION`)
- Dead-man's switch: 10s with no command → auto-stop

## What To Do

This is a **hands-on E2E testing session**. You drive the car, monitor logs, fix any issues live. Rajesh will have battery connected and car in an open area.

### Pre-Flight Checks

1. Verify all systems healthy:
```bash
ssh rajesh@192.168.68.61 "curl -s http://localhost:8080/health | python3 -m json.tool"
```
Expected: `sonar_healthy: true`, `safety_daemon_healthy: true`, `lidar_healthy: true`, real `distance_mm` (not 99999).

2. Verify sonar responds to proximity — ask Rajesh to place hand 20cm in front:
```bash
ssh rajesh@192.168.68.61 "curl -s -H 'Authorization: Bearer 8cX80yIBws1PfBjFuvPz0k9egPSZD0LvS02oUD6ijfg' http://localhost:8080/obstacles | python3 -m json.tool"
```
Expected: `sonar_cm` drops to ~20, `safe_forward` becomes `false`.

3. Monitor Pi logs in a separate terminal:
```bash
ssh rajesh@192.168.68.61 "sudo journalctl -u turbopi-server -f"
```
Watch for: `SAFETY ESTOP: Sonar: obstacle at Xcm`

### Test 1: Sonar ESTOP (Low Obstacle)

**Goal:** Verify sonar catches objects that lidar/camera miss.

1. Place car in open area with battery connected
2. Trigger navigation via Telegram: **"Annie, explore the room"**
3. While car is moving, place a **low flat object** (book, cable, shoe) directly in front
4. **Expected:** Car stops with `SAFETY ESTOP: Sonar: obstacle at Xcm` in Pi logs
5. Monitor Annie logs on Titan:
```bash
ssh rajesh@192.168.68.52 "tail -f /tmp/annie-voice.log | grep -E 'Nav cycle|ESTOP|sonar'"
```

**If sonar doesn't fire:** Check `/obstacles` — is `sonar_cm` updating? Is the object within 30cm? Sonar beam is narrow (~15° cone) — object must be directly in front.

### Test 2: Full Navigation with All 3 Sensors

**Goal:** Verify sonar integrates smoothly with lidar + YOLO during multi-cycle navigation.

1. Place car facing a room corner or hallway
2. Telegram: **"Annie, explore around the dining table"** (or similar goal with obstacles)
3. Monitor nav cycles — each should log sonar value:
```
Nav cycle 3/10: action=forward, obstacles=1, safe_fwd=True, lidar='Forward: clear 2.1m, ...', sonar=85.3
```
4. **Expected:** Car navigates normally, sonar_cm appears in logs, safe_forward accounts for sonar

**If navigation hangs:** Check if VLM is responding:
```bash
ssh rajesh@192.168.68.52 "curl -s http://localhost:8003/health"
```

### Test 3: Sonar Degradation (Graceful Failure)

**Goal:** Verify system continues when sonar fails.

1. **Ask Rajesh to unplug the sonar I2C cable** from the Pi's GPIO
2. Wait 1-2 seconds for staleness
3. Check: `curl /health` → `sonar_healthy: false`
4. Trigger navigation: **"Annie, drive forward slowly"**
5. **Expected:** Car drives normally using YOLO + lidar only. No crash, no error.
6. **Reconnect sonar** — should auto-recover within 500ms

### Test 4: Visual Return-to-Start

**Goal:** Test the teach-and-repeat visual return.

1. Mark starting position with tape on floor
2. Telegram: **"Annie, explore the room and come back"** (must include `return_to_start: true` in tool args)
   - Note: This requires the LLM to pass `return_to_start=True`. If it doesn't, you may need to call `handle_navigate_robot` directly with `{"goal": "explore", "return_to_start": true, "max_cycles": 5}`
3. **Outbound:** Car navigates 3-5 cycles, storing visual waypoints
4. **Return:** Car uses VLM 2-image matching to navigate back
5. Monitor return progress:
```
Return waypoint 3/5 attempt 1: action=forward
Return waypoint 3/5 attempt 2: action=scene_matched
```
6. **Expected:** Car ends up within ~1m of tape (visual matching should beat blind reversal)

**If return doesn't trigger:** `return_to_start` IS in the schema (added in commit `0c6220e`). The LLM may still not pass it — check Annie logs for the tool call args. If the LLM passes `return_to_start: false` (or omits it), try rephrasing: **"Annie, explore and then use visual waypoints to return to where you started"**. If still failing, call `handle_navigate_robot` directly.

### Test 5: Combined Stress Test

1. Navigate with all 3 sensors active
2. Mid-navigation, place obstacle → sonar ESTOP
3. Clear ESTOP via Annie: **"Annie, stop the car"** (or `drive stop` direct)
4. Remove obstacle, then resume: **"Annie, continue exploring"**
5. Verify sonar resumes reporting after ESTOP clear
6. Check Annie's nav log — should show `"ESTOP triggered"` in history, then resumed cycles

### Test 6: Edge Cases (Bonus)

**6a. Sonar at minimum range:**
- Place object touching the sensor (< 2cm)
- `/obstacles` should show `sonar_cm: null` (below `SONAR_MIN_MM=20`)
- Car should NOT ESTOP (null sonar is treated as "no data", not "blocked")

**6b. Multiple rapid ESTOPs:**
- Trigger sonar ESTOP, clear it, trigger again within 2s
- Verify no race condition (ESTOP uses `threading.Event` + `_uart_lock`)
- Pi logs should show clean ESTOP → clear → ESTOP sequence

**6c. VLM timeout during visual return:**
- If vLLM on Titan is slow (> 30s), return should log timeout and skip waypoint
- Simulate by killing vLLM briefly during return phase (risky — only if confident)

**6d. Navigation with blocked forward path:**
- Place car facing a wall at ~20cm
- `/obstacles` should show `safe_forward: false`
- VLM should choose `left` or `right` (never `forward` when unsafe)
- Sonar should read ~20cm, lidar Forward sector should be `blocked`

**6e. Sonar + lidar agreement:**
- Place object 25cm in front
- Sonar should read ~25cm, lidar Forward should read `near` (~25cm)
- Both sensors should agree — if they disagree by >50%, investigate mounting angle

## ESTOP Recovery Procedure

When ESTOP fires, the car stops and rejects all drive commands with HTTP 409.

**To clear ESTOP:**
```bash
# Option 1: Direct API call
ssh rajesh@192.168.68.61 "curl -s -X POST -H 'Authorization: Bearer 8cX80yIBws1PfBjFuvPz0k9egPSZD0LvS02oUD6ijfg' -H 'Content-Type: application/json' -d '{\"action\":\"stop\",\"speed\":0,\"duration\":0}' http://localhost:8080/drive"
```
Expected: `{"status": "stopped", "estop_cleared": true}`

**Verify ESTOP cleared:**
```bash
ssh rajesh@192.168.68.61 "curl -s http://localhost:8080/health | python3 -c 'import sys,json; d=json.load(sys.stdin); print(f\"estop_active: {d[\"estop_active\"]}\")\'"
```
Expected: `estop_active: False`

**If ESTOP won't clear (stuck motors):**
```bash
ssh rajesh@192.168.68.61 "sudo systemctl restart turbopi-server"
```
systemd `ExecStartPre` zeros all motors before process restart — safe.

**Nuclear option (disconnect battery):**
Unplug USB-C power cable from the car. Motors lose power instantly.

## Monitoring Commands

**Pi sensor dashboard (run on dev machine):**
```bash
watch -n 1 'ssh rajesh@192.168.68.61 "curl -s -H \"Authorization: Bearer 8cX80yIBws1PfBjFuvPz0k9egPSZD0LvS02oUD6ijfg\" http://localhost:8080/obstacles | python3 -m json.tool"'
```

**Pi logs (safety ESTOP events):**
```bash
ssh rajesh@192.168.68.61 "sudo journalctl -u turbopi-server -f --grep 'ESTOP\|Sonar\|sonar'"
```

**Annie nav logs:**
```bash
ssh rajesh@192.168.68.52 "tail -f /tmp/annie-voice.log" | grep -E 'Nav cycle|Return waypoint|ESTOP|sonar'
```

**Quick sonar reading:**
```bash
ssh rajesh@192.168.68.61 "curl -s http://localhost:8080/distance"
```

## Known Issues to Watch For

| Issue | Symptom | Fix |
|-------|---------|-----|
| Sonar beam too narrow | Object not detected from side | Sonar is forward-only ~15° cone — by design |
| Sonar returns 0 at very close range | sonar_cm=None when object <2cm | SONAR_MIN_MM=20 filters this |
| Lidar sync error on startup | `sync bytes are mismatched` in logs | Auto-reconnects with backoff — normal |
| VLM timeout during return | `Nav return VLM call failed` | Check vLLM health on Titan port 8003 |
| LLM ignores `return_to_start` | Return never triggers despite "come back" prompt | Rephrase prompt or call tool directly with `{"return_to_start": true}` |
| ESTOP not clearing | Car stuck after sonar trigger | Send explicit `drive stop` command |
| Sonar reads 99999 after I2C replug | I2C bus not re-initialized | `sudo systemctl restart turbopi-server` |

## Critical Files

| File | What to Monitor/Fix |
|------|-------------------|
| `services/turbopi-server/safety.py` | SonarPoller (10 Hz poll, staleness, health) + HailoSafetyDaemon ESTOP logic |
| `services/turbopi-server/main.py` | Lifespan startup order, /obstacles (sonar_cm + safe_forward), /health, ESTOP callback |
| `services/turbopi-server/lidar.py` | LidarDaemon, 12-sector binning, BLOCKED/NEAR thresholds, LIDAR_FORWARD_OFFSET_DEG |
| `services/annie-voice/robot_tools.py` | Nav sense-think-act loop, VLM prompt (sonar section), `_return_via_waypoints()`, NavWaypoint |
| `services/annie-voice/tool_schemas.py` | NavigateRobotInput: goal, max_cycles, speed, return_to_start |

## Pass/Fail Criteria

| Test | PASS | FAIL | PARTIAL |
|------|------|------|---------|
| **Pre-Flight** | All 3 `_healthy: true`, sonar_cm is a real number | Any sensor unhealthy | Sonar reads 99999 (I2C not enabled) |
| **T1: Sonar ESTOP** | Pi logs `SAFETY ESTOP: Sonar: obstacle at Xcm` within 500ms of placing object | Sonar never fires, car drives into object | ESTOP fires but distance reading is wrong (> 50% off) |
| **T2: Full Nav** | 3+ cycles complete, sonar_cm appears in every nav cycle log, car avoids obstacles | Car crashes, nav hangs, sonar_cm missing from logs | Car navigates but sonar reads None on some cycles (stale) |
| **T3: Degradation** | `sonar_healthy: false` after unplug, car drives on YOLO+lidar only, no crash | Car crashes or refuses to drive without sonar | `sonar_healthy` stays true after unplug (staleness bug) |
| **T4: Visual Return** | Car ends within ~1m of tape, ≥50% waypoints show `scene_matched` | Return never triggers, car drives away, 0 waypoints matched | Car returns to general area but > 2m from tape |
| **T5: Stress** | ESTOP → clear → resume → sonar resumes reporting | ESTOP won't clear, sonar dead after clear, motor stuck | Resume works but sonar takes > 2s to resume |
| **T6a: Min range** | `sonar_cm: null`, no ESTOP | ESTOP on null reading | — |
| **T6d: Blocked path** | VLM picks left/right, never forward | VLM sends forward into wall | VLM picks stop (overly cautious) |
| **T6e: Agreement** | Sonar and lidar within 50% at 25cm | Readings differ by > 100% | — |

## Post-Test Cleanup

After all tests are complete:

1. **Stop the car:**
```bash
ssh rajesh@192.168.68.61 "curl -s -X POST -H 'Authorization: Bearer 8cX80yIBws1PfBjFuvPz0k9egPSZD0LvS02oUD6ijfg' -H 'Content-Type: application/json' -d '{\"action\":\"stop\",\"speed\":0,\"duration\":0}' http://localhost:8080/drive"
```

2. **Verify motors zeroed:**
```bash
ssh rajesh@192.168.68.61 "curl -s http://localhost:8080/health | python3 -m json.tool | grep estop"
```

3. **Reconnect sonar** if unplugged during Test 3. Verify:
```bash
ssh rajesh@192.168.68.61 "curl -s http://localhost:8080/health | python3 -c 'import sys,json; print(json.load(sys.stdin)[\"sonar_healthy\"])'"
```

4. **Disconnect battery** (USB-C) — don't leave car powered overnight

5. **Check Pi thermal state** (Hailo NPU can heat up):
```bash
ssh rajesh@192.168.68.61 "vcgencmd measure_temp && cat /sys/class/thermal/thermal_zone*/temp"
```

## Result Recording Template

Copy this block after testing. Fill in results and commit to `docs/`.

```markdown
## E2E Test Results — Sonar + Visual Return (DATE: YYYY-MM-DD)

**Environment:**
- Pi 5: 192.168.68.61, turbopi-server running
- Titan: 192.168.68.52, vLLM port 8003 healthy
- Battery: USB-C connected
- Firmware: commit ______

**Pre-Flight:**
- sonar_healthy: ___
- lidar_healthy: ___
- safety_daemon_healthy: ___
- sonar_cm reading: ___

| Test | Result | Notes |
|------|--------|-------|
| T1: Sonar ESTOP | PASS / FAIL / PARTIAL | |
| T2: Full Nav (3 sensors) | PASS / FAIL / PARTIAL | cycles completed: ___ |
| T3: Sonar Degradation | PASS / FAIL / PARTIAL | |
| T4: Visual Return | PASS / FAIL / PARTIAL | waypoints matched: ___/___, distance from tape: ___m |
| T5: Stress Test | PASS / FAIL / PARTIAL | |
| T6a: Min Range | PASS / FAIL / SKIP | |
| T6b: Rapid ESTOP | PASS / FAIL / SKIP | |
| T6d: Blocked Path | PASS / FAIL / SKIP | |
| T6e: Sensor Agreement | PASS / FAIL / SKIP | sonar: ___cm, lidar: ___cm |

**Bugs Found:**
1. ___
2. ___

**Fixes Applied (during session):**
1. ___

**Overall Verdict:** PASS / PARTIAL / FAIL
```

## Troubleshooting Deep Dives

### Sonar reads `null` consistently
1. Check I2C bus: `ssh rajesh@192.168.68.61 "sudo i2cdetect -y 1"` — sonar should appear at 0x77
2. If missing: `sudo raspi-config nonint do_i2c 0 && sudo reboot`
3. If present but reads fail: check wiring (VCC=5V, GND, SDA=GPIO2, SCL=GPIO3)

### ESTOP fires but car doesn't stop
1. Check `_uart_lock` isn't deadlocked: `sudo journalctl -u turbopi-server --since "1 min ago"`
2. Look for `"Board is None"` — means shutdown race triggered guard
3. Check USB serial: `ls /dev/ttyUSB*` — CH340 should be on ttyUSB0

### Visual return matches 0 waypoints
1. Lighting changed between outbound and return? (VLM is sensitive to shadows)
2. Car rotated significantly? The 2-image prompt needs similar angles
3. Check waypoint images: add `logger.debug` to dump base64 lengths (should be > 10KB each)
4. vLLM overloaded? Check `curl http://192.168.68.52:8003/health` — if queue > 5, cycles slow

### Nav cycle takes > 10s (should be ~3.5s)
1. vLLM cold start: first call after idle can take 15-20s (KV cache allocation)
2. Photo endpoint slow: camera not warmed up (FrameGrabber needs 1-2 frames)
3. Lidar scan slow: RPLIDAR spin-up takes ~2s on first call after idle

### Car drifts left/right during "forward"
1. Motor calibration: left/right motors may have different power curves
2. Floor surface: carpet vs tile affects traction differently per wheel
3. Not a software bug — mechanical. Note drift direction for future calibration

## Start Command

Read this file, then run Pre-Flight Checks. If all healthy, proceed through Tests 1-6 in order, monitoring logs and fixing any issues live. Do not stop until all tests have been attempted and any bugs are fixed. Record results using the template above.