# Next Session: Implement SLAM Odometry Fix

## What

Add commanded-velocity odometry to SLAM prediction step on the TurboPi robot. SLAM currently
reports 6.9m displacement for <1m actual travel because `_slam_step()` has zero XY prediction —
only IMU heading delta is used. The scan matcher has no initial translation guess, converges
to local minima, and drift bakes into the map permanently.

**This plan was adversarially reviewed** by architecture and code quality destructors. 12 severe
issues were found and fixed. All code specs below reflect the post-review version.

## Problem Details

- SLAM reports 6.9m displacement when car moved <1m
- 211-degree heading drift over 8 drive cycles
- Lidar works perfectly (477 pts/rotation, rplidarc1 library, all 12 sectors)
- Root cause: `slam.py:_slam_step()` line 569 — only `pose[2] += IMU_heading`. X/Y unchanged.
- Scan matching on sparse map with zero initial XY guess → Gauss-Newton converges wrong → drift

## Architecture: Velocity Hint Pattern

A thread-safe `OdometryHint` stores body-frame velocity (m/s). Set by `/drive` endpoint
AFTER motor command fires, cleared BEFORE stop command (in finally block). SLAM daemon
integrates at each 10Hz step for continuous odometry during drives.

**Why not post-drive odometry:** During a 1s drive, SLAM runs ~10 steps without XY prediction.
Post-drive correction would leave those 10 steps with drifted poses baked into the map.

**Timing (critical — from adversarial review):**
```
/drive handler                          SLAM daemon (10Hz thread)
─────────────                           ────────────────────────
_drive_sync(action, speed)              
  → _uart_lock → send motor bytes       
  → return (motors now ramping)          
                                        _slam_step():
odom_hint.set_velocity(vx, vy, omega)     dt = monotonic - last_step_time
  ← SET AFTER motor command fires         body_dx = vx * dt  (hint active)
                                          pose_predict += rotate(body_dx, body_dy)
asyncio.wait_for(duration)                → repeats every ~100ms
                                        
wait returns (timeout or ESTOP)         
finally:                                
  odom_hint.clear()                     _slam_step():
    ← CLEAR BEFORE stop command           body_dx = 0 * dt = 0  (correct)
  _stop_motors_sync()                   
    → motors decelerating               
```

**Motor mode:** set_motor_duty ONLY (existing open-loop PWM). set_motor_speed (closed-loop)
was removed from this plan — STM32 sign convention for speed mode is unknown. Velocity is
estimated from calibration constant DUTY_TO_MPS.

## Key Design Decisions (from adversarial review — DO NOT revert these)

1. **Set hint AFTER motor cmd fires, clear BEFORE stop cmd** — eliminates pre/post-motion phantom. Ramp errors cancel (~+9mm/-9mm = net <1mm).
2. **Phase B (set_motor_speed) REMOVED** — sign convention unknown, gets its own plan later.
3. **ESTOP callback ALSO calls hint.clear()** — idempotent, prevents phantom after safety stops.
4. **ALWAYS consume IMU delta, even when stationary** — prevents accumulated heading jump.
5. **Stationary mode = skip MAP UPDATE only, NOT scan matching** — scan matching still runs for IMU drift correction. Old MIN_TRAVEL skipped everything — that was a regression.
6. **WARMUP_SCANS = 10** (was 3, originally proposed 15). 15 is too high — first drive could corrupt unmatched map.
7. **DUTY_TO_MPS defaults to 0.0** — fictional defaults cause worse drift than zero odometry. /health flags uncalibrated.
8. **compute_match_score vectorized with numpy** — Python for-loop over 500 points blows 10Hz budget on Pi 5.
9. **_last_step_time init to time.monotonic() in __init__** — prevents garbage dt on first SLAM step.

## Known Gotchas (must be respected)

