initial commit

2026-06-22 12:24:22 +09:00
commit 21206b44e2
1 changed files with 316 additions and 0 deletions
@@ -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.
+```