317 lines
14 KiB
Markdown
317 lines
14 KiB
Markdown
# 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.
|
||
```
|