| # | Gotcha | How Addressed |
|---|--------|---------------|
| G1 | Motor ports 1,3 need sign negation (`-v1, -v3`) in set_motor_duty | _drive_sync unchanged. Odometry uses body-frame velocity pre-inversion. |
| G2 | `consume_heading_delta_deg()` is DESTRUCTIVE — resets on read | ALWAYS consume before stationary check. Never double-read. |
| G3 | Pico IMU drops to REPL on serial conflicts | Stationary logic handles IMU-unhealthy: scan matching still runs. |
| G5 | Session 61: 12 adversarial fixes in HectorSLAM | No changes to scan_match/ray_cast/grid classes. Only prediction step + guards. |
| G8 | Session 62: MIN_TRAVEL removed (displacement=0 without encoders) | Stationary mode skips MAP UPDATE only (not scan matching). Fundamentally different. |

## Files to Modify (in build order)

### Task 1: OdometryHint class (no dependencies)
**New file:** `services/turbopi-server/odometry.py`

```python
import logging
import threading

logger = logging.getLogger("odometry")


class OdometryHint:
    """Thread-safe velocity hint for SLAM prediction.

    Set by /drive endpoint AFTER motor command fires.
    Consumed by SLAM daemon at each 10Hz step.
    Body-frame: +x = right, +y = forward, +omega = clockwise.

    Timing: hint active only while motors at commanded speed.
    Ramp error is ~+-9mm per drive (cancels out).
    """

    def __init__(self) -> None:
        self._lock = threading.Lock()
        self._vx_mps: float = 0.0
        self._vy_mps: float = 0.0
        self._omega_rps: float = 0.0
        self._active: bool = False

    def set_velocity(self, vx: float, vy: float, omega: float) -> None:
        """Set body-frame velocity (m/s, m/s, rad/s). Called after motor command."""
        with self._lock:
            self._vx_mps = vx
            self._vy_mps = vy
            self._omega_rps = omega
            self._active = True

    def clear(self) -> None:
        """Clear velocity. Idempotent -- safe to call from ESTOP + finally."""
        with self._lock:
            self._vx_mps = 0.0
            self._vy_mps = 0.0
            self._omega_rps = 0.0
            self._active = False

    def get_velocity(self) -> tuple[float, float, float, bool]:
        """Returns (vx, vy, omega, active). Non-destructive read."""
        with self._lock:
            return (self._vx_mps, self._vy_mps, self._omega_rps, self._active)

    def get_displacement(self, dt_s: float) -> tuple[float, float, float]:
        """Body-frame displacement for dt seconds."""
        with self._lock:
            dx = self._vx_mps * dt_s
            dy = self._vy_mps * dt_s
            dtheta = self._omega_rps * dt_s
        if abs(dx) > 1e-6 or abs(dy) > 1e-6 or abs(dtheta) > 1e-6:
            logger.debug(
                "Odom displacement: dx=%.4f dy=%.4f dtheta=%.4f (dt=%.3f)",
                dx, dy, dtheta, dt_s,
            )
        return (dx, dy, dtheta)

    def __repr__(self) -> str:
        vx, vy, omega, active = self.get_velocity()
        return (
            f"OdometryHint(vx={vx:.3f}, vy={vy:.3f}, omega={omega:.3f}, "
            f"active={active})"
        )
```

**Tests (~8):** Thread safety (concurrent set/get), set/clear/get cycle, displacement math,
repr, idempotent clear, zero-velocity no-op, debug logging.

---

### Task 2: Compute body-frame velocity + wire into /drive
**Modify:** `services/turbopi-server/main.py`

