commit 21206b44e2d9b4eb2b70a45de151657ea0900db1 Author: Wan Ziyu Date: Mon Jun 22 12:24:22 2026 +0900 initial commit diff --git a/README.md b/README.md new file mode 100644 index 0000000..2c4eb49 --- /dev/null +++ b/README.md @@ -0,0 +1,316 @@ +# codriver + +A deterministic, local navigation daemon that emits turn directives ("Turn left") +at the right moment. No LLM on the hot path. The LLM is a later, off-path layer +that decides *whether to speak* and *how to phrase*; this repo is the dumb, fast +substrate it sits on top of. + +Target hardware: NVIDIA Jetson Orin Nano with bolt-on GPS (NMEA-over-UART, wrapped +by `gpsd`) and IMU (I2C). Visual input and LLM co-driver are explicitly out of scope +for the current milestone. + +--- + +## Scope (current milestone — "step 0") + +**In:** Consume a pre-computed route, poll position, emit a maneuver string at a +speed-adaptive lead time, detect going off-route and reroute. + +**Out (deferred):** TTS, LLM phrasing/decision layer, IMU sensor fusion, visual +input, rally pace-note generation, recce-run note authoring. + +The deliverable of step 0 is: a process that prints `"Turn left"` (or the configured +flavour) when the driver should turn, not too late, and recomputes the route when the +driver deviates. + +--- + +## SWE-1 — Software Requirements + +Requirements are tagged `SWR-n` for traceability into the SWE-2 architecture and +later test cases (SWE-4/5/6). + +### Functional + +- **SWR-1** The system shall obtain a route from an origin to a destination as an + ordered geometry (polyline) plus a sparse, ordered list of maneuvers. +- **SWR-2** The system shall acquire the vehicle's current position, speed, and + heading by polling a position source. +- **SWR-3** The system shall project the current position onto the active route + geometry, yielding distance-along-route and cross-track distance. +- **SWR-4** The system shall identify the next not-yet-announced maneuver ahead of + the current along-route position. +- **SWR-5** The system shall compute a trigger distance as a function of current + speed and a configured lead time (constant *time* to maneuver, not constant + distance). +- **SWR-6** The system shall emit exactly one directive string per maneuver, when + the distance to that maneuver first falls within the trigger distance. +- **SWR-7** The system shall detect an off-route condition when sustained + cross-track distance exceeds a threshold (debounced over N consecutive fixes). +- **SWR-8** On off-route, the system shall request a new route from current position + to the unchanged destination and atomically replace the active route. +- **SWR-9** The directive string content shall be configurable ("flavour"), so the + phrasing layer is decoupled from the trigger logic. + +### Non-functional / constraints + +- **SWR-10** The decision path (acquire → match → decide → emit) shall contain no + network call and no LLM invocation; route acquisition (incl. reroute) is the only + component permitted to block on I/O and shall run off the decision path. +- **SWR-11** The position source has no native subscribe/interrupt mechanism; + the architecture shall treat polling as the normative interface. +- **SWR-12** Routing shall be performable fully offline from a periodically-updated + local dataset (no per-trip dependency on a cloud routing service). +- **SWR-13** The route source shall be replaceable (local engine, cloud API, or + recorded file) without change to the decision path. +- **SWR-14** "Not too late": at the configured lead time, the directive shall be + emitted with enough distance for the driver to act at the current speed. +- **SWR-15** A single noisy position fix shall not by itself cause a reroute or a + spurious/duplicate directive. + +--- + +## SWE-2 — Software Architectural Design + +### Layering + +Two tiers. The **decision tier** is the hot loop: pure arithmetic over an +in-memory route, runs at the position-source cadence (~1–10 Hz), never blocks on +network. The **planning tier** is off-path: it produces a `Route` artifact and hands +it to the decision tier through a single shared, atomically-swappable slot. + +``` + planning tier (slow, blocking I/O OK) + ┌─────────────────────────────────────────────────────────┐ + │ RouteProvider ──► Route artifact ──► ActiveRouteStore │ + └───────────────────────────────────────────▲──────────────┘ + │ atomic swap + ┌────────────────────────────────────────────┼─────────────┐ + │ PositionSource ─► StateStore ─► [ poll loop: MapMatcher │ + │ ─► ManeuverTracker │ + │ ─► TriggerPolicy │ + │ ─► OffRouteMonitor ] │ + │ ─► DirectiveEmitter ─► │ + └────────────────────────────────── decision tier (fast) ───┘ +``` + +### Module decomposition + +Each module below maps to a Python package/module. Interfaces are given as the +contract; types reference the data model in the next section. + +#### `route_provider/` — route acquisition (planning tier) +- **Responsibility:** turn `(origin, destination)` into a normalized `Route`. + Isolates all backend messiness (OSRM/Valhalla/Google/file) behind one contract. +- **Interface:** + - `RouteProvider.get_route(origin: LatLon, dest: LatLon) -> Route` +- **Implementations (selected at config time):** + - `route_provider/osrm.py` — `OsrmProvider`: HTTP client to a local OSRM + instance; normalizes `legs[].steps[].maneuver` + decoded `geometry` into the + internal `Route`/`Maneuver` model. **Primary backend.** + - `route_provider/file.py` — `FileProvider`: loads a pre-decoded route from disk. + For desk testing and reproducible runs. + - `route_provider/google.py` — `GoogleProvider`: optional online fallback. +- **Satisfies:** SWR-1, SWR-12, SWR-13. Normalization point also serves SWR-9's + decoupling (maneuver semantics fixed here, phrasing fixed downstream). +- **Note:** Owns *no* routing algorithm. Preprocessing of OSM extracts + (`osrm-extract`/`partition`/`customize`) is an operational step documented in + `ops/`, not code in this repo. + +#### `position/` — position acquisition (decision tier, ingress) +- **Responsibility:** poll the position source, parse to a `Fix`, publish the + freshest fix. +- **Interface:** + - `PositionSource.read() -> Fix` *(blocking until next fix; the GPS cadence is + the loop clock)* +- **Implementations:** + - `position/gpsd_source.py` — `GpsdSource`: client of local `gpsd`; `gpsd.next()` + as the blocking read. **Primary.** + - `position/nmea_source.py` — `NmeaSource`: direct `/dev/ttyUSB*` NMEA parse + (fallback if `gpsd` is undesired). + - `position/replay_source.py` — `ReplaySource`: replays a recorded NMEA/fix log + at wall-clock or accelerated rate. For desk testing without hardware. +- **Satisfies:** SWR-2, SWR-11. + +#### `state/` — shared state slots +- **Responsibility:** last-write-wins handoff between threads. Two slots: + current vehicle `Fix`, and the `ActiveRouteStore` for the route. +- **Interface:** + - `StateStore.put(fix: Fix)` / `StateStore.latest() -> Fix | None` + - `ActiveRouteStore.swap(route: Route)` / `ActiveRouteStore.current() -> Route | None` +- **Satisfies:** SWR-8 (atomic swap), SWR-10 (decouples blocking planning tier from + the non-blocking decision tier). + +#### `matching/` — map matching +- **Responsibility:** project a `Fix` onto the active `Route` polyline. +- **Interface:** + - `MapMatcher.match(fix: Fix, route: Route) -> MatchResult` + where `MatchResult = {along_dist_m, cross_track_m, segment_index, snapped: LatLon}` +- **Step-0 algorithm:** nearest-segment + perpendicular projection. No probabilistic + HMM matching yet. +- **Satisfies:** SWR-3. Produces the cross-track value consumed by OffRouteMonitor + for free. + +#### `maneuvers/` — maneuver tracking +- **Responsibility:** given `along_dist`, find the next not-yet-announced maneuver + and the along-route distance to it; own the per-maneuver "announced" markers. +- **Interface:** + - `ManeuverTracker.next_pending(along_dist_m: float) -> tuple[Maneuver, float] | None` + - `ManeuverTracker.mark_announced(maneuver_id) -> None` + - `ManeuverTracker.reset() -> None` *(called on route swap)* +- **Satisfies:** SWR-4, SWR-6 (exactly-once via markers). + +#### `trigger/` — trigger policy +- **Responsibility:** decide whether the next maneuver should fire *now*. +- **Interface:** + - `TriggerPolicy.should_fire(distance_to_maneuver_m: float, fix: Fix) -> bool` +- **Step-0 policy:** `trigger_distance = speed_mps * lead_time_s + reaction_buffer_m`; + fire when `distance_to_maneuver <= trigger_distance`. +- **Config:** `lead_time_s`, `reaction_buffer_m`. (A later second-stage early + pre-announce — "in 300 meters…" — slots in here as an additional threshold without + touching other modules.) +- **Satisfies:** SWR-5, SWR-14. + +#### `offroute/` — off-route monitor +- **Responsibility:** debounced off-route detection; request reroute. +- **Interface:** + - `OffRouteMonitor.update(match: MatchResult) -> OffRouteState` + (`ON_ROUTE` | `OFF_ROUTE`) +- **Step-0 algorithm:** `cross_track_m > threshold_m` sustained for `N` consecutive + fixes (hysteresis on recovery). On transition to `OFF_ROUTE`, signals the planning + tier to call `RouteProvider.get_route(current, dest)` and `ActiveRouteStore.swap`, + then `ManeuverTracker.reset`. +- **Config:** `threshold_m`, `confirm_fixes_n`. +- **Satisfies:** SWR-7, SWR-8, SWR-15. + +#### `directive/` — directive emitter (decision tier, egress) +- **Responsibility:** render a `Maneuver` into the output string and emit it. +- **Interface:** + - `DirectiveEmitter.emit(maneuver: Maneuver, fix: Fix) -> None` + - `Phrasebook.render(maneuver: Maneuver) -> str` +- **Step-0 implementation:** table-driven phrasebook (`maneuver.type` + + `modifier` → string); emit = `print()` / write to stdout or a socket. TTS and the + LLM phrasing layer subscribe here later; the seam is `Phrasebook`. +- **Satisfies:** SWR-6, SWR-9. + +#### `app/` — composition root +- **Responsibility:** wire modules from config, own the two threads (position + poller, decision loop) and the planning-tier reroute worker, handle lifecycle. +- **Interface:** `main()` + `config.py` (dataclass-validated config: provider + selection, lead time, thresholds, phrasebook flavour, source selection). +- **Decision loop (pseudocode):** + ``` + fix = state.latest() + route = active_route.current() + m = matcher.match(fix, route) + if offroute.update(m) == OFF_ROUTE: + request_reroute(fix.pos, dest) # hands off to planning tier + pending = tracker.next_pending(m.along_dist_m) + if pending: + maneuver, dist = pending + if trigger.should_fire(dist, fix): + emitter.emit(maneuver, fix) + tracker.mark_announced(maneuver.id) + ``` +- **Satisfies:** SWR-10 (keeps blocking reroute off this loop). + +### Data model (`model/`) + +```python +@dataclass(frozen=True) +class LatLon: + lat: float + lon: float + +@dataclass(frozen=True) +class Fix: + pos: LatLon + speed_mps: float + heading_deg: float | None # may be unreliable at low speed + t_monotonic: float + valid: bool + +@dataclass(frozen=True) +class Maneuver: + id: int + location: LatLon + type: str # turn | keep | uturn | roundabout | arrive ... + modifier: str | None # left | right | slight_left ... + entry_heading: float | None + exit_heading: float | None + road_name: str | None + +@dataclass(frozen=True) +class Route: + polyline: list[LatLon] # dense shape points + maneuvers: list[Maneuver] # sparse, ordered along polyline + destination: LatLon +``` + +`Maneuver.type`/`modifier` is the fixed internal vocabulary every provider +normalizes to; the phrasebook and (later) the LLM phrasing layer key off it. This is +the single contract that keeps backend messiness out of the rest of the system. + +### Concurrency model + +Three threads, no shared mutable state except the two `state/` slots (last-write-wins, +guarded by a lock or an atomic reference): + +1. **Position poller** — `PositionSource.read()` in a loop → `StateStore.put`. +2. **Decision loop** — reads both slots, runs the pipeline above at its cadence, + emits directives. Never blocks on I/O. +3. **Reroute worker** — waits for an off-route signal, calls the (blocking) + `RouteProvider`, swaps `ActiveRouteStore`. Keeps SWR-10 intact. + +### Traceability summary + +| Module | Requirements | +|-----------------|-------------------------------| +| route_provider | SWR-1, 9, 12, 13 | +| position | SWR-2, 11 | +| state | SWR-8, 10 | +| matching | SWR-3 | +| maneuvers | SWR-4, 6 | +| trigger | SWR-5, 14 | +| offroute | SWR-7, 8, 15 | +| directive | SWR-6, 9 | +| app | SWR-10 | + +--- + +## Repository layout + +``` +codriver/ +├── README.md +├── pyproject.toml +├── codriver/ +│ ├── app/ # composition root, config, main() +│ ├── model/ # LatLon, Fix, Maneuver, Route +│ ├── route_provider/ # osrm.py, file.py, google.py +│ ├── position/ # gpsd_source.py, nmea_source.py, replay_source.py +│ ├── state/ # StateStore, ActiveRouteStore +│ ├── matching/ # MapMatcher +│ ├── maneuvers/ # ManeuverTracker +│ ├── trigger/ # TriggerPolicy +│ ├── offroute/ # OffRouteMonitor +│ └── directive/ # DirectiveEmitter, Phrasebook +├── ops/ # OSM extract + OSRM preprocessing runbook (not app code) +└── tests/ + ├── fixtures/ # recorded NMEA logs, pre-decoded routes + └── ... +``` + +--- + +## Out of scope, but the seams are placed for them + +- **TTS** — subscribes at `DirectiveEmitter`; today's emit is `print()`. +- **LLM co-driver** — a *gate* before `DirectiveEmitter.emit` (decide whether to + speak) and/or a `Phrasebook` implementation (decide phrasing). Off the hot path by + construction. +- **IMU fusion** — a `PositionSource` decorator that fuses I2C accelerometer with + GPS to stabilize heading/speed at low speed; the `Fix` contract is unchanged. +- **Rally pace notes** — a different `RouteProvider` (notes authored from a recce + run) plus a richer `Maneuver` vocabulary; the decision tier is reused as-is. +```