**2a. New constants (near line 73, after SPIN_SCALE):**
```python
# Calibration: PWM duty -> physical velocity (measured empirically).
# Default 0.0 = UNCALIBRATED. Run scripts/calibrate_odometry.py to set.
# speed=50 at DUTY_TO_MPS=0.003 -> 0.15 m/s (typical for TurboPi on hard floor)
DUTY_TO_MPS = float(os.getenv("DUTY_TO_MPS", "0.0"))
DUTY_TO_RPS = float(os.getenv("DUTY_TO_RPS", "0.0"))
assert DUTY_TO_MPS >= 0, f"DUTY_TO_MPS must be >= 0, got {DUTY_TO_MPS}"
assert DUTY_TO_RPS >= 0, f"DUTY_TO_RPS must be >= 0, got {DUTY_TO_RPS}"
```

**2b. New function (after ACTION_MAP, ~line 120):**
```python
def _compute_body_velocity(action: str, speed: int) -> tuple[float, float, float]:
    """Compute body-frame velocity (m/s, m/s, rad/s) from drive parameters.

    Uses ACTION_MAP direction angles to derive body-frame vx/vy.
    Angular velocity from angular multiplier x speed x DUTY_TO_RPS.
    Returns (0, 0, 0) for uncalibrated (DUTY_TO_MPS == 0).
    """
    if action == "stop" or DUTY_TO_MPS == 0.0:
        return (0.0, 0.0, 0.0)
    vel_mult, direction_deg, ang_mult = ACTION_MAP[action]
    velocity_mps = speed * vel_mult * DUTY_TO_MPS
    rad = math.radians(direction_deg)
    vx = velocity_mps * math.cos(rad)  # body +x = right
    vy = velocity_mps * math.sin(rad)  # body +y = forward
    omega = speed * ang_mult * DUTY_TO_RPS  # +ve = clockwise
    return (vx, vy, omega)
```

**2c. Global + lifespan wiring:**
- Add `from odometry import OdometryHint` at top of file
- Add module-level `_odom_hint: OdometryHint | None = None` near other globals (~line 163)
- In lifespan function: create `_odom_hint = OdometryHint()`, pass to SlamDaemon constructor
- In lifespan: log warning if DUTY_TO_MPS == 0.0 or DUTY_TO_RPS == 0.0

**2d. Wire into /drive handler (currently lines 760-794):**
The critical change is the TIMING of set_velocity/clear. Read current code first.
```python
async with _motor_lock:
    # ... ESTOP checks (unchanged) ...
    _stop_event.clear()

    # Compute velocity BEFORE starting motors (just the math, no side effects)
    vx, vy, omega = _compute_body_velocity(req.action, speed)

    # START motors (UART write, returns when command sent to STM32)
    await loop.run_in_executor(None, _drive_sync, _board, req.action, speed)
    _last_command_time = time.monotonic()
    _command_timestamps.append(time.monotonic())

    # Set hint AFTER motor command fires (review fix A1: no pre-motion phantom)
    if _odom_hint:
        _odom_hint.set_velocity(vx, vy, omega)

    try:
        await asyncio.wait_for(_stop_event.wait(), timeout=duration)
    except asyncio.TimeoutError:
        pass
    finally:
        # Clear hint BEFORE stopping motors (review fix A1: no post-motion phantom)
        if _odom_hint:
            _odom_hint.clear()
        await loop.run_in_executor(None, _stop_motors_sync, _board)
```

**2e. Wire ESTOP callback (review fix A3):**
Find the safety ESTOP callback function. Add `_odom_hint.clear()` to it:
```python
# In the ESTOP callback (wherever _hardware_estop.set() is called):
if _odom_hint:
    _odom_hint.clear()  # idempotent -- also cleared in finally
```

**2f. Add odometry to /drive response (after the async with block):**
```python
return {
    "status": "completed",
    "action": req.action,
    "duration_s": duration,
    "speed": speed,
    "distance_mm": distance,
    "battery_v": round(battery / 1000, 2) if battery and battery > 0 else 0,
    "odometry": {
        "dx_m": round(vx * duration, 4),
        "dy_m": round(vy * duration, 4),
        "dtheta_rad": round(omega * duration, 4),
    },
}
```

**2g. Add odom_calibrated to /health response:**
```python
"odom_calibrated": DUTY_TO_MPS > 0.0 and DUTY_TO_RPS > 0.0,
```

**Tests (~10):** Body velocity for all 11 actions, zero when uncalibrated, assertion on
negative DUTY_TO_MPS, ESTOP clears hint, try/finally clearing, odometry response fields.

---

### Task 3: SLAM prediction with odometry
**Modify:** `services/turbopi-server/slam.py`

**3a. New constants (replace/add near line 55):**
```python
WARMUP_SCANS = 10                                   # was 3 (review: 15 too high, A5)
MATCH_CONFIDENCE_THRESHOLD = 0.3                     # avg P(occupied) at scan points
STATIONARY_ANGULAR_THRESHOLD = math.radians(0.5)     # per SLAM step
```

**3b. Add TYPE_CHECKING import for OdometryHint:**
```python
if TYPE_CHECKING:
    from imu import ImuReader
    from lidar import LidarDaemon
    from odometry import OdometryHint  # NEW
```

**3c. SlamDaemon constructor (currently lines 415-439):**
Add `odom_hint` parameter and `_last_step_time`:
```python
def __init__(
    self,
    lidar: "LidarDaemon",
    imu: "ImuReader | None" = None,
    forward_offset_deg: float = 0.0,
    odom_hint: "OdometryHint | None" = None,  # NEW (review fix C3)
):
    super().__init__(daemon=True, name="slam-daemon")
    self._lidar = lidar
    self._imu = imu
    self._forward_offset_deg = forward_offset_deg
    self._odom_hint = odom_hint                      # NEW (C3)
    self._running = True
    self._state = SlamState.INIT

    self._multi_grid = MultiResGrid(MAP_SIZE_M)
    self._pose = np.array([0.0, 0.0, 0.0])
    self._scan_count = 0
    self._last_epoch = -1
    self._last_step_time = time.monotonic()          # NEW (C2) -- init to now

    self._slam_lock = threading.Lock()
    self._cached_map_png: bytes = b""
    self._cached_map_epoch: int = -1
```

**3d. New function: compute_match_score (vectorized, add before SlamDaemon class):**
```python
def compute_match_score(
    scan_xy: np.ndarray,
    grid: OccupancyGrid,
    pose: np.ndarray,
) -> float:
    """Average map probability at matched scan points. 1.0 = perfect.

    On unexplored grid (log-odds=0): sigmoid(0)=0.5 > threshold 0.3,
    so gate passes for new areas (correct -- we want to map them).
    Gate rejects only when match puts points in genuinely wrong cells.
    """
    world_xy = transform_scan_to_world(scan_xy, pose)
    n = grid.n_cells
    cols = ((world_xy[:, 0] - grid.origin_m) / grid.cell_size_m).astype(int)
    rows = ((world_xy[:, 1] - grid.origin_m) / grid.cell_size_m).astype(int)
    valid = (rows >= 1) & (rows < n - 1) & (cols >= 1) & (cols < n - 1)
    if not np.any(valid):
        return 0.0
    vr, vc = rows[valid], cols[valid]
    log_odds = grid.grid[vr, vc]
    probs = 1.0 / (1.0 + np.exp(-np.clip(log_odds, -10, 10)))
    return float(np.mean(probs))
```

**3e. Replace _slam_step() (currently lines 541-599):**
The full replacement is in the plan. Key changes:
1. Compute actual dt from time.monotonic(), clamped [0.01, 1.0]
2. ALWAYS consume IMU delta (before stationary check)
3. Add odometry body-to-world rotation after IMU heading
4. Add confidence gate after scan matching
5. Stationary mode: skip MAP UPDATE only (not scan matching or pose update)

Full code for _slam_step:
```python
def _slam_step(self) -> None:
    """Single SLAM iteration: predict, match, update."""
    if not self._lidar.is_healthy():
        return

    epoch = self._lidar.get_scan_epoch()
    if epoch == self._last_epoch:
        return
    self._last_epoch = epoch

    raw_points = self._lidar.get_raw_points()
    if len(raw_points) < MIN_SCAN_POINTS:
        return

    scan_xy = lidar_polar_to_xy(raw_points, self._forward_offset_deg)
    if len(scan_xy) < MIN_SCAN_POINTS:
        return

    # Subsample for real-time performance (Fix 3)
    if len(scan_xy) > MAX_SLAM_POINTS:
        indices = np.linspace(0, len(scan_xy) - 1, MAX_SLAM_POINTS, dtype=int)
        scan_xy = scan_xy[indices]

    # -- TIMING (review fix C2) --
    now = time.monotonic()
    dt = min(max(now - self._last_step_time, 0.01), 1.0)
    self._last_step_time = now

    # -- PREDICT: IMU heading + odometry XY --
    # ALWAYS consume IMU delta (review fix C5/G2: prevents accumulated jump)
    imu_delta_rad = 0.0
    if self._imu is not None and self._imu.is_healthy():
        imu_delta_rad = math.radians(self._imu.consume_heading_delta_deg())

    pose_predict = self._pose.copy()
    pose_predict[2] += imu_delta_rad

    # Odometry: body-frame velocity -> world-frame displacement
    odom_active = False
    if self._odom_hint is not None:
        body_dx, body_dy, body_dtheta = self._odom_hint.get_displacement(dt)
        odom_active = self._odom_hint.get_velocity()[3]
        cos_t = math.cos(pose_predict[2])
        sin_t = math.sin(pose_predict[2])
        # Same rotation convention as transform_scan_to_world
        pose_predict[0] += cos_t * body_dx - sin_t * body_dy
        pose_predict[1] += sin_t * body_dx + cos_t * body_dy
        # Odometry theta only if IMU unhealthy (IMU more accurate for heading)
        if not (self._imu is not None and self._imu.is_healthy()):
            pose_predict[2] += body_dtheta

    # Transition from INIT on first valid scan
    if self._state == SlamState.INIT:
        imu_ok = self._imu is not None and self._imu.is_healthy()
        self._state = SlamState.RUNNING if imu_ok else SlamState.DEGRADED
        logger.info(
            "SLAM -> %s (first scan, %d points)",
            self._state.value, len(scan_xy),
        )

    # -- MATCH: multi-resolution Gauss-Newton --
    if self._scan_count >= WARMUP_SCANS:
        pose_matched = scan_match_multi_res(
            scan_xy, self._multi_grid, pose_predict
        )
        # Confidence gate (review fix C6: vectorized)
        score = compute_match_score(
            scan_xy, self._multi_grid.levels[0], pose_matched
        )
        if score < MATCH_CONFIDENCE_THRESHOLD:
            logger.debug("Match score %.2f < threshold -- using prediction", score)
            pose_matched = pose_predict
    else:
        pose_matched = pose_predict

    # -- STATIONARY MODE (review fix C9/G8/A7) --
    # When stationary: still update POSE (drift correction) but skip MAP UPDATE
    # (prevents noise accumulation). Different from old MIN_TRAVEL.
    imu_rotating = abs(imu_delta_rad) > STATIONARY_ANGULAR_THRESHOLD
    is_stationary = (
        not odom_active
        and not imu_rotating
        and self._scan_count > WARMUP_SCANS
    )

    # -- UPDATE --
    with self._slam_lock:
        self._pose = pose_matched
        self._scan_count += 1
        if not is_stationary:
            update_map(scan_xy, pose_matched, self._multi_grid.levels[0])
            self._multi_grid.downsample()
```

**Tests (~15):** Odometry prediction (forward, strafe, diagonal at various headings),
dt clamping (first step, GC pause), IMU-healthy vs unhealthy theta behavior,
stationary mode (pose updated, map NOT updated), confidence gate (occupied/empty/mixed grids),
vectorized score correctness, _last_step_time initialization, odom_hint=None backward compat.

---

### Task 4: Navigation tuning
**Modify:** `services/annie-voice/robot_tools.py` + `services/annie-voice/tool_schemas.py`

**4a.** robot_tools.py line 599: change `"duration": 2.0` to `"duration": 1.0`
**4b.** robot_tools.py line 480: change `min(..., 10), 10)` to `min(..., 15)), 20)`
**4c.** tool_schemas.py ~line 408: change `default=10, ge=1, le=10` to `default=15, ge=1, le=20`
**4d.** robot_tools.py: parse `odometry` from drive_result in history append

**Tests (~5):** Duration/cycles clamping, schema validation, odometry field parsing.

---

### Task 5: Calibration scripts + deployment
**New file:** `scripts/calibrate_odometry.py`
- Drives forward at speed 50 for 2s, user measures distance, computes DUTY_TO_MPS
- Spins in place, reads IMU heading delta, computes DUTY_TO_RPS
- Prints env vars to set

**New file:** `scripts/test_motor_speed.py`
- Tests if `board.set_motor_speed()` works on this STM32 firmware
- Informational for future Phase B plan. Not used in this implementation.

---

## Existing Functions to Reuse

| Function | File:Line | Purpose |
|----------|-----------|---------|
| `transform_scan_to_world()` | slam.py:237 | Same rotation for body->world |
| `ACTION_MAP` | main.py:108 | Velocity/direction per action |
| `_uart_lock` | main.py:167 | Motor I/O protection |
| `_motor_lock` | main.py:~400 | Drive sequencing |
| `_FakeLidar`, `_FakeImu` | test_slam.py:319-351 | Mock patterns for tests |

## Files NOT to modify (preserving session 61 fixes)

- `slam.py` scan_match(), scan_match_multi_res() -- no optimizer changes
- `slam.py` ray_cast_update() -- no Bresenham changes
- `slam.py` OccupancyGrid, MultiResGrid -- no grid class changes
- `lidar.py` -- no lidar changes
- `imu.py` -- no IMU changes

## Build Order

```
Task 1: OdometryHint class (no dependencies)
    |
    v
Task 2: Wire into /drive + body velocity (depends on Task 1)
    |
    v
Task 3: SLAM prediction + tuning (depends on Task 1)
    |   (Tasks 2 and 3 can be parallel after Task 1)
    v
Task 4: Navigation tuning (independent of Tasks 1-3)
    |
    v
Task 5: Calibration + deployment (depends on all above)
```

## Verification

1. `cd services/turbopi-server && python -m pytest tests/ -v` -- expect 217 existing + ~33 new pass
2. `cd services/annie-voice && python -m pytest tests/ -v` -- expect 111 existing + ~5 new pass
3. Deploy: `ssh pi "cd ~/workplace/her/her-os && git pull && sudo systemctl restart turbopi-server"`
4. Calibrate: `ssh pi "cd ~/workplace/her/her-os && python3 scripts/calibrate_odometry.py"`
5. Health: `curl -s http://192.168.68.61:8080/health | jq '.slam_healthy, .odom_calibrated'`
6. Drive test: POST /drive forward 1s -> check /pose -> displacement within 2x of actual
7. E2E: "Annie, explore this room" via Telegram -> map shows walls, not radiating artifacts

## Success Criteria

1. SLAM displacement within **2x** of actual (was 7x)
2. Heading drift < **30 degrees** over 10 drive cycles (was 211 degrees)
3. Map shows room walls, not radiating artifacts
4. All existing tests pass (217 turbopi + 111 annie-voice)
5. New tests pass (~38 new)
6. /health shows `odom_calibrated: true` after calibration

## Git State

Main at `c390bdb`. Clean working tree. Pi running rplidarc1 lidar (session 63).