Select qualified speed-limit sources, preserve acceptance and override decisions, and publish distinct control and display coordinates. Carry accepted and pending source identity with each displayed decision.

Co-authored-by: firestarsdog <229254897+firestarsdog@users.noreply.github.com>
This commit is contained in:
firestar5683
2026-09-25 14:53:19 -05:00
parent 9a16e8e922
commit 58427d61d9
44 changed files with 7484 additions and 9 deletions
+7
View File
@@ -0,0 +1,7 @@
Copyright © 2025 Jacob Pfeifer
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+5
View File
@@ -0,0 +1,5 @@
{
"scope": "Synthetic provider v1 Event decoded by both Go and host Python tests; no actual road data.",
"schema_sha256": "4a4f4fa734b6ed9a57be8cfc9d35dc22106c0155c737208060b2d4a63f165e89",
"event_base64": "AAAAAB8AAAAAAAAAAgABAADQtytqAAAAjwAAAAAAAAAAAAAADgAGAEaUVkEAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAACAAEAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAgAMAAEABQDLBPtxHwEAAAIAAAAAAAAAgN+8KGoAAACAOR8ragAAAAMAAAAAAAAAEVDNqzQSAAAAAAAAAAAAAAAAAAAAAAAADQAAAMIAAAAAAAAAAAAAAAAAAAAAAAAADQAAAMoAAABTeW50aGV0aWMgcHJvdmlkZXIgcm9hZAAyMCBAIChNby1GciAwODowMC0wOTowMCkAAAAAAAAAAA=="
}
@@ -0,0 +1,81 @@
"""Optional normalized cruise ceiling for the existing longitudinal planner."""
import math
from dataclasses import dataclass
from openpilot.starpilot.speed_limits.acceptance import Authority, authority_is_valid
@dataclass(frozen=True)
class CruiseCeiling:
speed_mps: float
authority: Authority
@dataclass(frozen=True)
class CurveCeiling:
speed_mps: float
model_ns: int
@dataclass(frozen=True)
class CeilingDecision:
effective_mps: float | None
status: str
def _positive_finite(value: object) -> bool:
if type(value) not in (int, float):
return False
assert isinstance(value, (int, float))
try:
return math.isfinite(value) and value > 0
except OverflowError:
return False
def select_cruise_ceiling(ceiling: object, *, driver_v_cruise_kph: float, driver_cruise_mps: float,
system_long_available: bool, long_control_active: bool,
selfdrive_enabled: bool, force_decel: bool) -> CeilingDecision:
"""Qualify a supplied cap; None means retain the planner's ordinary cruise path."""
if ceiling is None:
return CeilingDecision(None, "absent")
if not isinstance(ceiling, CruiseCeiling) or not _positive_finite(ceiling.speed_mps) or not authority_is_valid(ceiling.authority):
return CeilingDecision(None, "invalid")
if any(type(flag) is not bool for flag in (system_long_available, long_control_active, selfdrive_enabled, force_decel)):
return CeilingDecision(None, "invalid")
if (not system_long_available or not long_control_active or not selfdrive_enabled or
not ceiling.authority.can_control):
return CeilingDecision(None, "inactive")
if not _positive_finite(driver_v_cruise_kph) or driver_v_cruise_kph == 255 or not _positive_finite(driver_cruise_mps):
return CeilingDecision(None, "driver_cruise_unavailable")
if force_decel:
return CeilingDecision(None, "force_decel")
if ceiling.speed_mps >= driver_cruise_mps:
return CeilingDecision(None, "above_driver")
return CeilingDecision(ceiling.speed_mps, "applied")
def select_curve_ceiling(ceiling: object, *, model_ns: int, driver_v_cruise_kph: float, effective_cruise_mps: float,
system_long_available: bool, long_control_active: bool, car_long_active: bool,
selfdrive_enabled: bool, force_decel: bool, driver_override: bool) -> CeilingDecision:
"""A host-qualified curve cap must still match this exact native model frame."""
if ceiling is None:
return CeilingDecision(None, "absent")
if (not isinstance(ceiling, CurveCeiling) or not _positive_finite(ceiling.speed_mps) or
type(ceiling.model_ns) is not int or ceiling.model_ns <= 0 or type(model_ns) is not int or model_ns <= 0):
return CeilingDecision(None, "invalid")
if ceiling.model_ns != model_ns:
return CeilingDecision(None, "wrong_model_frame")
if any(type(flag) is not bool for flag in (system_long_available, long_control_active, car_long_active,
selfdrive_enabled, force_decel, driver_override)):
return CeilingDecision(None, "invalid")
if not system_long_available or not long_control_active or not car_long_active or not selfdrive_enabled or driver_override:
return CeilingDecision(None, "inactive")
if not _positive_finite(driver_v_cruise_kph) or driver_v_cruise_kph == 255 or not _positive_finite(effective_cruise_mps):
return CeilingDecision(None, "driver_cruise_unavailable")
if force_decel:
return CeilingDecision(None, "force_decel")
if ceiling.speed_mps >= effective_cruise_mps:
return CeilingDecision(None, "not_binding")
return CeilingDecision(ceiling.speed_mps, "selected")
+79 -9
View File
@@ -2,6 +2,7 @@
import hashlib
import json
import math
import os
from pathlib import Path
import re
@@ -83,8 +84,8 @@ def _invalid_json_constant(_value):
raise ValueError('nonfinite snapshot JSON number')
def _selected_snapshot(root: Path) -> bool:
"""Check bounded receipt identity; Go checks each requested tile lazily."""
def _selected_receipt(root: Path):
"""Read the bounded, content-addressed receipt selected by Go."""
try:
with _open_regular(root / 'current.json', max_bytes=MAX_SELECTOR_BYTES) as file:
selector = json.loads(file.read(MAX_SELECTOR_BYTES + 1), object_pairs_hook=_unique_object,
@@ -92,15 +93,15 @@ def _selected_snapshot(root: Path) -> bool:
if (type(selector) is not dict or set(selector) != {'version', 'generation'} or
type(selector['version']) is not int or selector['version'] != 1 or
type(selector['generation']) is not str or re.fullmatch(r'[0-9a-f]{64}', selector['generation']) is None):
return False
return None
generations = root / 'generations'
generation = generations / selector['generation']
if not stat.S_ISDIR(generations.lstat().st_mode) or not stat.S_ISDIR(generation.lstat().st_mode):
return False
return None
with _open_regular(generation / 'receipt.json', max_bytes=MAX_RECEIPT_BYTES) as file:
raw = file.read(MAX_RECEIPT_BYTES + 1)
if hashlib.sha256(raw).hexdigest() != selector['generation']:
return False
return None
receipt = json.loads(raw, object_pairs_hook=_unique_object, parse_constant=_invalid_json_constant)
if (type(receipt) is not dict or receipt.get('version') != 1 or type(receipt.get('version')) is not int or
receipt.get('tileSchema') != 'offline.capnp:0xda3a0d9284ca402f' or
@@ -109,10 +110,56 @@ def _selected_snapshot(root: Path) -> bool:
receipt.get('sourceKind') not in ('local-pbf', 'provided-group-archives') or
type(receipt.get('tiles')) is not list or not 0 < len(receipt['tiles']) <= 65536 or
type(receipt.get('inputs')) is not list or not receipt['inputs']):
return False
return True
return None
return selector['generation'], receipt, len(raw)
except (OSError, ValueError, TypeError, OverflowError, RecursionError):
return False
return None
def _selected_snapshot(root: Path) -> bool:
"""Check bounded receipt identity; Go checks each requested tile lazily."""
return _selected_receipt(root) is not None
def selected_snapshot_info(root: Path) -> dict | None:
"""Disk inventory of the selected package; Go verifies tile content at selection/use."""
selected = _selected_receipt(root)
if selected is None:
return None
generation_id, receipt, receipt_size = selected
generation = root / 'generations' / generation_id
total = receipt_size
seen = set()
try:
if not stat.S_ISDIR(root.lstat().st_mode):
return None
for tile in receipt['tiles']:
if (type(tile) is not dict or type(tile.get('path')) is not str or type(tile.get('size')) is not int or
not 0 < tile['size'] <= 73 << 20 or type(tile.get('sha256')) is not str or
re.fullmatch(r'[0-9a-f]{64}', tile['sha256']) is None):
return None
relative = Path(tile['path'])
if (relative.is_absolute() or len(relative.parts) != 3 or '..' in relative.parts or
'\\' in tile['path'] or tile['path'] in seen or relative.as_posix() != tile['path']):
return None
seen.add(tile['path'])
for parent in (generation / relative.parts[0], generation / relative.parts[0] / relative.parts[1]):
if not stat.S_ISDIR(parent.lstat().st_mode):
return None
with _open_regular(generation / relative) as file:
info = os.fstat(file.fileno())
if info.st_size != tile['size']:
return None
total += info.st_size
bounds = receipt.get('bounds')
if (type(bounds) is not list or len(bounds) != 4 or
any(type(value) not in (int, float) or not math.isfinite(value) for value in bounds) or
not -90 <= bounds[0] < bounds[2] <= 90 or not -180 <= bounds[1] < bounds[3] <= 180):
return None
return {'generation': generation_id, 'storedBytes': total, 'tileCount': len(seen), 'bounds': bounds}
except (OSError, ValueError, TypeError):
return None
def preflight(binary: Path = PROVIDER_BINARY, manifest_path: Path = PROVIDER_MANIFEST,
@@ -164,9 +211,32 @@ def enabled(started: bool, params: Params, CP: car.CarParams) -> bool:
return False
try:
with _open_regular(Path(params.get_param_path(OPT_IN_KEY)), max_bytes=1) as file:
return file.read(2) == b'1'
if file.read(2) == b'1':
return True
except (OSError, ValueError):
pass
# Selecting Maps owns producer demand directly. Do not rewrite the separate
# diagnostic opt-in, and do not start control inside the Go shadow provider.
from openpilot.starpilot.saved_source import read_saved
from openpilot.starpilot.speed_limits.runtime_settings import LABELS, read_params
from openpilot.starpilot.speed_limits.selection import SelectionMode, Source
# Default Vision/Dashboard needs no Maps producer or full settings scan.
# These bounded reads are demand hints only; canonical parsing below owns
# the enabled/display state and rejects malformed complete preferences.
labels = []
for key in ('SLCPriority1', 'SLCPriority2'):
raw, readable = read_saved(params, key, 32)
if not readable:
return False
try:
labels.append(raw.decode('utf-8') if raw is not None else None)
except UnicodeError:
return False
if not any(LABELS.get(label) is Source.MAP for label in labels) and labels[0] not in ('Highest', 'Lowest'):
return False
settings = read_params(params)
return bool(not settings.errors and (settings.enabled or settings.display) and
(Source.MAP in settings.selection.slots or settings.selection.mode is not SelectionMode.ORDERED))
class MapdShadowProcess(NativeProcess):
@@ -0,0 +1,74 @@
# Causal cruise-action contract
`action_arbitration` preserves the relationship between an input action and its
later cruise-speed effects. It does not infer a button press from a scalar speed
change, authorize control, read messages or change cruise speed. The runtime
adapter connects it to physical receipts, UI requests and the override policy.
The input adapter must establish real action identities and final ownership.
If a vehicle exposes only a scalar change without a reliable button/effect
association, this capability remains unresolved. A guessed frame window is not
a substitute for causal evidence.
## Lifecycle
1. Create `new_ledger(unique_session_id)` once for a session. Use that same session
for acceptance and action sequencing; never reuse it across drives.
2. `BeginAction` records a monotonically increasing action ID, its observed origin
and an explicit `context_id`. Origins distinguish a driver cruise action, another
driver action and an automatic operation. The context must bind the adapter's
accepted-limit and authority/policy episode. This ledger carries it unchanged;
the override policy must check current applicability independently.
3. `ResolveAction` supplies a final disposition after arbitration: unconsumed
driver intent, consumed by SLC, automatic or unrelated. `resolve_acceptance`
instead binds a matching final receipt from the actual acceptance reducer.
It checks session, action ID, action/decision timing and receipt consumption.
An acknowledgment, replay, missing receipt or errored decision cannot establish
final ownership. A final unconsumed receipt does not swallow a real cruise action.
4. `CruiseChange` supplies its own increasing effect ID, causal action ID, and
normalized previous/selected speeds. It emits immutable classification data,
including the original action time and context. Multiple effects can refer to
one action until explicitly completed. Effects may arrive after other actions
have begun or resolved; they keep their own origin.
5. `CompleteAction` releases the transaction after its effects have been resolved
or explicitly abandoned. Button release alone does not prove completion.
Effects arriving after completion are unresolved and cannot recreate intent.
Consumed confirmation/adoption transactions remain consumed regardless of delay;
there is no one-frame Boolean or elapsed-time guess. Automatic and unrelated
effects likewise cannot become driver intent. A first scalar sample, an unchanged
speed or an unknown origin never establishes a new driver selection. An effect
received before ownership resolution is explicitly unresolved and watermarked;
it is not retroactively emitted as fresh intent when resolution arrives later.
## Bounds and failure behavior
The ledger holds at most 128 active transactions. Explicit completion frees
capacity. Exhaustion latches `reset_required` without forgetting consumed actions.
Conflicting final dispositions, impossible origin/disposition pairs, malformed
events and reversed clocks also require a fresh session. Duplicate action/effect
IDs are acknowledged by status without reopening transactions or emitting an
effect twice. Foreign sessions cannot advance current watermarks or close actions.
Invalid state is inert and remains unchanged for diagnosis. A latched ledger emits
no classified changes. Callers must inspect `status`/`errors` and clear any pending
interpretation; lack of new data must not be treated as a retained control target.
The ledger has no actuator output or persistence proposal.
## Separate policy and adapter obligations
`DRIVER_INTENT` means a supplied, positively associated, unconsumed cruise change;
it does **not** mean that an SLC override is currently eligible. Both increases
and decreases remain visible. The ordinary above-limit override and qualified
stock-setpoint policies have different rules and must retain that distinction.
They must check current longitudinal authority, mode, display-only state, source
freshness, accepted/effective target, action context and origin eligibility before
using a classified change. Context changes must invalidate old applicability;
an action begun while inactive cannot become eligible merely because its effect
arrived after re-enable.
Offsets, cluster conversions, pedal behavior, suspension/rearm policy and vehicle
button handling are not implemented by this ledger. All speeds at this boundary
must use one documented normalized coordinate. The acceptance helper and tests
establish pure composition, not a complete physical causal association or a fix
deployed in the current controller.
@@ -0,0 +1,75 @@
# SLC composition contract
`composition.step` joins the existing pure source selector, acceptance
reducer, offset/coordinate adapter, override reducer, lead easing helper and
optional `CruiseCeiling`. It never reads Params, changes cruise set speed,
publishes a message or invokes the planner. No runtime caller supplies it yet.
Its `Result` returns every intermediate decision and receipt with immutable
next state, even if a later stage prevents a ceiling.
The caller supplies all four normalized source observations and an explicit
selection policy. Source adapters own eligibility, freshness and geographic or
episode identity. The caller also supplies an offset schedule, current ego
raw/cluster pair stamped to the decision frame, pedal state, qualified lead,
actual host gates and optional action/adoption events. `classified_cruise` must
be the actual same-frame result of a causal ledger cruise-change event. A
stale ledger result or simultaneous acceptance action and cruise effect is
rejected; the event sequencer must resolve or defer the effect. This module
cannot infer physical button association from a speed change.
`HostState.driver_v_cruise_kph` is the single raw selected-speed input. The
composer converts it to m/s once for the domain and override reducers;
`selected_cluster_kph` is a separate display-coordinate measurement. It does
not compare a second raw copy using a tolerance, so float32 CarState precision
cannot create a contradictory duplicate. The uninitialized 255 km/h sentinel
and zero selected speed suppress the cap. The selected and ego pair stamps
represent a caller-qualified common decision frame, not their original
producer timestamps. No universal source or lead TTL is invented.
Frame order is selection, acceptance, offset/domain resolution, override
reduction, one planner-coordinate conversion, lead easing, then an optional
`CruiseCeiling`. The composer derives `SourceEvidence.cap_qualified` only from
a valid domain and positive coordinate; the caller cannot set that Boolean.
Only the two active system-longitudinal modes can emit a ceiling. Stock ACC
ownership has no system cap or stock setpoint command through this API.
Display-only, source uncertainty, absent coordinates, missing driver set
speed, force stop and force deceleration yield no ceiling. A qualified
previous-limit fallback can emit the raw SLC cap without lead easing.
The actual `Authority` is passed unchanged into acceptance and overrides.
When system-longitudinal host flags disagree with that authority, the
composition fails closed, clears lead prior and requires a new session. It
does not run acceptance on that contradictory frame, which avoids crediting
unobserved active time to a pending confirmation. A consistent inactive
authority does run acceptance, so its timer pauses under the reducer's own
rules. A downstream domain failure after acceptance still returns the
acceptance receipt and accepted history in the result; the caller must not
discard an already consumed action disposition merely because no cap was
produced.
Lead prior uses an internal control generation, not accepted-limit identity.
It clears immediately on host loss, mode or owner change, force stop,
force deceleration, missing cap and source loss. A resumed active path starts
a new generation and cannot ease from the old one. The override reducer may
retain selected driver intent through its own short inactivity policy, but
neither that latent intent nor an old ceiling is output while the host gate is
off. Elapsed time comes from monotonic frame timestamps without an invented
maximum gap. Lead or policy errors suppress this composition's ceiling and
clear lead prior, while preserving the returned helper diagnostic.
Scoped tests use actual selector, acceptance, action ledger, override,
coordinate and lead objects. They cover offset/ego delta once, source fallback
and uncertainty, missing coordinates, all four modes and stock ownership,
host-authority disagreement, a consumed acceptance action followed by a
downstream error, rejection through absent-source fallback and reconnection,
reversed monotonic time, causal driver intent, lead easing and continuity reset.
Native tests use real Cap'n Proto messages and compiled MPC on independent
planners over no-cap, ordinary, lead, e2e and force-deceleration transitions;
their ego and selected raw/cluster fields match the composition frame before
the byte snapshot, and they check unchanged input bytes. These establish
software composition, not vehicle qualification or safe tuning.
Physical action/UI association, saved-setting mapping, source and lead
freshness, dashboard/schema production, stock bidirectional setpoint control
and plannerd transport remain separate integration work. Retained source
notices and attribution must be preserved if later runtime code is adapted.
@@ -0,0 +1,56 @@
# Lead-drop easing contract
`lead_relaxation.step` is a pure, optional operation after a qualified SLC cap
has been converted to planner m/s coordinates. It returns a cap proposal and a
`PriorApplied` value for the next call. It does not write cruise settings,
request acceleration, read a lead service or enter the runtime control loop.
The caller passes the actual speed-domain cap only after acceptance, source
freshness, offset and cluster/ego conversion have been qualified. Easing never
adds an offset or changes speed coordinates again.
`cap_qualified=True` must come from a valid speed-domain resolution and its
positive planner-coordinate cap bound to the same acceptance decision. This
module does not infer that qualification from a bare number.
`SourceEvidence` distinguishes `VALID`, `ABSENT`, `UNKNOWN` and `STALE` using
the acceptance observation kind. A valid cap from an explicitly qualified
previous-limit fallback may be `ABSENT`; it passes through raw without easing.
`UNKNOWN`, `STALE`, an unqualified cap, no cap or an inactive control path
returns no contribution and clears the previous target. The caller must mark
force-stop and inactive system-longitudinal paths inactive. It must also rotate
`continuity_id` on mode, longitudinal-owner or force-deceleration discontinuity.
`PriorApplied` binds the previous *output* to drive session and control
continuity, not to accepted-limit identity: an accepted limit drop is exactly
when the prior output may be needed. A mismatched prior returns the qualified
raw cap with an error and clears previous state. Active override returns the
raw cap and clears prior so later release cannot inherit the driver's raised
override target.
`LeadEvidence` explicitly classifies current lead validity. Missing, unknown
or stale lead evidence passes through a qualified raw cap without easing.
Valid evidence includes tracking, lead presence, distance in meters, lead
speed in m/s and signed acceleration in m/s². Malformed evidence, policy or
elapsed time also passes through a qualified raw cap with an error and clears
prior. It never turns an unqualified source into a cap. The helper invents no
source or lead TTL, and a long valid elapsed interval reaches the raw cap.
The caller supplies every `Policy` value. First, a current tracked lead and a
lower target must be present without an active override. Ego speed must meet
the minimum and exceed the raw cap by the configured guard. The lead must be
at least the greater of minimum distance and ego speed times headway; its
speed must not be too far below the new cap; its braking must not exceed the
configured maximum. Then the policy's positive deceleration curve is linearly
interpolated over current overspeed and the output is
`max(raw_cap, prior_cap - deceleration × elapsed_seconds)`. The near-equality
guards, distance thresholds and curve are explicit fixture values, not
fresh-install defaults or validated safe calibration. The frozen source used
a fixed 0.05 s model period and defaulted missing lead fields to zero. This
unit requires typed evidence and caller-supplied elapsed time.
Scoped tests reproduce the frozen two-frame `24.9325 → 24.865` m/s trace from
raw `20`, prior `25`, ego `27` m/s, a far nonbraking lead and the frozen
policy. A 0.1 s step gives the same endpoint within floating-point tolerance.
Tests cover source/lead loss, fallback, close or braking lead, prior continuity,
invalid inputs, elapsed-time bounds and actual native planner/MPC composition.
The native case confirms that lead and e2e braking remain below the cruise
candidate and force deceleration stays authoritative. These are software
contracts, not vehicle qualification.
@@ -0,0 +1,9 @@
# Map source boundary (disabled)
`map_source.py` decodes v2 `mapdOut` events and checks source ordering, boot-time freshness, and structural consistency. V1 remains decodable but cannot produce a candidate because it treated Python GPS Event MONOTONIC timestamps as BOOTTIME. It is not subscribed from plannerd and does not create a valid or absent SLC observation. A matched road can therefore be inspected in isolated tests without becoming a cruise ceiling, accepted-history fallback, or geographic identity claim.
The boundary keeps the GPS observation time separate from computation and output times. V2 converts a Python GPS Event's MONOTONIC timestamp to BOOTTIME only after a post-start/resume fence and clock-pair check; an offset step invalidates queued fixes and advances source generation. External fixes expire after 500 ms, internal fixes after 2 s; computation and output expire after 250 ms. These are transport/input deadlines, not proof that the selected road is correct. Linux callers must provide `CLOCK_BOOTTIME` nanoseconds, matching cereal and mapd output; Apple uses monotonic as the host fallback. The current plannerd live `time.monotonic_ns()` is not interchangeable with boot time after Linux suspend, so no runtime subscription is wired here.
`matchedNoLimit` means a geographically matched sample with no posted numeric limit. It is not malformed and is never turned into SLC `ABSENT` by this disabled boundary. Old pre-v1 records, unknown versions, conflicting status/limit fields, stale evidence, and reordering cannot authorize a map candidate. Source-generation changes clear held evidence. A source change without a generation change, generation regression, boot-clock regression, or excessive restarts fails closed until an explicit tracker reset; late packets from retired producer sessions cannot restore a limit. No state is persisted.
`wayId`, way-selection type, and distance from center do not establish correct-road confidence. The provider can choose a parallel road, ramp, junction branch or wrong direction despite a fresh GPS fix and loaded tile. Real map provenance, tile integrity/freshness, matched-road alternatives, direction and segment applicability, route faults, and restart recovery need separate evidence before this boundary can ever yield a `VALID` or `ABSENT` map observation. Bare `wayId` is not a durable geographic identity.
@@ -0,0 +1,69 @@
# Speed limit override contribution
`overrides.step` is a pure reducer for the system-owned longitudinal SLC target. It
returns a proposed contribution in m/s, not a vehicle command or final planner
target. It requires the actual `acceptance.Decision` from the same session and
monotonic timestamp. The decision carries the authority and policy snapshot used
by acceptance; a mismatch, invalid decision, or inconsistent accepted target is
an error and yields no contribution. `UNKNOWN` and `STALE` observations have no
target. Explicit `ABSENT` may use only the actual acceptance decision's qualified
previous-limit fallback. The selected-speed and pedal observations carry explicit
validity; missing or stale evidence suppresses output. Valid ego speed may be
zero at standstill; selected and accepted limit speeds remain positive.
The reducer issues a `Context` in an eligible prior output. A caller can place
its `context_id` in `BeginAction`. A fresh ledger `ClassifiedChange` can affect
persistence only if it is `DRIVER_INTENT`, has a changed previous speed, matches
that prior context and current selected-speed observation, and originated after
the context was issued while the reducer remained eligible. A selected-speed
number alone cannot arm intent. Same-session effect IDs are watermarked even when
ineligible, including while display-only. `EventReceipt.acknowledged` means the
event was observed; `consumed` means qualified intent changed override state.
Automatic, unresolved, consumed, foreign, replayed and completed effects never
arm persistence. The action ledger supplies causal classification; the override
reducer never guesses a physical button association or time window.
The reducer rotates context on accepted numeric target change, temporary
inactivity and re-enable, mode or owner change, and each new consumed adoption.
An accepted identity or provider change at the same numeric speed retains both
intent and context. This policy uses the numeric accepted target as the override
boundary; acceptance still owns candidate identity and rejection history. A new
lower accepted target clears persistence before processing the event or emitting
output. A higher target preserves it until the target reaches the selected speed.
Pending and rejected candidates leave the accepted target unchanged. Adoption
clears persistence once per action sequence and invalidates previous contexts;
the current pedal is then evaluated normally. A delayed reconciliation effect
remains classified `SLC_CONSUMED` by the ledger.
The current valid pedal press contributes the current ego speed when above the
accepted target, temporarily preceding persistent intent. Releasing the pedal
reveals retained persistence. Pedal speed is never latched. Temporary inactive
system-longitudinal authority suppresses output immediately and invalidates the
origin context. Persistent intent remains latent for less than 750 ms in the same
mode and owner; at or after 750 ms it clears before a resumed output. Mode or
owner changes clear immediately. Display-only never contributes or consumes an
effect. Stock setpoint buttons are a separate capability and output domain.
Malformed evidence, a reversed clock, or an invalid acceptance snapshot clears
intent and origin context, then latches `reset_required`. Further calls in that
state remain inert. Recovery requires `new_session` with a distinct session ID
and a fresh acceptance/ledger lifecycle; old effects belong to the prior session
and cannot arm the new one. A malformed `State` itself returns an explicit error
without trusting or modifying its fields.
This unit assumes normalized m/s inputs and supplies no source TTL, Params,
CAN, or vehicle adapter behavior. The accepted decision is still the gate:
`control_target_mps=None` cannot be bypassed by stored intent.
An optional qualified `speed_domain.DomainResolution` now supplies an effective
limit threshold and same-frame raw/cluster deltas. Without it, the existing
zero-offset identity-coordinate behavior remains. With it, the reducer compares
driver-selected and pedal speeds in cluster coordinates against accepted limit
plus offset, while retaining causal selected intent in raw m/s. A lower raw
accepted limit still clears intent; a higher effective threshold clears intent
when it catches the converted retained value. Offset decrease alone cannot arm
intent. An effective-threshold change rotates action context, while cluster
delta jitter alone does not. Unknown or stale coordinate evidence suppresses
contribution; malformed or mismatched context latches the existing session
reset contract. [Speed domain](SPEED_DOMAIN.md) describes construction and
planner-coordinate conversion.
+251
View File
@@ -0,0 +1,251 @@
# Speed-limit decisions
This package combines decision policies with the SLC runtime in card and
plannerd. `feature_runtime` starts optional owners from saved preferences and
finalized vehicle capabilities. Each owner separately checks current source,
clock and control authority. Replay flags remain available for isolated tests.
SLC defaults off. Supported dashboard parsers provide OEM observations when the
vehicle supplies the required messages. Missing, stale and explicitly absent
observations remain distinct. Map, navigation and online providers are not yet
connected as control sources.
Vision uses the pinned camera-sign models when Vision is selected and either
Speed Limit Controller or Show Speed Limits is on. Registered Ioniq 6 and GM
sessions can display camera signs. Speed control additionally requires a
registered system-longitudinal owner, an enabled SLC preference and fresh driver
confirmation for each new sign episode or changed limit. Factory-cruise and
display-only sessions never gain speed authority. Source loss clears an accepted
limit; old dashboard episodes are never adopted as new-drive observations.
Saved preferences are decoded strictly before use. A malformed Boolean, source,
number or unreadable setting disables SLC without rewriting the saved bytes.
Only missing settings use defaults; corruption cannot silently remove confirmation
or change speed units. Without an adopted offset document, the seven legacy
`Offset*` values still use the saved display unit for conversion and band
selection. The final band ends at 44.2 m/s (imperial) or 38.9 m/s (metric);
the offset is zero at and above that boundary, matching the frozen controller.
The earlier open-ended repeat of `Offset7` had no reviewed rationale and was a
migration discrepancy.
An optional versioned `SLCOffsetSchedule` document holds seven signed SI offsets
and eight SI band boundaries. Its tail is always zero. When present and valid,
it is the only numeric offset authority: old `Offset*` bytes and `IsMetric` are
ignored for control, even if malformed. A malformed document or the explicit
`needs_review` state disables SLC without falling back to legacy values or
rewriting the saved control bit. The document helper exposes strict decoding,
serialization and pure adoption of a currently valid legacy schedule. Native
settings UI adoption and display-unit writes are a separate pending integration;
this backend does not write Params.
All transport additions use cereal's reserved custom messages. The official
vehicle schema is unchanged. Slot 9 retains its six historical model-status fields
at ordinals 0–5 and appends SLC state at 6–30. Slots 12–15 carry UI actions,
physical receipts, dashboard observations and one-shot cruise commands. Historical
custom slots already used by the fork remain reserved for compatible restoration.
Earlier unreleased Domathon SLC development recordings require their exact schema
for replay; this layout does not reinterpret them. It also does not convert
recordings made with older divergent vehicle schemas.
The Vision envelope restores the historical `StarPilotModelDataV2` at Event
ordinal 111 with its original turn-direction field and enum, then appends an
independent typed Vision payload. The old ordinal 137 remains reserved and empty.
## Camera sign source
Manager starts the optional onroad VisionIPC producer and plannerd subscribes to
`slcVisionObservation` for an admitted saved source choice. The producer uses
the pinned US detector/classifier ONNX files documented in
[`vision/assets/PROVENANCE.md`](vision/assets/PROVENANCE.md). It reads camerad's
road or wide-road NV12 stream, copies and releases each VisionIPC buffer, and
publishes a typed observation. Source selection does not change stock engagement.
The runtime distinguishes source availability, display permission and speed
control permission. A display-only source can populate the sign widget but
cannot create a planner ceiling or cruise command. Controlling a detected limit
requires current longitudinal authority and matching driver confirmation, even
if general SLC confirmation is off. Unqualified replay observations remain
isolated from normal speed control.
Camera EOF uses CLOCK_BOOTTIME; Python receipt and inference completion use
CLOCK_MONOTONIC. The adapter checks paired clocks, including after suspend,
without assuming a fixed offset. Candidate support expires after two seconds;
each message must also be within 250 ms of its captured frame. Camera reconnects
reset sign support and producer identity. Runtime and source tests do not establish
sign-reading accuracy or onroad timing under load.
`selection.select_limit` supplies this reducer with configured source observations.
It preserves priority order, highest/lowest selection, explicit Vision inclusion
and online-only fallback. The [selection contract](SELECTION.md) describes its
eligibility threshold, uncertainty handling and separate adapter obligations.
## Acceptance contract
Call `new_session(unique_session_id, qualified_accepted_history)` once for an
explicit session, then pass each returned `Decision.state` into `step`. A session
ID must not be reused; creating a new state resets rejection and action history.
History supplied by a caller needs a separate schema, origin, and applicability
check. This module neither loads nor qualifies stored history. `history_write` is
an optional proposal produced only by an acceptance; persistence remains outside
the reducer. Its timestamp is session-relative monotonic nanoseconds, not UTC or
a source freshness measurement.
The four observation kinds have different meanings:
| Kind | Caller assertion | Accepted-history fallback |
| --- | --- | --- |
| `VALID` | A source has an eligible, normalized candidate | Previous accepted limit is held while a new candidate awaits a decision or remains rejected |
| `ABSENT` | Eligible sources explicitly have no current candidate | Only when `fallback_previous` is enabled |
| `UNKNOWN` | Evidence is incomplete or cannot be classified | No control target |
| `STALE` | Available evidence fails the caller's freshness contract | No control target |
Only `VALID` carries a `Candidate(source, observation_identity, speed_mps)`.
`ObservationIdentity` explicitly distinguishes three forms:
| Kind | Required identity | Meaning |
| --- | --- | --- |
| `GEOGRAPHIC` | Nonempty `value`, no session field | Source-supplied road/zone identity |
| `PRODUCER_EPISODE` | Nonempty `value`, no session field | Source-supplied stable observation episode |
| `SESSION_VALUE` | Current `session_id`, no free-form value | Source and exact normalized speed within this session; no geographic claim |
The same source, typed identity and exact normalized speed form the candidate
identity. A dashboard producer without road identity can use `SESSION_VALUE`.
It remains stable across dropout and provider switching. Rejected dashboard 45
therefore remains rejected for this session even if a later 45 belongs to another
road; explicit adoption is available to accept it. Do not invent new episode IDs
on signal loss to evade rejection. Geographic or episode IDs require independent
source evidence; selecting a tag does not establish that evidence.
Values must be finite and positive; no comparison epsilon or universal source TTL
is hidden in the reducer. Unit conversion, meaningful rounding, source eligibility
and freshness belong to adapters. `observation_is_valid` exposes structural
validation; its optional `session_id` enforces session-value applicability.
The reducer always supplies its current session. `new_session` rejects prior-session
session-value history without silently rekeying it. Independently qualified
geographic/episode history can retain its original accepted-session metadata.
Rejections are not global cross-source equivalence claims. A rejected dashboard
limit that appears from a map is a different candidate, even with the same identity
string and numeric speed. It still follows directional confirmation relative to
the last accepted limit. Returning to the original rejected identity stays
rejected. Cross-provider canonical road identity has not been implemented.
State keeps current observation, pending decision, rejected candidates and last
accepted history separately. A rejection never updates accepted history. Source
loss, reconnection and intervening sources retain rejections. A changed identity or
speed is a new candidate. Rejection storage is bounded at 256 distinct candidates;
attempting another rejection latches all control output off until an explicit new
session. It never evicts a rejection and later silently accepts that candidate.
## Authority and confirmation
`Authority` supplies the selected mode (off, lateral-only, longitudinal-only or
combined), longitudinal owner (none, system or stock), each effective host axis,
stock ACC activity, and explicit full disengagement. Contradictory combinations
fail closed. Selected mode and assigned owner can remain set while inactive.
Full disengagement requires both host axes and stock ACC to be inactive; inactive
axes alone do not imply full disengagement.
A control target or consumed driver action requires active system longitudinal
authority in longitudinal-only or combined mode. Stock ACC never gains a target
through this reducer. Explicit full disengagement accepts an eligible candidate
without a button or control output, unless that candidate was rejected. With no
longitudinal authority and no full disengagement, history is preserved and any
required confirmation is paused. `display_only` shows a valid candidate but emits
no target, acceptance, fallback or consumed action, and clears pending confirmation.
Lower and higher limits have independent confirmation settings. The first limit
uses `confirm_higher` as an explicit initial-limit policy. Exactly equal speeds
need no directional confirmation even if source/identity changes; accepting the new
identity can still propose a history update. Adjacent representable unequal
floating-point values are changes and follow the corresponding direction policy.
Confirmation expires after 30 seconds of accumulated authoritative active-long
time. An interval counts only when the same pending decision exists and both
endpoint samples have active longitudinal authority. A pause/resume boundary does
not count the uncertain transition interval. Callers must deliver authority
transitions; two active endpoint samples cannot reveal an omitted inactive period.
Equal timestamps are allowed; negative, non-integer or reversed clocks fail
closed, clear the prompt and preserve history/rejections. A valid long gap is
counted, because freshness has already been classified by the caller.
This accumulated timer, uncertainty suppression, stable rejection retention and
authority-gated button handling are explicit design choices requiring integration
review. They are not claims of exact prior-runtime behavior.
## Driver actions and outputs
Each action includes session ID, monotonic sequence ID, pending decision ID and
accept/reject kind. A new same-session sequence is watermarked even when an action
is unrelated, ineligible, or another input is invalid. A foreign session cannot
advance the current sequence watermark. Repeated or older events are acknowledged
but never consumed. A decision must have appeared in the previous state; an action
cannot guess a newly created prompt ID. Loss or change of the candidate creates a
new decision ID. An eligible driver action at the timeout boundary takes priority.
`ActionReceipt.acknowledged` means an event was identified; only `consumed=True`
means SLC handled that input. The adapter must not swallow ordinary cruise actions
based on acknowledgment or watermarking alone. Malformed actions produce an input
error and no receipt. The runtime arbitrates identified physical receipts and UI
requests. The live UI action publisher is not yet connected.
## Explicit adoption
`AdoptRequest(session_id, sequence_id, presentation_id, candidate)` is separate from
pending confirmation. It can accept the currently displayed candidate after a
rejection. Each valid observation produces `state.presentation`; its ID remains
stable until the candidate changes, disappears or an invalid input clears the
context. An adoption must match the exact candidate and a presentation from the
prior reducer output. Guessing the ID of a same-frame arrival is ineligible.
Adoption requires active system longitudinal authority, ordinary control policy,
and an unexhausted session. It removes only the matching rejection, accepts that
candidate, clears pending confirmation, and emits an `AdoptionProposal` with an
explicit action sequence and presentation ID. The proposal asks the composition
and runtime adapters to clear override intent and reconcile cruise selection.
The acceptance reducer itself performs neither action. An already accepted
candidate still produces this proposal, but need not produce a duplicate history
write. The initial proposed
speed has no offset or cluster conversion; those require a later effective-target
adapter bound to the same accepted context.
`adoption_receipt` distinguishes acknowledgment from consumption. Adoption and
confirmation share one sequence watermark. Supplying both in one step is an
ambiguous input error: neither is consumed, no output/effect is proposed, and
valid same-session IDs are watermarked. Replays, foreign sessions, changed or lost
presentations, display-only mode and missing longitudinal authority cannot produce
an adoption effect. Exhausted rejection storage requires a new session; adoption
cannot bypass its latch. Eligible adoption precedes confirmation timeout handling.
An ineligible adoption does not disable independent observation processing. For
example, the existing fully-disengaged autoaccept rule can still record a new,
unrejected observation, while its adoption receipt remains unconsumed and no
reconciliation proposal is emitted. Callers must inspect the receipt/proposal,
not mistake a history update for successful adoption.
The [causal action ledger](ACTIONS.md) preserves ownership across delayed effects.
The runtime binds it to physical receipts and the override policy. Card consumes
a qualified confirmation on press and suppresses its release and held repeats.
If a cached prompt cannot be qualified, the ordinary cruise-button path handles
the press. A consumed press is never replayed later.
Only card changes its software-owned selected speed. A higher accepted limit or
identified adoption may produce a one-shot command. Card requires matching
session, source episode, presentation, current selected speed and active system
longitudinal control, with bounded message age and no unrelated button action.
Stock PCM cruise is excluded. Applied and rejected receipts are separate from
limit acceptance; accepting a limit does not prove a selected-speed change.
`control_target_mps` is optional decision data, never a cruise or actuator command.
A missing target must clear any adapter's previous SLC contribution; callers must
not retain an old target on an error. `errors` is explicit and history proposals
are absent on errors. Invalid state remains inert until the caller replaces it
with valid state through an explicit lifecycle decision.
## Remaining integration
Map lookahead, Vision control qualification, online provider permissions and their actual
producers remain separate work. The live UI action publisher, stock-button speed
control, replay of recorded vehicle routes and device timing are also outstanding.
Stock ACC ownership does not grant permission to change its setpoint through this
API. Runtime tests and source compatibility checks are evidence for their scoped
contracts, not whole-feature migration or fleet safety.
@@ -0,0 +1,51 @@
# Source selection contract
`selection.select_limit` chooses an observation for the acceptance reducer. It
does not accept a speed, command a car, request an online provider, read settings
or judge sensor health. There is no production adapter yet.
Each of dashboard, map, vision and online must supply an explicit `Observation`.
Source adapters own normalized m/s, typed observation identity, freshness and eligibility.
For example, vision support and its separate display/control filters must be
resolved before this boundary. A missing producer is unknown; it is not proof
that no limit was observed. The selector invents no expiry period or road identity.
Geographic, producer-episode and session-value identities pass through unchanged.
Selection checks their structure but owns no drive session; the acceptance reducer
must verify session-value applicability against its current session before use.
`SelectionPolicy` has an explicit mode, exactly two primary source slots and an
online-fallback Boolean. A slot may be empty or repeat another slot. Online is
never a primary slot. Saved UI labels need a separately reviewed settings adapter;
this module does not activate or translate preferences.
| Mode | Primary candidates | Choice |
| --- | --- | --- |
| Ordered | Only the configured slots | First eligible valid observation |
| Highest | Dashboard and map; vision only if present in either slot | Highest eligible value |
| Lowest | Dashboard and map; vision only if present in either slot | Lowest eligible value |
Equal extrema choose dashboard, then map, then vision, independent of input map
iteration order. The minimum eligible limit is explicitly 1 m/s, inclusive.
Smaller positive observations are reported in `below_minimum` and excluded;
zero, negative, nonfinite or malformed valid candidates are input errors.
The online observation is considered only if no configured primary is eligible
and fallback is enabled. It cannot override a valid primary. An eligible
secondary or online observation can replace an unavailable primary; all
considered unavailable sources remain in the result. Disabled and unconfigured
sources cannot create uncertainty in that choice.
If no eligible alternative exists, the result retains unknown or stale state.
Unknown takes precedence over stale, which takes precedence over absence; the
full per-source statuses remain available. Explicit absence, exclusion below the
minimum or an empty configured source set produces an absent result. This distinction prevents the
acceptance reducer from treating discarded stale data as permission for a
previous-limit fallback. Malformed input produces `invalid_input`, errors and
an unknown observation without selecting a source.
The tests include composition through the acceptance reducer: accepted 65 mph,
proposed 45, driver rejection, then source loss and reconnect. Last accepted
history remains 65 for both longitudinal-only and combined authority. Unknown
or stale sources suppress that fallback, and lateral-only or stock ownership
cannot turn a selected speed into a system longitudinal target. These are
deterministic policy tests, not native producer, planner, CAN or vehicle evidence.
@@ -0,0 +1,41 @@
# SLC offset and speed coordinates
`speed_domain.resolve` binds an actual acceptance decision to an explicit offset
schedule and current raw/cluster speed pairs. Every number is m/s except the
input to `selected_pair_from_kph`, which converts a selected cluster value from
km/h. Each valid pair carries the same session ID and monotonic nanosecond
timestamp as the acceptance decision; a mismatch is invalid. The schedule
contains explicit half-open bands with signed m/s offsets;
it supplies no settings defaults or source freshness policy. A missing band or
unavailable target/pair produces `UNAVAILABLE`; malformed values produce
`INVALID`. `SpeedPair` contains measured cluster speed, which may be below raw
speed; resolution clamps each delta to `max(cluster - raw, 0)`. A nonvalid
resolution with a context is malformed and triggers the override session-reset
contract. Only a `VALID` context may enter `overrides.step(domain=...)`.
Call order is acceptance, domain resolution, override reduction, then
`to_planner_coordinate(context, contribution_raw_mps, basis)`. The reducer
compares current selected and retained intent against `accepted raw + offset`
in cluster coordinates. It keeps retained intent in raw selected-speed units,
even when the current selected value later changes for an unrelated reason.
The conversion adds the *current qualified selected cluster delta* to that
retained raw speed. A current pedal contribution uses the current ego cluster
delta. The larger of limit plus offset and override is converted to planner
coordinates by subtracting the ego cluster delta exactly once. A nonpositive
result gives no cap. The host may then build the optional
`CruiseCeiling(speed_mps, authority)`; the native planner still owns its driver
ceiling and lead, e2e, force-deceleration and acceleration arbitration.
This is a pure integration contract. No caller activates it, and it has no
Params access, UI action, stock setpoint command, source TTL, lead relaxation
or vehicle output. Stock bidirectional selection needs a separate owner and
output domain. The frozen implementation compared raw ego speed with an
offset-adjusted threshold, then used cluster ego speed as the override. This
unit deliberately compares both values in the same cluster coordinate. It
prevents a pedal override from arming when raw ego speed crosses one coordinate
while the effective cluster threshold is in another.
The tests compose actual acceptance and causal override decisions, including
retained raw intent with a changed current selected value, offset changes,
negative offsets, missing coordinates, and one native planner/MPC case. These
are deterministic software contracts, not on-road qualification.
@@ -0,0 +1,12 @@
"""Pure speed-limit decisions; runtime and source adapters are separate contracts."""
from openpilot.starpilot.speed_limits.acceptance import (
Candidate,
IdentityKind,
Observation,
ObservationIdentity,
ObservationKind,
observation_is_valid,
)
__all__ = ["Candidate", "IdentityKind", "Observation", "ObservationIdentity", "ObservationKind", "observation_is_valid"]
@@ -0,0 +1,453 @@
"""Immutable acceptance decisions with explicit authority and no runtime I/O."""
import math
from dataclasses import dataclass, replace
from enum import Enum
CONFIRMATION_NS = 30_000_000_000
MAX_REJECTIONS = 256
class ObservationKind(Enum):
VALID = "valid"
ABSENT = "absent"
UNKNOWN = "unknown"
STALE = "stale"
class IdentityKind(Enum):
GEOGRAPHIC = "geographic"
PRODUCER_EPISODE = "producer_episode"
SESSION_VALUE = "session_value"
@dataclass(frozen=True)
class ObservationIdentity:
kind: IdentityKind
value: str | None = None
session_id: str | None = None
@dataclass(frozen=True)
class Candidate:
source: str
observation_identity: ObservationIdentity
speed_mps: float
@dataclass(frozen=True)
class Observation:
kind: ObservationKind
candidate: Candidate | None = None
class Mode(Enum):
OFF = "off"
LATERAL_ONLY = "lateral_only"
LONGITUDINAL_ONLY = "longitudinal_only"
COMBINED = "combined"
class LongitudinalOwner(Enum):
NONE = "none"
SYSTEM = "system"
STOCK = "stock"
@dataclass(frozen=True)
class Authority:
mode: Mode
owner: LongitudinalOwner
lateral_active: bool
longitudinal_active: bool
stock_acc_active: bool
fully_disengaged: bool
@property
def can_control(self) -> bool:
return (self.mode in (Mode.LONGITUDINAL_ONLY, Mode.COMBINED) and
self.owner is LongitudinalOwner.SYSTEM and self.longitudinal_active)
@dataclass(frozen=True)
class Policy:
confirm_lower: bool = True
confirm_higher: bool = True
fallback_previous: bool = False
display_only: bool = False
vision_driver_confirm: bool = False
class ActionKind(Enum):
ACCEPT = "accept"
REJECT = "reject"
@dataclass(frozen=True)
class DriverAction:
session_id: str
sequence_id: int
decision_id: int
kind: ActionKind
@dataclass(frozen=True)
class AdoptRequest:
session_id: str
sequence_id: int
presentation_id: int
candidate: Candidate
@dataclass(frozen=True)
class ActionReceipt:
sequence_id: int
acknowledged: bool
consumed: bool
status: str
@dataclass(frozen=True)
class AcceptedLimit:
candidate: Candidate
session_id: str
accepted_at_ns: int
@dataclass(frozen=True)
class PendingDecision:
candidate: Candidate
decision_id: int
active_elapsed_ns: int = 0
@dataclass(frozen=True)
class Presentation:
presentation_id: int
candidate: Candidate
@dataclass(frozen=True)
class AdoptionProposal:
session_id: str
action_sequence_id: int
presentation_id: int
accepted: AcceptedLimit
reconciliation_speed_mps: float
clear_override: bool = True
@dataclass(frozen=True)
class State:
session_id: str
observation: Observation = Observation(ObservationKind.ABSENT)
accepted: AcceptedLimit | None = None
pending: PendingDecision | None = None
rejected: frozenset[Candidate] = frozenset()
last_timestamp_ns: int | None = None
timer_running: bool = False
last_action_sequence: int = -1
next_decision_id: int = 1
rejection_capacity_exhausted: bool = False
presentation: Presentation | None = None
next_presentation_id: int = 1
@dataclass(frozen=True)
class Decision:
state: State
display_candidate: Candidate | None = None
control_target_mps: float | None = None
basis: str = "none"
history_write: AcceptedLimit | None = None
action_receipt: ActionReceipt | None = None
errors: tuple[str, ...] = ()
adoption_receipt: ActionReceipt | None = None
adoption: AdoptionProposal | None = None
authority: Authority | None = None
policy: Policy | None = None
def _token(value: object) -> bool:
return type(value) is str and 0 < len(value) <= 256 and bool(value.strip()) and not any(ord(c) < 32 for c in value)
def _integer(value: object, minimum: int = 0) -> bool:
return type(value) is int and value >= minimum
def _identity_valid(identity: object) -> bool:
if not isinstance(identity, ObservationIdentity) or not isinstance(identity.kind, IdentityKind):
return False
if identity.kind is IdentityKind.SESSION_VALUE:
return identity.value is None and _token(identity.session_id)
return _token(identity.value) and identity.session_id is None
def _candidate_valid(candidate: object, session_id: str | None = None) -> bool:
if not isinstance(candidate, Candidate) or not _token(candidate.source) or not _identity_valid(candidate.observation_identity):
return False
identity = candidate.observation_identity
if session_id is not None and identity.kind is IdentityKind.SESSION_VALUE and identity.session_id != session_id:
return False
try:
return type(candidate.speed_mps) in (int, float) and math.isfinite(candidate.speed_mps) and candidate.speed_mps > 0
except OverflowError:
return False
def observation_is_valid(observation: object, *, session_id: str | None = None) -> bool:
"""Check the shared candidate contract without choosing a source or freshness policy."""
return isinstance(observation, Observation) and isinstance(observation.kind, ObservationKind) and (
_candidate_valid(observation.candidate, session_id) if observation.kind is ObservationKind.VALID else observation.candidate is None)
def _accepted_valid(accepted: object) -> bool:
return (isinstance(accepted, AcceptedLimit) and _candidate_valid(accepted.candidate, accepted.session_id) and
_token(accepted.session_id) and _integer(accepted.accepted_at_ns))
def _authority_errors(authority: object) -> list[str]:
if not isinstance(authority, Authority) or not isinstance(authority.mode, Mode) or not isinstance(authority.owner, LongitudinalOwner):
return ["invalid authority type"]
if any(type(v) is not bool for v in (authority.lateral_active, authority.longitudinal_active,
authority.stock_acc_active, authority.fully_disengaged)):
return ["authority flags must be booleans"]
errors = []
if authority.lateral_active and authority.mode not in (Mode.LATERAL_ONLY, Mode.COMBINED):
errors.append("active lateral axis contradicts mode")
if authority.longitudinal_active and (authority.mode not in (Mode.LONGITUDINAL_ONLY, Mode.COMBINED) or
authority.owner is not LongitudinalOwner.SYSTEM):
errors.append("active longitudinal axis contradicts mode or owner")
if authority.stock_acc_active and authority.owner is not LongitudinalOwner.STOCK:
errors.append("active stock ACC contradicts owner")
if authority.fully_disengaged and (authority.lateral_active or authority.longitudinal_active or authority.stock_acc_active):
errors.append("fully disengaged contradicts active authority")
return errors
def authority_is_valid(authority: object) -> bool:
"""Validate the shared mode, owner and activity contract."""
return not _authority_errors(authority)
def _state_errors(state: State) -> list[str]:
errors = []
if not isinstance(state, State):
return ["invalid state type"]
if not _token(state.session_id) or not observation_is_valid(state.observation, session_id=state.session_id):
errors.append("invalid state identity or observation")
if state.accepted is not None and (not _accepted_valid(state.accepted) or not _candidate_valid(state.accepted.candidate, state.session_id)):
errors.append("invalid accepted history")
if state.last_timestamp_ns is not None and not _integer(state.last_timestamp_ns):
errors.append("invalid state clock")
if not _integer(state.last_action_sequence, -1) or not _integer(state.next_decision_id, 1) or not _integer(state.next_presentation_id, 1):
errors.append("invalid state sequence")
if type(state.timer_running) is not bool or type(state.rejection_capacity_exhausted) is not bool:
errors.append("invalid state flags")
rejections_valid = (type(state.rejected) is frozenset and len(state.rejected) <= MAX_REJECTIONS and
all(_candidate_valid(c, state.session_id) for c in state.rejected))
if not rejections_valid:
errors.append("invalid rejection history")
elif state.accepted is not None and _accepted_valid(state.accepted) and state.accepted.candidate in state.rejected:
errors.append("accepted candidate is also rejected")
if state.pending is not None:
pending = state.pending
if (not isinstance(pending, PendingDecision) or not _candidate_valid(pending.candidate) or
not _integer(pending.decision_id, 1) or not _integer(pending.active_elapsed_ns) or
not _integer(state.next_decision_id, 1) or pending.decision_id >= state.next_decision_id or
state.observation != Observation(ObservationKind.VALID, pending.candidate)):
errors.append("invalid pending decision")
elif rejections_valid and pending.candidate in state.rejected:
errors.append("pending candidate is already rejected")
if state.timer_running and (state.pending is None or state.last_timestamp_ns is None):
errors.append("timer lacks pending decision or clock")
if state.presentation is not None:
shown = state.presentation
if (not isinstance(shown, Presentation) or not _integer(shown.presentation_id, 1) or
not _integer(state.next_presentation_id, 1) or shown.presentation_id >= state.next_presentation_id or
not _candidate_valid(shown.candidate, state.session_id) or state.last_timestamp_ns is None or
state.observation != Observation(ObservationKind.VALID, shown.candidate)):
errors.append("invalid presentation context")
elif isinstance(state.observation, Observation) and state.observation.kind is ObservationKind.VALID:
errors.append("valid observation lacks presentation context")
return errors
def new_session(session_id: str, accepted: AcceptedLimit | None = None) -> State:
"""Explicitly reset transient state; a caller may supply independently qualified history."""
state = State(session_id=session_id, accepted=accepted)
errors = _state_errors(state)
if errors:
raise ValueError("; ".join(errors))
return state
def _action_valid(action: object) -> bool:
return (isinstance(action, DriverAction) and _token(action.session_id) and _integer(action.sequence_id) and
_integer(action.decision_id, 1) and isinstance(action.kind, ActionKind))
def _adopt_valid(adopt: object) -> bool:
return (isinstance(adopt, AdoptRequest) and _token(adopt.session_id) and _integer(adopt.sequence_id) and
_integer(adopt.presentation_id, 1) and _candidate_valid(adopt.candidate, adopt.session_id))
def _acknowledge(state: State, action: DriverAction | AdoptRequest | None) -> tuple[State, ActionReceipt | None]:
if action is None:
return state, None
if action.session_id != state.session_id:
return state, ActionReceipt(action.sequence_id, True, False, "foreign_session")
if action.sequence_id <= state.last_action_sequence:
return state, ActionReceipt(action.sequence_id, True, False, "already_seen")
return replace(state, last_action_sequence=action.sequence_id), ActionReceipt(action.sequence_id, True, False, "seen")
def _accept(state: State, candidate: Candidate, now_ns: int) -> tuple[State, AcceptedLimit | None]:
if state.accepted is not None and state.accepted.candidate == candidate:
return replace(state, pending=None, timer_running=False), None
accepted = AcceptedLimit(candidate, state.session_id, now_ns)
return replace(state, accepted=accepted, pending=None, timer_running=False), accepted
def _reject(state: State, candidate: Candidate) -> State:
if candidate not in state.rejected and len(state.rejected) == MAX_REJECTIONS:
return replace(state, pending=None, timer_running=False, rejection_capacity_exhausted=True)
return replace(state, rejected=state.rejected | {candidate}, pending=None, timer_running=False)
def step(state: State, observation: Observation, authority: Authority, policy: Policy, *, now_ns: int,
action: DriverAction | None = None, adopt: AdoptRequest | None = None) -> Decision:
"""Return a decision, never a cruise command. Invalid input produces no control target."""
state_errors = _state_errors(state)
if state_errors:
return Decision(state, errors=tuple(state_errors))
errors = _authority_errors(authority)
if not observation_is_valid(observation, session_id=state.session_id):
errors.append("invalid observation")
if not isinstance(policy, Policy) or any(type(v) is not bool for v in (
policy.confirm_lower, policy.confirm_higher, policy.fallback_previous, policy.display_only,
policy.vision_driver_confirm)):
errors.append("invalid policy")
if not _integer(now_ns) or (state.last_timestamp_ns is not None and now_ns < state.last_timestamp_ns):
errors.append("invalid or reversed monotonic clock")
if action is not None and not _action_valid(action):
errors.append("invalid driver action")
if adopt is not None and not _adopt_valid(adopt):
errors.append("invalid adopt request")
if action is not None and adopt is not None:
errors.append("ambiguous action and adopt request")
if errors:
# Do not defer a stale prompt or credit the invalid interval on recovery.
invalid = replace(state, observation=Observation(ObservationKind.UNKNOWN), pending=None, timer_running=False, presentation=None)
# A valid same-session event is seen even when another input makes it ineligible.
invalid, receipt = _acknowledge(invalid, action if _action_valid(action) else None)
invalid, adopt_receipt = _acknowledge(invalid, adopt if _adopt_valid(adopt) else None)
if receipt is not None and receipt.status == "seen":
receipt = replace(receipt, status="invalid_input")
if adopt_receipt is not None and adopt_receipt.status == "seen":
adopt_receipt = replace(adopt_receipt, status="invalid_input")
return Decision(invalid, action_receipt=receipt, adoption_receipt=adopt_receipt, errors=tuple(errors))
old = state
state, receipt = _acknowledge(replace(state, observation=observation, last_timestamp_ns=now_ns), action)
state, adopt_receipt = _acknowledge(state, adopt)
candidate = observation.candidate
if candidate is None:
state = replace(state, presentation=None)
elif old.presentation is None or old.presentation.candidate != candidate:
state = replace(state, presentation=Presentation(state.next_presentation_id, candidate), next_presentation_id=state.next_presentation_id + 1)
def finish(basis: str, *, target: float | None = None, history: AcceptedLimit | None = None,
adoption: AdoptionProposal | None = None) -> Decision:
final_receipt = replace(receipt, status=basis) if receipt is not None and receipt.status == "seen" else receipt
final_adopt_receipt = replace(adopt_receipt, status=basis) if adopt_receipt is not None and adopt_receipt.status == "seen" else adopt_receipt
return Decision(state, candidate, target, basis, history, final_receipt, adoption_receipt=final_adopt_receipt,
adoption=adoption, authority=authority, policy=policy)
def previous_target() -> float | None:
return state.accepted.candidate.speed_mps if state.accepted is not None and authority.can_control else None
if state.rejection_capacity_exhausted:
state = replace(state, pending=None, timer_running=False)
return finish("session_reset_required")
if policy.display_only:
state = replace(state, pending=None, timer_running=False)
return finish("display_only")
if observation.kind is not ObservationKind.VALID:
state = replace(state, pending=None, timer_running=False,
accepted=None if policy.vision_driver_confirm and state.accepted is not None and
state.accepted.candidate.source == 'vision' else state.accepted)
target = previous_target() if observation.kind is ObservationKind.ABSENT and policy.fallback_previous else None
return finish("previous_accepted" if target is not None else observation.kind.value, target=target)
assert candidate is not None
vision_confirm = policy.vision_driver_confirm and candidate.source == 'vision'
if (policy.vision_driver_confirm and state.accepted is not None and
state.accepted.candidate.source == 'vision' and state.accepted.candidate != candidate):
state = replace(state, accepted=None)
if adopt_receipt is not None and adopt_receipt.status == "seen":
assert adopt is not None
if (old.presentation is None or old.presentation.presentation_id != adopt.presentation_id or
old.presentation.candidate != candidate or adopt.candidate != candidate):
adopt_receipt = replace(adopt_receipt, status="different_presentation")
elif not authority.can_control:
adopt_receipt = replace(adopt_receipt, status="no_longitudinal_authority")
else:
state, history = _accept(replace(state, rejected=state.rejected - {candidate}), candidate, now_ns)
assert state.accepted is not None
adopt_receipt = replace(adopt_receipt, consumed=True, status="adopt")
proposal = AdoptionProposal(state.session_id, adopt.sequence_id, adopt.presentation_id, state.accepted, candidate.speed_mps)
return finish("driver_adopt", target=candidate.speed_mps, history=history, adoption=proposal)
if candidate in state.rejected:
state = replace(state, pending=None, timer_running=False)
return finish("rejected", target=previous_target())
if authority.fully_disengaged and not vision_confirm:
state, history = _accept(state, candidate, now_ns)
return finish("disengaged_accept", history=history)
if vision_confirm and state.accepted is not None and state.accepted.candidate == candidate:
return finish("driver_confirmed_vision", target=candidate.speed_mps if authority.can_control else None)
previous_speed = state.accepted.candidate.speed_mps if state.accepted is not None else None
confirmation = True if vision_confirm else policy.confirm_higher if previous_speed is None or candidate.speed_mps > previous_speed else (
policy.confirm_lower if candidate.speed_mps < previous_speed else False)
if not confirmation:
if authority.can_control:
state, history = _accept(state, candidate, now_ns)
return finish("accepted", target=candidate.speed_mps, history=history)
state = replace(state, pending=None, timer_running=False)
return finish("no_longitudinal_authority")
if old.pending is not None and old.pending.candidate == candidate:
pending = old.pending
if old.timer_running and authority.can_control:
assert old.last_timestamp_ns is not None
pending = replace(pending, active_elapsed_ns=pending.active_elapsed_ns + now_ns - old.last_timestamp_ns)
else:
pending = PendingDecision(candidate, state.next_decision_id)
state = replace(state, next_decision_id=state.next_decision_id + 1)
state = replace(state, pending=pending, timer_running=authority.can_control)
if receipt is not None and receipt.status == "seen":
assert action is not None
if action.decision_id != pending.decision_id:
receipt = replace(receipt, status="different_decision")
elif old.pending is None or old.pending.decision_id != pending.decision_id:
receipt = replace(receipt, status="not_presented")
elif not authority.can_control:
receipt = replace(receipt, status="no_longitudinal_authority")
else:
receipt = replace(receipt, consumed=True, status=action.kind.value)
if action.kind is ActionKind.ACCEPT:
state, history = _accept(state, candidate, now_ns)
return finish("driver_accept", target=candidate.speed_mps, history=history)
state = _reject(state, candidate)
return finish("driver_reject", target=None if state.rejection_capacity_exhausted else previous_target())
if pending.active_elapsed_ns >= CONFIRMATION_NS and authority.can_control:
state = _reject(state, candidate)
return finish("timeout_reject", target=None if state.rejection_capacity_exhausted else previous_target())
return finish("pending", target=previous_target())
@@ -0,0 +1,251 @@
"""Track causal cruise effects without inferring driver intent from speed edges.
This ledger does not authorize control or associate physical buttons with CAN.
The input adapter must supply real action identities and a qualified context.
"""
import math
from dataclasses import dataclass, replace
from enum import Enum
from openpilot.starpilot.speed_limits.acceptance import ActionReceipt
from openpilot.starpilot.speed_limits.acceptance import Decision as AcceptanceDecision
from openpilot.starpilot.speed_limits.acceptance import State as AcceptanceState
MAX_TRANSACTIONS = 128
class Origin(Enum):
DRIVER_CRUISE = "driver_cruise"
DRIVER_OTHER = "driver_other"
AUTOMATIC = "automatic"
class Disposition(Enum):
UNRESOLVED = "unresolved"
DRIVER_INTENT = "driver_intent"
SLC_CONSUMED = "slc_consumed"
AUTOMATIC = "automatic"
UNRELATED = "unrelated"
@dataclass(frozen=True)
class Transaction:
action_id: int
origin: Origin
context_id: str
started_at_ns: int
disposition: Disposition = Disposition.UNRESOLVED
@dataclass(frozen=True)
class Ledger:
session_id: str
transactions: tuple[Transaction, ...] = ()
last_action_id: int = -1
last_effect_id: int = -1
last_timestamp_ns: int | None = None
reset_required: bool = False
@dataclass(frozen=True)
class BeginAction:
session_id: str
action_id: int
origin: Origin
context_id: str
@dataclass(frozen=True)
class ResolveAction:
session_id: str
action_id: int
disposition: Disposition
@dataclass(frozen=True)
class CompleteAction:
session_id: str
action_id: int
@dataclass(frozen=True)
class CruiseChange:
session_id: str
effect_id: int
action_id: int | None
previous_mps: float | None
selected_mps: float
@dataclass(frozen=True)
class ClassifiedChange:
session_id: str
effect_id: int
action_id: int | None
context_id: str | None
started_at_ns: int | None
previous_mps: float | None
selected_mps: float
disposition: Disposition
@dataclass(frozen=True)
class Result:
state: Ledger
status: str
change: ClassifiedChange | None = None
errors: tuple[str, ...] = ()
def _token(value: object) -> bool:
return type(value) is str and 0 < len(value) <= 256 and bool(value.strip()) and not any(ord(c) < 32 for c in value)
def _integer(value: object, minimum: int = 0) -> bool:
return type(value) is int and value >= minimum
def _speed(value: object) -> bool:
if type(value) is not int and type(value) is not float:
return False
try:
return math.isfinite(value) and value > 0
except OverflowError:
return False
def _coherent(origin: Origin, disposition: Disposition) -> bool:
if disposition is Disposition.DRIVER_INTENT:
return origin is Origin.DRIVER_CRUISE
if disposition is Disposition.AUTOMATIC:
return origin is Origin.AUTOMATIC
if disposition is Disposition.SLC_CONSUMED:
return origin in (Origin.DRIVER_CRUISE, Origin.DRIVER_OTHER)
return disposition in (Disposition.UNRESOLVED, Disposition.UNRELATED)
def _state_valid(state: object) -> bool:
if (not isinstance(state, Ledger) or not _token(state.session_id) or type(state.transactions) is not tuple or
len(state.transactions) > MAX_TRANSACTIONS or not _integer(state.last_action_id, -1) or
not _integer(state.last_effect_id, -1) or type(state.reset_required) is not bool or
(state.last_timestamp_ns is not None and not _integer(state.last_timestamp_ns))):
return False
last_id = -1
for tx in state.transactions:
if (not isinstance(tx, Transaction) or not _integer(tx.action_id) or not last_id < tx.action_id <= state.last_action_id or
not isinstance(tx.origin, Origin) or not _token(tx.context_id) or not isinstance(tx.disposition, Disposition) or
not _integer(tx.started_at_ns) or state.last_timestamp_ns is None or tx.started_at_ns > state.last_timestamp_ns or
not _coherent(tx.origin, tx.disposition)):
return False
last_id = tx.action_id
return True
def new_ledger(session_id: str) -> Ledger:
state = Ledger(session_id)
if not _state_valid(state):
raise ValueError("invalid session ID")
return state
def _event_valid(event: object) -> bool:
if not isinstance(event, (BeginAction, ResolveAction, CompleteAction, CruiseChange)) or not _token(event.session_id):
return False
if isinstance(event, CruiseChange):
return (_integer(event.effect_id) and (event.action_id is None or _integer(event.action_id)) and
(event.previous_mps is None or _speed(event.previous_mps)) and _speed(event.selected_mps))
if not _integer(event.action_id):
return False
if isinstance(event, BeginAction):
return isinstance(event.origin, Origin) and _token(event.context_id)
if isinstance(event, ResolveAction):
return isinstance(event.disposition, Disposition) and event.disposition is not Disposition.UNRESOLVED
return True
def step(state: Ledger, event: BeginAction | ResolveAction | CompleteAction | CruiseChange, *, now_ns: int) -> Result:
"""Classify supplied causal evidence. Missing evidence cannot establish intent."""
if not _state_valid(state):
return Result(state, "invalid_state", errors=("invalid ledger",))
if not _event_valid(event):
return Result(replace(state, reset_required=True), "invalid_input", errors=("invalid action event",))
if event.session_id != state.session_id:
return Result(state, "foreign_session")
if not _integer(now_ns) or (state.last_timestamp_ns is not None and now_ns < state.last_timestamp_ns):
return Result(replace(state, reset_required=True), "invalid_input", errors=("invalid or reversed monotonic clock",))
state = replace(state, last_timestamp_ns=now_ns)
if state.reset_required:
return Result(state, "session_reset_required")
transaction = next((tx for tx in state.transactions if tx.action_id == event.action_id), None)
if isinstance(event, BeginAction):
if event.action_id <= state.last_action_id:
return Result(state, "already_seen_action")
state = replace(state, last_action_id=event.action_id)
if len(state.transactions) == MAX_TRANSACTIONS:
return Result(replace(state, reset_required=True), "session_reset_required")
tx = Transaction(event.action_id, event.origin, event.context_id, now_ns)
return Result(replace(state, transactions=(*state.transactions, tx)), "begun")
if isinstance(event, CruiseChange):
if event.effect_id <= state.last_effect_id:
return Result(state, "already_seen_effect")
state = replace(state, last_effect_id=event.effect_id)
disposition = transaction.disposition if transaction is not None else Disposition.UNRESOLVED
# A first scalar sample or unchanged value is not a fresh cruise selection.
if disposition is Disposition.DRIVER_INTENT and (event.previous_mps is None or event.previous_mps == event.selected_mps):
disposition = Disposition.UNRESOLVED
change = ClassifiedChange(state.session_id, event.effect_id, event.action_id,
transaction.context_id if transaction is not None else None,
transaction.started_at_ns if transaction is not None else None,
event.previous_mps, event.selected_mps, disposition)
return Result(state, disposition.value, change)
if transaction is None:
return Result(state, "unknown_or_completed_action")
if isinstance(event, CompleteAction):
return Result(replace(state, transactions=tuple(tx for tx in state.transactions if tx.action_id != event.action_id)), "completed")
if not _coherent(transaction.origin, event.disposition):
return Result(replace(state, reset_required=True), "invalid_resolution", errors=("disposition contradicts action origin",))
if transaction.disposition is event.disposition:
return Result(state, "already_resolved")
if transaction.disposition is not Disposition.UNRESOLVED:
return Result(replace(state, reset_required=True), "invalid_resolution", errors=("final action disposition cannot change",))
return Result(replace(state, transactions=tuple(replace(tx, disposition=event.disposition) if tx.action_id == event.action_id else tx
for tx in state.transactions)), "resolved")
def resolve_acceptance(state: Ledger, action_id: int, decision: AcceptanceDecision, *, now_ns: int) -> Result:
"""Bind a matching final acceptance receipt; acknowledgment alone is insufficient.
This helper consumes an actual acceptance result, not its optional speed target.
A caller handles actions not submitted to acceptance with an explicit resolution.
"""
if not _state_valid(state):
return Result(state, "invalid_state", errors=("invalid ledger",))
if (not isinstance(decision, AcceptanceDecision) or not isinstance(decision.state, AcceptanceState) or not _integer(action_id) or
any(r is not None and (not isinstance(r, ActionReceipt) or not _integer(r.sequence_id) or
type(r.acknowledged) is not bool or type(r.consumed) is not bool or not _token(r.status))
for r in (decision.action_receipt, decision.adoption_receipt))):
return Result(replace(state, reset_required=True), "invalid_input", errors=("invalid acceptance resolution",))
if decision.state.session_id != state.session_id:
return Result(state, "foreign_session")
transaction = next((tx for tx in state.transactions if tx.action_id == action_id), None)
if transaction is None:
return Result(state, "unknown_or_completed_action")
if (not _integer(now_ns) or decision.state.last_timestamp_ns is None or not _integer(decision.state.last_timestamp_ns) or
not transaction.started_at_ns <= decision.state.last_timestamp_ns <= now_ns):
return Result(replace(state, reset_required=True), "invalid_input", errors=("acceptance clock does not match action lifecycle",))
receipts = [r for r in (decision.action_receipt, decision.adoption_receipt) if r is not None and r.sequence_id == action_id]
if decision.errors or len(receipts) != 1 or not receipts[0].acknowledged or receipts[0].status in ("already_seen", "foreign_session", "seen"):
return Result(state, "unresolved_acceptance")
if receipts[0].consumed:
disposition = Disposition.SLC_CONSUMED
elif transaction.origin is Origin.DRIVER_CRUISE:
disposition = Disposition.DRIVER_INTENT
elif transaction.origin is Origin.AUTOMATIC:
disposition = Disposition.AUTOMATIC
else:
disposition = Disposition.UNRELATED
return step(state, ResolveAction(state.session_id, action_id, disposition), now_ns=now_ns)
@@ -0,0 +1,221 @@
"""Pure composition of source, acceptance, offset, override and lead SLC decisions."""
import math
from collections.abc import Mapping
from dataclasses import dataclass, replace
from openpilot.starpilot.longitudinal.cruise_ceiling import CruiseCeiling
from openpilot.starpilot.speed_limits import acceptance as acc
from openpilot.starpilot.speed_limits import action_arbitration as arb
from openpilot.starpilot.speed_limits import lead_relaxation as lr
from openpilot.starpilot.speed_limits import overrides as ov
from openpilot.starpilot.speed_limits import selection as sel
from openpilot.starpilot.speed_limits import speed_domain as sd
@dataclass(frozen=True)
class HostState:
system_long_available: bool
long_control_active: bool
selfdrive_enabled: bool
force_decel: bool
force_stop_active: bool
driver_v_cruise_kph: float
selected_cluster_kph: float | None
@dataclass(frozen=True)
class Frame:
now_ns: int
observations: Mapping[sel.Source, acc.Observation]
selection_policy: sel.SelectionPolicy
authority: acc.Authority
acceptance_policy: acc.Policy
offset_schedule: sd.OffsetSchedule
selected_kind: sd.DomainStatus
ego_pair: sd.SpeedPair
pedal_pressed: bool | None
lead: lr.LeadEvidence
lead_policy: lr.Policy
host: HostState
action: acc.DriverAction | None = None
adopt: acc.AdoptRequest | None = None
classified_cruise: arb.Result | None = None
@dataclass(frozen=True)
class State:
session_id: str
acceptance: acc.State
override: ov.State
lead_prior: lr.PriorApplied | None = None
control_generation: int = 0
last_now_ns: int | None = None
previous_mode: acc.Mode | None = None
previous_owner: acc.LongitudinalOwner | None = None
was_active_path: bool = False
reset_required: bool = False
@dataclass(frozen=True)
class Result:
state: State
selection: sel.Selection | None = None
acceptance: acc.Decision | None = None
domain: sd.DomainResolution | None = None
override: ov.Decision | None = None
coordinate: sd.PlannerCoordinate | None = None
relaxation: lr.Result | None = None
ceiling: CruiseCeiling | None = None
status: str = "none"
errors: tuple[str, ...] = ()
def _integer(value: object) -> bool:
return type(value) is int and value >= 0
def _number(value: object, *, positive: bool = False) -> bool:
if type(value) not in (int, float):
return False
assert isinstance(value, (int, float))
try:
return math.isfinite(value) and (value > 0 if positive else value >= 0)
except OverflowError:
return False
def _state_valid(state: object) -> bool:
if not isinstance(state, State) or not isinstance(state.acceptance, acc.State) or not isinstance(state.override, ov.State):
return False
if (type(state.session_id) is not str or not state.session_id or state.acceptance.session_id != state.session_id or
state.override.session_id != state.session_id or not _integer(state.control_generation) or
(state.last_now_ns is not None and not _integer(state.last_now_ns)) or
type(state.was_active_path) is not bool or type(state.reset_required) is not bool):
return False
if ((state.previous_mode is None) != (state.previous_owner is None) or
(state.previous_mode is not None and not isinstance(state.previous_mode, acc.Mode)) or
(state.previous_owner is not None and not isinstance(state.previous_owner, acc.LongitudinalOwner))):
return False
return state.lead_prior is None or (isinstance(state.lead_prior, lr.PriorApplied) and
state.lead_prior.session_id == state.session_id and
state.lead_prior.continuity_id == f"control:{state.control_generation}" and
_number(state.lead_prior.cap_mps, positive=True))
def _host_valid(host: object) -> bool:
return (isinstance(host, HostState) and
all(type(value) is bool for value in (host.system_long_available, host.long_control_active,
host.selfdrive_enabled, host.force_decel, host.force_stop_active)) and
_number(host.driver_v_cruise_kph) and
(host.selected_cluster_kph is None or _number(host.selected_cluster_kph)))
def _override_evidence(kind: sd.DomainStatus) -> ov.EvidenceKind:
return {sd.DomainStatus.VALID: ov.EvidenceKind.VALID,
sd.DomainStatus.ABSENT: ov.EvidenceKind.ABSENT,
sd.DomainStatus.STALE: ov.EvidenceKind.STALE}.get(kind, ov.EvidenceKind.UNKNOWN)
def new_session(session_id: str, qualified_history: acc.AcceptedLimit | None = None) -> State:
return State(session_id, acc.new_session(session_id, qualified_history), ov.new_session(session_id))
def step(state: State, frame: Frame) -> Result:
if not _state_valid(state):
return Result(state, status="invalid_state", errors=("invalid composition state",))
if state.reset_required:
return Result(state, status="session_reset_required", errors=("new session required",))
if (not isinstance(frame, Frame) or not _integer(frame.now_ns) or
(state.last_now_ns is not None and frame.now_ns < state.last_now_ns) or
not _host_valid(frame.host) or not acc.authority_is_valid(frame.authority) or
not isinstance(frame.acceptance_policy, acc.Policy) or
any(type(value) is not bool for value in (frame.acceptance_policy.confirm_lower,
frame.acceptance_policy.confirm_higher,
frame.acceptance_policy.fallback_previous,
frame.acceptance_policy.display_only,
frame.acceptance_policy.vision_driver_confirm)) or
not isinstance(frame.selected_kind, sd.DomainStatus) or not isinstance(frame.ego_pair, sd.SpeedPair) or
not isinstance(frame.lead, lr.LeadEvidence)):
return Result(replace(state, lead_prior=None, reset_required=True), status="invalid_frame", errors=("invalid frame or clock",))
host = frame.host
selected_initialized = host.driver_v_cruise_kph > 0 and host.driver_v_cruise_kph != 255.0
host_long = host.system_long_available and host.long_control_active and host.selfdrive_enabled
system_mode = frame.authority.mode in (acc.Mode.LONGITUDINAL_ONLY, acc.Mode.COMBINED) and frame.authority.owner is acc.LongitudinalOwner.SYSTEM
if system_mode and frame.authority.longitudinal_active != host_long:
return Result(replace(state, lead_prior=None, last_now_ns=frame.now_ns, reset_required=True),
status="host_authority_disagreement", errors=("host and system-long authority disagree",))
active_path = (frame.authority.can_control and host_long and selected_initialized and
not host.force_decel and not host.force_stop_active and not frame.acceptance_policy.display_only)
mode_changed = state.previous_mode is not None and (state.previous_mode != frame.authority.mode or
state.previous_owner != frame.authority.owner)
generation = state.control_generation + int(active_path and (not state.was_active_path or mode_changed))
prior = state.lead_prior if active_path and state.was_active_path and not mode_changed else None
elapsed_s = 0.0 if state.last_now_ns is None else (frame.now_ns - state.last_now_ns) / 1e9
base = replace(state, control_generation=generation, lead_prior=prior, last_now_ns=frame.now_ns,
previous_mode=frame.authority.mode, previous_owner=frame.authority.owner, was_active_path=active_path)
if frame.classified_cruise is not None:
classified = frame.classified_cruise
if (not isinstance(classified, arb.Result) or classified.errors or not isinstance(classified.state, arb.Ledger) or
not isinstance(classified.change, arb.ClassifiedChange) or classified.state.session_id != state.session_id or
classified.change.session_id != state.session_id or classified.state.reset_required or
classified.state.last_timestamp_ns != frame.now_ns or
classified.state.last_effect_id != classified.change.effect_id or
not isinstance(classified.change.disposition, arb.Disposition) or
classified.status != classified.change.disposition.value or frame.action is not None or frame.adopt is not None):
return Result(replace(base, lead_prior=None, reset_required=True), status="invalid_ledger_result",
errors=("invalid, stale or ambiguous causal ledger result",))
selected = sel.select_limit(frame.observations, frame.selection_policy)
if selected.errors:
return Result(replace(base, lead_prior=None, reset_required=True), selection=selected, status="invalid_selection", errors=selected.errors)
acceptance = acc.step(base.acceptance, selected.observation, frame.authority, frame.acceptance_policy,
now_ns=frame.now_ns, action=frame.action, adopt=frame.adopt)
base = replace(base, acceptance=acceptance.state)
if acceptance.errors:
return Result(replace(base, lead_prior=None, reset_required=True), selected, acceptance,
status="invalid_acceptance", errors=acceptance.errors)
# The raw selected set speed is derived solely from the actual driver vCruise kph.
if frame.selected_kind is sd.DomainStatus.VALID and selected_initialized and host.selected_cluster_kph is not None:
pair = sd.selected_pair_from_kph(host.driver_v_cruise_kph / 3.6, host.selected_cluster_kph,
session_id=state.session_id, timestamp_ns=frame.now_ns)
elif selected_initialized:
pair = sd.SpeedPair(frame.selected_kind)
else:
pair = sd.SpeedPair(sd.DomainStatus.ABSENT)
domain = sd.resolve(acceptance, frame.offset_schedule, pair, frame.ego_pair)
selected_evidence = ov.SelectedEvidence(_override_evidence(pair.status), pair.raw_mps if pair.status is sd.DomainStatus.VALID else None)
pedal_evidence = ov.PedalEvidence(_override_evidence(frame.ego_pair.status),
frame.pedal_pressed if frame.ego_pair.status is sd.DomainStatus.VALID else None,
frame.ego_pair.raw_mps if frame.ego_pair.status is sd.DomainStatus.VALID else None)
change = frame.classified_cruise.change if frame.classified_cruise is not None else None
override = ov.step(base.override, acceptance, frame.authority, frame.acceptance_policy,
selected_evidence, pedal_evidence, now_ns=frame.now_ns, change=change, domain=domain)
base = replace(base, override=override.state)
if override.errors or domain.status is sd.DomainStatus.INVALID:
return Result(replace(base, lead_prior=None, reset_required=True), selected, acceptance, domain, override,
status="invalid_domain_or_override", errors=override.errors or (domain.reason,))
if not active_path or domain.status is not sd.DomainStatus.VALID:
return Result(replace(base, lead_prior=None), selected, acceptance, domain, override, status="inactive_or_unavailable")
context = domain.context
if (not isinstance(context, sd.DomainContext) or override.state.session_id != state.session_id or
override.state.last_timestamp_ns != frame.now_ns or override.state.accepted_mps != context.accepted_raw_mps or
override.state.effective_target_mps != context.effective_cluster_mps):
return Result(replace(base, lead_prior=None, reset_required=True), selected, acceptance, domain, override,
status="invalid_join", errors=("override and speed domain mismatch",))
coordinate = sd.to_planner_coordinate(context, override.contribution_mps, override.basis)
if coordinate.cap_mps is None:
return Result(replace(base, lead_prior=None, reset_required=True), selected, acceptance, domain, override, coordinate,
status="invalid_coordinate", errors=("planner coordinate unavailable",))
source = lr.SourceEvidence(acceptance.state.observation.kind, True)
relaxation = lr.step(coordinate.cap_mps, source=source, lead=frame.lead, prior=prior,
session_id=state.session_id, continuity_id=f"control:{generation}", ego_mps=context.ego_raw_mps,
override_active=override.contribution_mps is not None, active_path=True,
elapsed_s=elapsed_s, policy=frame.lead_policy)
base = replace(base, lead_prior=relaxation.next_prior)
if relaxation.errors or relaxation.cap_mps is None:
return Result(base, selected, acceptance, domain, override, coordinate, relaxation,
status="invalid_relaxation" if relaxation.errors else "no_cap", errors=relaxation.errors)
return Result(base, selected, acceptance, domain, override, coordinate, relaxation,
CruiseCeiling(relaxation.cap_mps, frame.authority), "ceiling")
@@ -0,0 +1,58 @@
"""Versioned accepted-limit persistence with explicit identity applicability."""
import json
import math
from openpilot.cereal import custom
from openpilot.starpilot.schema_cache import schema_fingerprint
from openpilot.starpilot.speed_limits import acceptance as acc
FORMAT = "starpilot-slc-accepted"
VERSION = 1
def encode(accepted: acc.AcceptedLimit, saved_wall_ns: int) -> str:
if not isinstance(accepted, acc.AcceptedLimit) or type(saved_wall_ns) is not int or saved_wall_ns <= 0:
raise ValueError("invalid accepted history")
candidate = accepted.candidate
identity = candidate.observation_identity
if not acc.observation_is_valid(acc.Observation(acc.ObservationKind.VALID, candidate), session_id=accepted.session_id):
raise ValueError("invalid accepted candidate")
return json.dumps({"format": FORMAT, "version": VERSION, "source_schema": schema_fingerprint(custom.SlcDashboardObservation.schema),
"saved_wall_ns": saved_wall_ns, "source": candidate.source, "speed_mps": candidate.speed_mps,
"identity_kind": identity.kind.value, "identity_value": identity.value,
"origin_session": accepted.session_id}, sort_keys=True, separators=(",", ":"))
def decode(raw: str | bytes | dict | None, *, session_id: str, current: acc.Observation,
now_wall_ns: int, max_age_ns: int) -> acc.AcceptedLimit | None:
"""Only durable geographic identity can be carried into a new drive.
The caller sets an applicability age from its source contract. Dashboard TSR
producer episodes are drive-local and cannot be restored across a new session.
"""
if raw is None or type(session_id) is not str or not session_id or type(now_wall_ns) is not int or type(max_age_ns) is not int:
return None
if max_age_ns < 0 or now_wall_ns <= 0:
return None
try:
value = raw if isinstance(raw, dict) else json.loads(raw)
if (type(value) is not dict or set(value) != {"format", "version", "source_schema", "saved_wall_ns", "source",
"speed_mps", "identity_kind", "identity_value", "origin_session"} or
value["format"] != FORMAT or type(value["version"]) is not int or value["version"] != VERSION or
value["source_schema"] != schema_fingerprint(custom.SlcDashboardObservation.schema) or
type(value["saved_wall_ns"]) is not int or value["saved_wall_ns"] > now_wall_ns or
now_wall_ns - value["saved_wall_ns"] > max_age_ns or
value["identity_kind"] != acc.IdentityKind.GEOGRAPHIC.value or
not isinstance(value["identity_value"], str) or not value["identity_value"] or
not isinstance(value["origin_session"], str) or not value["origin_session"] or
type(value["speed_mps"]) not in (int, float) or not math.isfinite(value["speed_mps"])):
return None
candidate = acc.Candidate(value["source"], acc.ObservationIdentity(acc.IdentityKind.GEOGRAPHIC, value["identity_value"]),
float(value["speed_mps"]))
if (not acc.observation_is_valid(acc.Observation(acc.ObservationKind.VALID, candidate), session_id=session_id) or
current != acc.Observation(acc.ObservationKind.VALID, candidate)):
return None
return acc.AcceptedLimit(candidate, session_id, 0)
except (TypeError, ValueError, OverflowError):
return None
@@ -0,0 +1,155 @@
"""Pure easing of a qualified SLC cruise cap after a lower accepted limit."""
import math
from dataclasses import dataclass
from enum import Enum
from openpilot.starpilot.speed_limits.acceptance import ObservationKind
class LeadKind(Enum):
VALID = "valid"
ABSENT = "absent"
UNKNOWN = "unknown"
STALE = "stale"
@dataclass(frozen=True)
class SourceEvidence:
kind: ObservationKind
cap_qualified: bool
@dataclass(frozen=True)
class LeadEvidence:
kind: LeadKind
tracking: bool | None = None
present: bool | None = None
distance_m: float | None = None
speed_mps: float | None = None
accel_mps2: float | None = None
@dataclass(frozen=True)
class Policy:
minimum_ego_mps: float
minimum_distance_m: float
minimum_headway_s: float
maximum_lead_deficit_mps: float
maximum_lead_brake_mps2: float
drop_guard_mps: float
overspeed_guard_mps: float
overspeed_breakpoints_mps: tuple[float, ...]
deceleration_mps2: tuple[float, ...]
@dataclass(frozen=True)
class PriorApplied:
session_id: str
continuity_id: str
cap_mps: float
@dataclass(frozen=True)
class Result:
cap_mps: float | None
next_prior: PriorApplied | None
status: str
errors: tuple[str, ...] = ()
def _number(value: object, *, positive: bool = False, signed: bool = False) -> bool:
if type(value) not in (int, float):
return False
assert isinstance(value, (int, float))
try:
return math.isfinite(value) and (signed or (value > 0 if positive else value >= 0))
except OverflowError:
return False
def _token(value: object) -> bool:
return type(value) is str and bool(value) and len(value) <= 256 and bool(value.strip()) and not any(ord(c) < 32 for c in value)
def _policy_valid(policy: object) -> bool:
if not isinstance(policy, Policy):
return False
if any(not _number(value) for value in (
policy.minimum_ego_mps, policy.minimum_distance_m, policy.minimum_headway_s,
policy.maximum_lead_deficit_mps, policy.maximum_lead_brake_mps2,
policy.drop_guard_mps, policy.overspeed_guard_mps,
)):
return False
bp, decel = policy.overspeed_breakpoints_mps, policy.deceleration_mps2
if (not isinstance(bp, tuple) or not isinstance(decel, tuple) or len(bp) < 2 or len(bp) != len(decel) or
any(not _number(v) for v in bp) or any(not _number(v, positive=True) for v in decel)):
return False
return all(bp[i] > bp[i - 1] for i in range(1, len(bp)))
def _lead_valid(lead: object) -> bool:
if not isinstance(lead, LeadEvidence) or not isinstance(lead.kind, LeadKind):
return False
if lead.kind is not LeadKind.VALID:
return all(value is None for value in (lead.tracking, lead.present, lead.distance_m, lead.speed_mps, lead.accel_mps2))
return (type(lead.tracking) is bool and type(lead.present) is bool and
_number(lead.distance_m) and _number(lead.speed_mps) and _number(lead.accel_mps2, signed=True))
def _interpolate(value: float, xs: tuple[float, ...], ys: tuple[float, ...]) -> float:
if value <= xs[0]:
return ys[0]
if value >= xs[-1]:
return ys[-1]
for i in range(1, len(xs)):
if value <= xs[i]:
ratio = (value - xs[i - 1]) / (xs[i] - xs[i - 1])
return ys[i - 1] + ratio * (ys[i] - ys[i - 1])
return ys[-1]
def step(raw_cap_mps: float | None, *, source: SourceEvidence, lead: LeadEvidence,
prior: PriorApplied | None, session_id: str, continuity_id: str, ego_mps: float,
override_active: bool, active_path: bool, elapsed_s: float, policy: Policy) -> Result:
"""Ease only a qualified cap; caller owns source, lead and control continuity."""
if not _token(session_id) or not _token(continuity_id) or not isinstance(source, SourceEvidence) or not isinstance(source.kind, ObservationKind):
return Result(None, None, "invalid_source", ("invalid source identity",))
if (type(source.cap_qualified) is not bool or source.kind in (ObservationKind.UNKNOWN, ObservationKind.STALE) or
not source.cap_qualified):
return Result(None, None, "source_unavailable")
if type(active_path) is not bool or not active_path or raw_cap_mps is None:
return Result(None, None, "inactive_or_absent")
if not _number(raw_cap_mps, positive=True):
return Result(None, None, "invalid_cap", ("invalid raw cap",))
assert raw_cap_mps is not None
if (not _policy_valid(policy) or not _number(ego_mps) or type(override_active) is not bool or
not _number(elapsed_s) or not _lead_valid(lead)):
return Result(raw_cap_mps, None, "invalid_input", ("invalid lead, policy or elapsed time",))
if prior is not None and (not isinstance(prior, PriorApplied) or not _token(prior.session_id) or
not _token(prior.continuity_id) or not _number(prior.cap_mps, positive=True) or
prior.session_id != session_id or prior.continuity_id != continuity_id):
return Result(raw_cap_mps, None, "invalid_prior", ("prior control continuity mismatch",))
if override_active:
return Result(raw_cap_mps, None, "override")
next_prior = PriorApplied(session_id, continuity_id, raw_cap_mps)
if source.kind is ObservationKind.ABSENT:
return Result(raw_cap_mps, next_prior, "fallback_source")
if lead.kind is not LeadKind.VALID:
return Result(raw_cap_mps, next_prior, "lead_unavailable")
assert lead.tracking is not None and lead.present is not None
assert lead.distance_m is not None and lead.speed_mps is not None and lead.accel_mps2 is not None
if not lead.tracking or not lead.present:
return Result(raw_cap_mps, next_prior, "no_tracked_lead")
if (prior is None or raw_cap_mps >= prior.cap_mps - policy.drop_guard_mps or
ego_mps < policy.minimum_ego_mps or raw_cap_mps >= ego_mps - policy.overspeed_guard_mps):
return Result(raw_cap_mps, next_prior, "no_eligible_drop")
if lead.distance_m < max(policy.minimum_distance_m, ego_mps * policy.minimum_headway_s):
return Result(raw_cap_mps, next_prior, "close_lead")
if lead.speed_mps < raw_cap_mps - policy.maximum_lead_deficit_mps:
return Result(raw_cap_mps, next_prior, "slow_lead")
if max(0.0, -lead.accel_mps2) > policy.maximum_lead_brake_mps2:
return Result(raw_cap_mps, next_prior, "braking_lead")
decel = _interpolate(max(0.0, ego_mps - raw_cap_mps), policy.overspeed_breakpoints_mps, policy.deceleration_mps2)
eased = max(raw_cap_mps, prior.cap_mps - decel * elapsed_s)
return Result(eased, PriorApplied(session_id, continuity_id, eased), "eased")
@@ -0,0 +1,46 @@
"""Current-road Maps projection; source ordering/freshness stays in MapTracker."""
import hashlib
import json
from openpilot.starpilot.speed_limits import acceptance as acc
from openpilot.starpilot.speed_limits import map_source as ms
def copy_frame(road, *, event_valid: bool, event_boot_ns: int) -> ms.MapFrame:
"""Copy a current mapdOut reader without retaining a mutable cereal view."""
try:
return ms.MapFrame(event_valid, event_boot_ns, int(road.sampleVersion), str(road.roadStatus),
str(road.gpsSource), int(road.gpsMonoTime), int(road.computedMonoTime),
int(road.sourceGeneration), int(road.producerSession), float(road.speedLimit),
bool(road.tileLoaded), int(road.wayId), str(road.waySelectionType))
except Exception as error: # pycapnp lazy reads may raise KjException, not ValueError
raise ValueError('invalid mapdOut reader') from error
class MapSource:
"""Plannerd's map observation owner; never grants longitudinal authority."""
def __init__(self):
self.tracker = ms.MapTracker()
def reset(self):
self.tracker.reset()
def step(self, frame: ms.MapFrame | None, *, received: bool, now_boot_ns: int,
transport_alive: bool, drive_session: str, car_fingerprint: str) -> acc.Observation:
evidence = self.tracker.step(frame, received=received, now_boot_ns=now_boot_ns,
transport_alive=transport_alive)
if evidence.observation.kind is acc.ObservationKind.STALE:
return evidence.observation
accepted = self.tracker.diagnostic(now_boot_ns=now_boot_ns).frame
if (evidence.kind != 'matched_limit_unqualified' or accepted is None or
accepted.way_selection != 'current' or
any(type(value) is not str or not 0 < len(value) <= 256 or not value.strip() or
any(ord(c) < 32 for c in value) for value in (drive_session, car_fingerprint))):
# Missing speed metadata, coverage/GPS loss and ambiguous/lookahead ways
# are incomplete evidence, not regulatory absence or previous-limit permission.
return acc.Observation(acc.ObservationKind.UNKNOWN)
identity = hashlib.sha256(json.dumps((drive_session, car_fingerprint, accepted.producer_session,
accepted.generation, accepted.source, accepted.way_id), separators=(',', ':')).encode()).hexdigest()
return acc.Observation(acc.ObservationKind.VALID, acc.Candidate('map',
acc.ObservationIdentity(acc.IdentityKind.GEOGRAPHIC, value=identity), accepted.speed_mps))
@@ -0,0 +1,240 @@
"""Disabled, source-aware boundary for v2 mapdOut evidence.
This module checks wire freshness and ordering. It deliberately never creates a
VALID or ABSENT SLC observation: a fresh map match is not road qualification.
"""
import math
from dataclasses import dataclass
from openpilot.cereal import log
from openpilot.starpilot.speed_limits import acceptance as acc
EVENT_MAX_AGE_NS = 250_000_000
COMPUTED_MAX_AGE_NS = 250_000_000
GPS_MAX_AGE_NS = {'external': 500_000_000, 'internal': 2_000_000_000, 'car': 2_500_000_000}
MAX_EVENT_BYTES = 2 * 1024 * 1024
MAX_RETIRED_SESSIONS = 16
MAX_UINT64 = (1 << 64) - 1
MAX_INT64 = (1 << 63) - 1
MAX_FLOAT32 = 3.4028234663852886e38
MATCHED = frozenset(('matchedLimit', 'matchedNoLimit'))
LOSSES = frozenset(('noGps', 'noCoverage', 'noMatch'))
WAY_SELECTIONS = frozenset(('current', 'predicted', 'possible', 'extended'))
UNKNOWN = acc.Observation(acc.ObservationKind.UNKNOWN)
STALE = acc.Observation(acc.ObservationKind.STALE)
@dataclass(frozen=True)
class MapFrame:
event_valid: bool
event_mono_ns: int
version: int
status: str
source: str
gps_mono_ns: int
computed_mono_ns: int
generation: int
producer_session: int
speed_mps: float
tile_loaded: bool
way_id: int
way_selection: str
@dataclass(frozen=True)
class MapEvidence:
kind: str
observation: acc.Observation
speed_mps: float | None = None # parsed value only; never SLC authority
@dataclass(frozen=True)
class MapDiagnostic:
"""Metadata from the tracker's accepted frame; never a control observation."""
frame: MapFrame | None
producer_restarts: int
source_switches: int
def decode_event(raw: bytes) -> MapFrame:
"""Copy scalar evidence from an actual serialized host cereal Event."""
if type(raw) is not bytes or not 0 < len(raw) <= MAX_EVENT_BYTES:
raise ValueError('invalid mapdOut event size/type')
try:
with log.Event.from_bytes(raw) as event:
if event.which() != 'mapdOut':
raise ValueError('wrong Event variant')
road = event.mapdOut
return MapFrame(bool(event.valid), int(event.logMonoTime), int(road.sampleVersion), str(road.roadStatus),
str(road.gpsSource), int(road.gpsMonoTime), int(road.computedMonoTime),
int(road.sourceGeneration), int(road.producerSession), float(road.speedLimit),
bool(road.tileLoaded), int(road.wayId), str(road.waySelectionType))
except (ValueError, TypeError) as error:
raise ValueError('invalid mapdOut event') from error
except Exception as error: # pycapnp reports malformed segment/pointer errors as KjException
raise ValueError('invalid mapdOut event') from error
def _uint(value: object, *, positive: bool = False, maximum: int = MAX_UINT64) -> bool:
return type(value) is int and (value > 0 if positive else value >= 0) and value <= maximum
def _ordering_fields(frame: object) -> bool:
return (isinstance(frame, MapFrame) and type(frame.event_valid) is bool and type(frame.status) is str and
type(frame.source) is str and type(frame.tile_loaded) is bool and type(frame.way_selection) is str and
_uint(frame.event_mono_ns, positive=True) and _uint(frame.generation, positive=True) and
_uint(frame.producer_session, positive=True))
def _trusted_wire_shape(frame: MapFrame) -> bool:
if (not _ordering_fields(frame) or not _uint(frame.version, maximum=(1 << 16)-1) or frame.version != 2 or
frame.status not in MATCHED | LOSSES or frame.source not in (*GPS_MAX_AGE_NS, 'none') or
frame.way_selection not in WAY_SELECTIONS | {'fail'} or not _uint(frame.gps_mono_ns) or
not _uint(frame.computed_mono_ns, positive=True) or frame.computed_mono_ns > frame.event_mono_ns or
type(frame.way_id) is not int or not -MAX_INT64-1 <= frame.way_id <= MAX_INT64 or
type(frame.speed_mps) is not float or not math.isfinite(frame.speed_mps) or
abs(frame.speed_mps) > MAX_FLOAT32):
return False
if frame.status == 'noGps':
return ((frame.source == 'none' and frame.gps_mono_ns == 0) or
(frame.source in GPS_MAX_AGE_NS and 0 < frame.gps_mono_ns <= frame.computed_mono_ns))
return frame.source in GPS_MAX_AGE_NS and 0 < frame.gps_mono_ns <= frame.computed_mono_ns
def _current_at(frame: MapFrame, now_boot_ns: int) -> bool:
if not _trusted_wire_shape(frame):
return False
if not (0 < frame.computed_mono_ns <= frame.event_mono_ns <= now_boot_ns and
now_boot_ns - frame.event_mono_ns <= EVENT_MAX_AGE_NS and
now_boot_ns - frame.computed_mono_ns <= COMPUTED_MAX_AGE_NS):
return False
if frame.status in MATCHED or frame.status in ('noCoverage', 'noMatch'):
if (frame.source not in GPS_MAX_AGE_NS or not 0 < frame.gps_mono_ns <= frame.computed_mono_ns or
now_boot_ns - frame.gps_mono_ns > GPS_MAX_AGE_NS[frame.source]):
return False
return True
def _classify(frame: MapFrame, now_boot_ns: int) -> MapEvidence:
if not _current_at(frame, now_boot_ns):
# A structurally malformed fresh message is UNKNOWN; expired evidence is
# STALE. Neither has a numeric SLC candidate.
timestamps = (frame.gps_mono_ns, frame.computed_mono_ns, frame.event_mono_ns)
expired = (all(_uint(value) for value in timestamps) and frame.event_mono_ns <= now_boot_ns and
(now_boot_ns - frame.event_mono_ns > EVENT_MAX_AGE_NS or
now_boot_ns - frame.computed_mono_ns > COMPUTED_MAX_AGE_NS or
(frame.source in GPS_MAX_AGE_NS and frame.gps_mono_ns > 0 and
now_boot_ns - frame.gps_mono_ns > GPS_MAX_AGE_NS[frame.source])))
return MapEvidence('stale' if expired else 'unknown', STALE if expired else UNKNOWN)
if frame.status == 'matchedLimit':
if (not frame.event_valid or not frame.tile_loaded or frame.way_id <= 0 or
frame.way_selection not in WAY_SELECTIONS or frame.speed_mps <= 0):
return MapEvidence('unknown', UNKNOWN)
return MapEvidence('matched_limit_unqualified', UNKNOWN, float(frame.speed_mps))
if frame.status == 'matchedNoLimit':
if (not frame.event_valid or not frame.tile_loaded or frame.way_id <= 0 or
frame.way_selection not in WAY_SELECTIONS or frame.speed_mps != 0):
return MapEvidence('unknown', UNKNOWN)
return MapEvidence('matched_no_limit_unqualified', UNKNOWN)
if frame.event_valid or frame.speed_mps != 0 or frame.way_id != 0 or frame.tile_loaded:
return MapEvidence('unknown', UNKNOWN)
if frame.status == 'noGps' and frame.source == 'none' and frame.gps_mono_ns != 0:
return MapEvidence('unknown', UNKNOWN)
return MapEvidence('loss', UNKNOWN)
class MapTracker:
"""Bounded in-memory ordering state; outputs never authorize map control."""
def __init__(self):
self.reset()
def reset(self) -> None:
self._session: int | None = None
self._generation = 0
self._source = 'none'
self._last_event_ns = 0
self._last_now_ns = 0
self._retired: set[int] = set()
self._blocked = False
self._current: MapFrame | None = None
self._producer_restarts = 0
self._source_switches = 0
def diagnostic(self, *, now_boot_ns: int) -> MapDiagnostic:
# The frame is the same immutable object that passed step() ordering and
# freshness checks. Recheck age so even a caller between step() calls
# cannot retain a stopped producer's old diagnostic frame.
if not _uint(now_boot_ns, positive=True) or now_boot_ns < self._last_now_ns:
raise ValueError('invalid map diagnostic clock')
frame = self._current
if frame is not None and _classify(frame, now_boot_ns).kind not in (
'matched_limit_unqualified', 'matched_no_limit_unqualified', 'loss'):
frame = None
return MapDiagnostic(frame, self._producer_restarts, self._source_switches)
def _held(self, now_boot_ns: int) -> MapEvidence:
if self._current is None:
return MapEvidence('unknown', UNKNOWN)
result = _classify(self._current, now_boot_ns)
if result.kind == 'stale':
self._current = None
return result
def step(self, frame: MapFrame | None, *, received: bool, now_boot_ns: int,
transport_alive: bool = True) -> MapEvidence:
if not _uint(now_boot_ns, positive=True) or type(received) is not bool or type(transport_alive) is not bool:
raise ValueError('invalid map source clock/transport flags')
if now_boot_ns < self._last_now_ns:
self._blocked = True # new drive/replay epoch requires explicit reset()
self._last_now_ns = now_boot_ns
if self._blocked:
self._current = None
return MapEvidence('unknown', UNKNOWN)
if not transport_alive:
self._current = None
return MapEvidence('stale', STALE)
if not received:
return self._held(now_boot_ns)
if not _ordering_fields(frame):
self._current = None
return MapEvidence('unknown', UNKNOWN)
assert frame is not None
if frame.producer_session in self._retired:
return self._held(now_boot_ns)
if self._session is not None:
if frame.event_mono_ns <= self._last_event_ns:
return self._held(now_boot_ns) # old/duplicate packet cannot revoke or renew
if not _trusted_wire_shape(frame):
self._current = None # unsupported/malformed wire cannot move trusted ordering
return MapEvidence('unknown', UNKNOWN)
if frame.event_mono_ns > now_boot_ns:
self._current = None # future packet is invalid, but cannot poison the high-water mark
return MapEvidence('unknown', UNKNOWN)
if self._session is not None:
if frame.producer_session == self._session:
if frame.generation < self._generation or (frame.generation == self._generation and frame.source != self._source):
self._current = None
self._blocked = True
return MapEvidence('unknown', UNKNOWN)
else:
if len(self._retired) >= MAX_RETIRED_SESSIONS:
self._current = None
self._blocked = True
return MapEvidence('unknown', UNKNOWN)
self._retired.add(self._session)
self._producer_restarts += 1
self._current = None
if frame.producer_session != self._session or frame.generation != self._generation:
self._current = None
if self._session == frame.producer_session and frame.generation != self._generation:
self._source_switches += 1
self._session = frame.producer_session
self._generation = frame.generation
self._source = frame.source
self._last_event_ns = frame.event_mono_ns
result = _classify(frame, now_boot_ns)
self._current = frame if result.kind in ('matched_limit_unqualified', 'matched_no_limit_unqualified', 'loss') else None
return result
@@ -0,0 +1,102 @@
"""Strict SI authority for adopted SLC offsets; legacy source bytes stay untouched."""
from __future__ import annotations
from dataclasses import dataclass
import json
import math
from typing import cast
from openpilot.starpilot.speed_limits import speed_domain as sd
MPH_TO_MPS = 0.44704
KPH_TO_MPS = 1.0 / 3.6
IMPERIAL_BOUNDS = (0.0, 11.2, 15.2, 19.6, 24.1, 28.6, 33.1, 44.2)
METRIC_BOUNDS = (0.0, 8.1, 13.6, 16.4, 21.9, 27.5, 33.1, 38.9)
MAX_DOCUMENT_BYTES = 4096
@dataclass(frozen=True)
class OffsetDocument:
bounds_mps: tuple[float, ...]
offsets_mps: tuple[float, ...]
def schedule(self) -> sd.OffsetSchedule:
bands = tuple(sd.OffsetBand(self.bounds_mps[i], self.bounds_mps[i + 1], self.offsets_mps[i]) for i in range(7))
return sd.OffsetSchedule((*bands, sd.OffsetBand(self.bounds_mps[-1], None, 0.0)))
@dataclass(frozen=True)
class NeedsReview:
"""A unit change with corrupt saved control settings must not restore control."""
NEEDS_REVIEW = NeedsReview()
def _finite(value: object) -> float:
if not isinstance(value, (int, float)) or type(value) not in (int, float) or not math.isfinite(value):
raise ValueError('SLC offset value must be finite')
return float(value)
def adopt_legacy(metric: bool, offsets: tuple[object, ...]) -> OffsetDocument:
if type(metric) is not bool or len(offsets) != 7:
raise ValueError('invalid legacy SLC offsets or units')
conversion = KPH_TO_MPS if metric else MPH_TO_MPS
bounds = METRIC_BOUNDS if metric else IMPERIAL_BOUNDS
return OffsetDocument(bounds, tuple(_finite(_finite(value) * conversion) for value in offsets))
def validate(value: object) -> OffsetDocument | NeedsReview:
if value is NEEDS_REVIEW:
return NEEDS_REVIEW
if isinstance(value, OffsetDocument):
value = {'version': 1, 'bounds_mps': value.bounds_mps, 'offsets_mps': value.offsets_mps}
if not isinstance(value, dict) or type(value) is not dict:
raise ValueError('invalid SLC offset document version')
data = cast(dict[str, object], value)
if type(data.get('version')) is not int or data['version'] != 1:
raise ValueError('invalid SLC offset document version')
if data.keys() == {'version', 'state'} and data['state'] == 'needs_review':
return NEEDS_REVIEW
if data.keys() != {'version', 'bounds_mps', 'offsets_mps'}:
raise ValueError('invalid SLC offset document fields')
bounds, offsets = data['bounds_mps'], data['offsets_mps']
if not isinstance(bounds, (list, tuple)) or not isinstance(offsets, (list, tuple)) or \
type(bounds) not in (list, tuple) or type(offsets) not in (list, tuple) or len(bounds) != 8 or len(offsets) != 7:
raise ValueError('invalid SLC offset document shape')
bounds_si = tuple(_finite(bound) for bound in bounds)
offsets_si = tuple(_finite(offset) for offset in offsets)
if bounds_si[0] != 0.0 or any(next_bound <= bound for bound, next_bound in zip(bounds_si[:-1], bounds_si[1:], strict=True)):
raise ValueError('invalid SLC offset boundaries')
return OffsetDocument(bounds_si, offsets_si)
def decode(raw: bytes) -> OffsetDocument | NeedsReview:
if len(raw) > MAX_DOCUMENT_BYTES:
raise ValueError('SLC offset document too large')
def unique(pairs):
result = {}
for key, value in pairs:
if key in result:
raise ValueError('duplicate SLC offset document field')
result[key] = value
return result
def reject_nonfinite(_value):
raise ValueError('nonfinite SLC offset document JSON')
try:
value = json.loads(raw, object_pairs_hook=unique, parse_constant=reject_nonfinite)
except (UnicodeDecodeError, json.JSONDecodeError, RecursionError) as error:
raise ValueError('invalid SLC offset document JSON') from error
return validate(value)
def to_value(document: OffsetDocument | NeedsReview) -> dict[str, object]:
checked = validate(document)
return ({'version': 1, 'state': 'needs_review'} if checked is NEEDS_REVIEW else
{'version': 1, 'bounds_mps': list(checked.bounds_mps), 'offsets_mps': list(checked.offsets_mps)})
def encode(document: OffsetDocument | NeedsReview) -> bytes:
return json.dumps(to_value(document), sort_keys=True, separators=(',', ':'), allow_nan=False).encode('utf-8')
@@ -0,0 +1,329 @@
"""Pure SLC override contribution from qualified driver intent and current pedal evidence."""
import math
from dataclasses import dataclass, replace
from enum import Enum
from openpilot.starpilot.speed_limits import acceptance as acc
from openpilot.starpilot.speed_limits import action_arbitration as arb
from openpilot.starpilot.speed_limits import speed_domain as sd
SUSPENSION_NS = 750_000_000
class EvidenceKind(Enum):
VALID = "valid"
ABSENT = "absent"
UNKNOWN = "unknown"
STALE = "stale"
@dataclass(frozen=True)
class SelectedEvidence:
kind: EvidenceKind
speed_mps: float | None = None
@dataclass(frozen=True)
class PedalEvidence:
kind: EvidenceKind
pressed: bool | None = None
ego_mps: float | None = None
@dataclass(frozen=True)
class Context:
context_id: str
issued_at_ns: int
accepted_target_mps: float
@dataclass(frozen=True)
class State:
session_id: str
persistent_selected_mps: float | None = None
accepted_mps: float | None = None
context: Context | None = None
next_context_id: int = 1
active: bool = False
mode: acc.Mode | None = None
owner: acc.LongitudinalOwner | None = None
suspension_started_ns: int | None = None
last_effect_id: int = -1
last_adoption_sequence: int = -1
last_timestamp_ns: int | None = None
reset_required: bool = False
effective_target_mps: float | None = None
@dataclass(frozen=True)
class EventReceipt:
effect_id: int
acknowledged: bool
consumed: bool
status: str
@dataclass(frozen=True)
class Decision:
state: State
contribution_mps: float | None = None
basis: str = "none"
context: Context | None = None
event_receipt: EventReceipt | None = None
errors: tuple[str, ...] = ()
def _token(value: object) -> bool:
return type(value) is str and 0 < len(value) <= 256 and bool(value.strip()) and not any(ord(c) < 32 for c in value)
def _integer(value: object, minimum: int = 0) -> bool:
return type(value) is int and value >= minimum
def _speed(value: object) -> bool:
if type(value) not in (int, float):
return False
assert isinstance(value, (int, float))
try:
return math.isfinite(value) and value > 0
except OverflowError:
return False
def _ego_speed(value: object) -> bool:
if type(value) not in (int, float):
return False
assert isinstance(value, (int, float))
try:
return math.isfinite(value) and value >= 0
except OverflowError:
return False
def _state_valid(state: object) -> bool:
if not isinstance(state, State) or not _token(state.session_id):
return False
if any(v is not None and not _speed(v) for v in (state.persistent_selected_mps, state.accepted_mps, state.effective_target_mps)):
return False
if (not _integer(state.next_context_id, 1) or type(state.active) is not bool or
not _integer(state.last_effect_id, -1) or not _integer(state.last_adoption_sequence, -1) or
type(state.reset_required) is not bool or
any(v is not None and not _integer(v) for v in (state.suspension_started_ns, state.last_timestamp_ns))):
return False
if (state.mode is not None and not isinstance(state.mode, acc.Mode)) or (state.owner is not None and not isinstance(state.owner, acc.LongitudinalOwner)):
return False
if (state.mode is None) != (state.owner is None):
return False
if state.mode is not None and state.last_timestamp_ns is None:
return False
if state.reset_required and (state.persistent_selected_mps is not None or state.active or state.context is not None or
state.suspension_started_ns is not None):
return False
if state.suspension_started_ns is not None and (state.active or state.last_timestamp_ns is None or
state.suspension_started_ns > state.last_timestamp_ns or
state.mode not in (acc.Mode.LONGITUDINAL_ONLY, acc.Mode.COMBINED) or
state.owner is not acc.LongitudinalOwner.SYSTEM):
return False
# Retained intent is raw selected speed; a negative offset can make it lower
# than the raw accepted limit while still exceeding the effective threshold.
if state.context is not None:
c = state.context
if (not isinstance(c, Context) or not _token(c.context_id) or not _integer(c.issued_at_ns) or
not _speed(c.accepted_target_mps) or state.last_timestamp_ns is None or c.issued_at_ns > state.last_timestamp_ns or
not state.active or state.accepted_mps != c.accepted_target_mps or state.next_context_id <= 1 or
c.context_id != f"override:{state.next_context_id - 1}" or
state.mode not in (acc.Mode.LONGITUDINAL_ONLY, acc.Mode.COMBINED) or state.owner is not acc.LongitudinalOwner.SYSTEM):
return False
if state.active and (state.context is None or state.suspension_started_ns is not None):
return False
if state.active and state.effective_target_mps is None:
return False
return True
def new_session(session_id: str) -> State:
state = State(session_id)
if not _state_valid(state):
raise ValueError("invalid session ID")
return state
def _selected_valid(selected: object) -> bool:
return isinstance(selected, SelectedEvidence) and isinstance(selected.kind, EvidenceKind) and (
_speed(selected.speed_mps) if selected.kind is EvidenceKind.VALID else selected.speed_mps is None)
def _pedal_valid(pedal: object) -> bool:
return isinstance(pedal, PedalEvidence) and isinstance(pedal.kind, EvidenceKind) and (
(type(pedal.pressed) is bool and ((pedal.ego_mps is None or _ego_speed(pedal.ego_mps)) if not pedal.pressed else _ego_speed(pedal.ego_mps)))
if pedal.kind is EvidenceKind.VALID else pedal.pressed is None and pedal.ego_mps is None)
def _change_valid(change: object) -> bool:
return (isinstance(change, arb.ClassifiedChange) and _token(change.session_id) and _integer(change.effect_id) and
(change.action_id is None or _integer(change.action_id)) and
(change.context_id is None or _token(change.context_id)) and
(change.started_at_ns is None or _integer(change.started_at_ns)) and
(change.previous_mps is None or _speed(change.previous_mps)) and _speed(change.selected_mps) and
isinstance(change.disposition, arb.Disposition))
def _acceptance_valid(decision: object, authority: object, policy: object, session_id: str, now_ns: int) -> bool:
if (not isinstance(decision, acc.Decision) or not isinstance(decision.state, acc.State) or decision.errors or
decision.state.session_id != session_id or decision.state.last_timestamp_ns != now_ns or
not isinstance(authority, acc.Authority) or not isinstance(policy, acc.Policy) or
decision.authority != authority or decision.policy != policy or
not acc.observation_is_valid(decision.state.observation, session_id=session_id)):
return False
if decision.state.observation.kind in (acc.ObservationKind.UNKNOWN, acc.ObservationKind.STALE):
return decision.control_target_mps is None
if policy.display_only or not authority.can_control:
return decision.control_target_mps is None
if decision.control_target_mps is None:
return True
accepted = decision.state.accepted
if (accepted is None or not _token(accepted.session_id) or not _speed(accepted.candidate.speed_mps) or
decision.control_target_mps != accepted.candidate.speed_mps):
return False
if decision.state.observation.kind is acc.ObservationKind.ABSENT:
return policy.fallback_previous and decision.basis == "previous_accepted"
return decision.state.observation.kind is acc.ObservationKind.VALID
def step(state: State, acceptance: acc.Decision, authority: acc.Authority, policy: acc.Policy,
selected: SelectedEvidence, pedal: PedalEvidence, *, now_ns: int,
change: arb.ClassifiedChange | None = None, domain: sd.DomainResolution | None = None) -> Decision:
"""Propose an SLC contribution, never an actuator or stock-button command."""
if not _state_valid(state):
return Decision(state, errors=("invalid override state",))
if state.reset_required:
return Decision(state, basis="session_reset_required", errors=("new session required",))
if (not _integer(now_ns) or (state.last_timestamp_ns is not None and now_ns < state.last_timestamp_ns) or
not _selected_valid(selected) or not _pedal_valid(pedal) or
(change is not None and not _change_valid(change)) or
(domain is not None and (not isinstance(domain, sd.DomainResolution) or not isinstance(domain.status, sd.DomainStatus)))):
failed = replace(state, persistent_selected_mps=None, context=None, active=False, suspension_started_ns=None,
reset_required=True)
return Decision(failed, basis="session_reset_required", errors=("invalid evidence or monotonic clock",))
old = state
state = replace(state, last_timestamp_ns=now_ns)
receipt = None
if change is not None:
if change.session_id != state.session_id:
receipt = EventReceipt(change.effect_id, True, False, "foreign_session")
elif change.effect_id <= state.last_effect_id:
receipt = EventReceipt(change.effect_id, True, False, "already_seen")
else:
state = replace(state, last_effect_id=change.effect_id)
receipt = EventReceipt(change.effect_id, True, False, "ineligible")
if not _acceptance_valid(acceptance, authority, policy, state.session_id, now_ns):
failed = replace(state, persistent_selected_mps=None, active=False, context=None, suspension_started_ns=None,
reset_required=True)
return Decision(failed, basis="session_reset_required", event_receipt=receipt,
errors=("acceptance authority, policy, target or clock mismatch",))
assert isinstance(acceptance, acc.Decision) and isinstance(acceptance.state, acc.State)
assert isinstance(authority, acc.Authority) and isinstance(policy, acc.Policy)
accepted = acceptance.state.accepted
accepted_speed = accepted.candidate.speed_mps if accepted is not None else None
domain_unavailable = False
selected_delta = 0.0
ego_delta = 0.0
effective_target = acceptance.control_target_mps
if domain is not None:
if domain.status is sd.DomainStatus.INVALID:
failed = replace(state, persistent_selected_mps=None, active=False, context=None, suspension_started_ns=None,
reset_required=True)
return Decision(failed, basis="session_reset_required", event_receipt=receipt, errors=("invalid speed domain",))
if domain.status is not sd.DomainStatus.VALID:
if domain.context is not None:
failed = replace(state, persistent_selected_mps=None, active=False, context=None, suspension_started_ns=None,
reset_required=True)
return Decision(failed, basis="session_reset_required", event_receipt=receipt, errors=("invalid speed domain",))
domain_unavailable = True
else:
c = domain.context
if (not isinstance(c, sd.DomainContext) or not sd.context_is_valid(c) or
c.session_id != state.session_id or c.timestamp_ns != now_ns or
c.accepted_raw_mps != acceptance.control_target_mps or selected.speed_mps != c.selected_raw_mps or
(pedal.pressed and pedal.ego_mps != c.ego_raw_mps) or
not _speed(c.effective_cluster_mps)):
failed = replace(state, persistent_selected_mps=None, active=False, context=None, suspension_started_ns=None,
reset_required=True)
return Decision(failed, basis="session_reset_required", event_receipt=receipt, errors=("speed domain mismatch",))
selected_delta = c.selected_delta_mps
ego_delta = c.ego_delta_mps
effective_target = c.effective_cluster_mps
mode_changed = old.mode is not None and (old.mode != authority.mode or old.owner != authority.owner)
if mode_changed:
state = replace(state, persistent_selected_mps=None, suspension_started_ns=None)
if old.accepted_mps is not None and accepted_speed is not None and accepted_speed < old.accepted_mps:
state = replace(state, persistent_selected_mps=None)
if (not domain_unavailable and state.persistent_selected_mps is not None and effective_target is not None and
effective_target >= state.persistent_selected_mps + selected_delta):
state = replace(state, persistent_selected_mps=None)
state = replace(state, accepted_mps=accepted_speed, effective_target_mps=effective_target,
mode=authority.mode, owner=authority.owner)
adoption = acceptance.adoption
adopted_now = False
if adoption is not None and adoption.session_id == state.session_id and adoption.clear_override and acceptance.adoption_receipt is not None:
if (acceptance.adoption_receipt.consumed and acceptance.adoption_receipt.sequence_id == adoption.action_sequence_id and
adoption.action_sequence_id > state.last_adoption_sequence):
state = replace(state, persistent_selected_mps=None, last_adoption_sequence=adoption.action_sequence_id)
adopted_now = True
eligible = (authority.can_control and not policy.display_only and acceptance.control_target_mps is not None and
acceptance.state.observation.kind in (acc.ObservationKind.VALID, acc.ObservationKind.ABSENT) and
selected.kind is EvidenceKind.VALID and pedal.kind is EvidenceKind.VALID and not domain_unavailable)
if not eligible:
# Only temporary inactivity under the same selected mode and owner gets a grace period.
temporary_inactivity = (not mode_changed and not authority.longitudinal_active and
authority.mode in (acc.Mode.LONGITUDINAL_ONLY, acc.Mode.COMBINED) and
authority.owner is acc.LongitudinalOwner.SYSTEM)
if temporary_inactivity:
started = old.suspension_started_ns if old.suspension_started_ns is not None else now_ns
state = replace(state, suspension_started_ns=started)
if now_ns - started >= SUSPENSION_NS:
state = replace(state, persistent_selected_mps=None)
else:
state = replace(state, suspension_started_ns=None)
state = replace(state, active=False, context=None)
return Decision(state, basis="display_only" if policy.display_only else "inactive_or_missing_evidence", event_receipt=receipt)
target = acceptance.control_target_mps
assert target is not None and _speed(target) and selected.speed_mps is not None and pedal.pressed is not None
if state.suspension_started_ns is not None and now_ns - state.suspension_started_ns >= SUSPENSION_NS:
state = replace(state, persistent_selected_mps=None)
state = replace(state, suspension_started_ns=None)
rotate = (not old.active or old.context is None or old.context.accepted_target_mps != target or
old.effective_target_mps != effective_target or
mode_changed or adopted_now)
if rotate:
context = Context(f"override:{state.next_context_id}", now_ns, target)
state = replace(state, context=context, next_context_id=state.next_context_id + 1, active=True)
else:
state = replace(state, active=True)
if receipt is not None and receipt.status == "ineligible" and change is not None:
if policy.display_only:
receipt = replace(receipt, status="display_only")
elif (change.disposition is arb.Disposition.DRIVER_INTENT and change.action_id is not None and
change.previous_mps is not None and change.previous_mps != change.selected_mps and
old.active and old.context is not None and not rotate and change.context_id == old.context.context_id and
change.started_at_ns is not None and old.context.issued_at_ns <= change.started_at_ns <= now_ns and
change.selected_mps == selected.speed_mps):
state = replace(state, persistent_selected_mps=(change.selected_mps if
effective_target is not None and change.selected_mps + selected_delta > effective_target else None))
receipt = replace(receipt, consumed=True, status="driver_intent")
else:
receipt = replace(receipt, status="unqualified_effect")
if pedal.pressed and pedal.ego_mps is not None and effective_target is not None and pedal.ego_mps + ego_delta > effective_target:
return Decision(state, pedal.ego_mps, "pedal", state.context, receipt)
if state.persistent_selected_mps is not None:
return Decision(state, state.persistent_selected_mps, "persistent", state.context, receipt)
return Decision(state, basis="adoption_clear" if adopted_now else "none", context=state.context, event_receipt=receipt)
@@ -0,0 +1,79 @@
"""Card-side qualification for SLC buttons and selected-speed commands."""
import math
from openpilot.cereal.services import SERVICE_LIST
from openpilot.selfdrive.car.cruise import SlcPendingConfirmation
STATE_MAX_AGE_NS = int(2e9 / SERVICE_LIST['slcState'].frequency)
COMMAND_MAX_AGE_NS = int(2e9 / SERVICE_LIST['slcState'].frequency)
def finite_speed(value: object) -> bool:
if type(value) not in (int, float):
return False
assert isinstance(value, (int, float))
return math.isfinite(value) and value > 0
def source_matches(state, observation, producer_session: str, now_ns: int) -> bool:
return (str(state.source) == 'dashboard' and str(state.observationKind) == 'valid' and
str(state.sourceProducerSessionId) == producer_session and producer_session != '' and
getattr(observation.status, 'value', observation.status) == 'valid' and int(state.sourceEpisode) > 0 and
int(state.sourceEpisode) == int(observation.episode) and
int(state.sourceObservedMonoTime) == int(observation.observed_ns) and
int(state.sourceValidUntilMonoTime) == int(observation.valid_until_ns) and
0 < int(observation.observed_ns) <= now_ns <= int(observation.valid_until_ns) and
finite_speed(state.speedLimit) and finite_speed(observation.speed_mps) and
abs(float(state.speedLimit) - float(observation.speed_mps)) <= 0.02)
def state_current(sm, observation, producer_session: str, now_ns: int):
if not (sm.valid['slcState'] and sm.alive['slcState'] and
0 < int(sm.logMonoTime['slcState']) <= now_ns and
now_ns - int(sm.logMonoTime['slcState']) <= STATE_MAX_AGE_NS):
return None
state = sm['slcState']
if not (state.enabled and str(state.sessionId) and state.frameMonoTime <= now_ns and
int(state.frameMonoTime) <= int(sm.logMonoTime['slcState']) and
source_matches(state, observation, producer_session, now_ns)):
return None
return state
def pending_confirmation(sm, observation, producer_session: str, now_ns: int,
*, long_active: bool, pcm_cruise: bool) -> SlcPendingConfirmation | None:
if not long_active or pcm_cruise:
return None
state = state_current(sm, observation, producer_session, now_ns)
if state is None or not state.hasPending or int(state.decisionId) <= 0 or int(state.presentationId) <= 0:
return None
if not finite_speed(state.pendingSpeedLimit) or abs(float(state.pendingSpeedLimit) - float(state.speedLimit)) > 0.02:
return None
return SlcPendingConfirmation(str(state.sessionId), int(state.decisionId), int(state.presentationId))
def command_applicable(command, state, observation, producer_session: str, now_ns: int,
selected_mps: float, *, long_active: bool, pcm_cruise: bool, button_event: bool) -> bool:
if state is None or button_event or pcm_cruise or not long_active or not state.hasAccepted:
return False
if (str(command.kind) != 'adoptAcceptedHigherLimit' or not str(command.sessionId) or
str(command.sessionId) != str(state.sessionId) or int(command.commandId) <= 0 or
int(command.actionId) < 0 or int(command.presentationId) <= 0 or
int(command.commandId) != int(state.commandId) or
int(command.presentationId) != int(state.presentationId) or
int(command.sourceEpisode) != int(state.sourceEpisode) or
str(command.sourceProducerSessionId) != producer_session or
int(command.sourceObservedMonoTime) != int(state.sourceObservedMonoTime) or
int(command.sourceValidUntilMonoTime) != int(state.sourceValidUntilMonoTime) or
not source_matches(state, observation, producer_session, now_ns)):
return False
issued, expires = int(command.issuedMonoTime), int(command.expiresMonoTime)
if not (0 < issued <= now_ns <= expires and expires - issued <= COMMAND_MAX_AGE_NS):
return False
expected, target = float(command.expectedSelectedMps), float(command.targetMps)
return (finite_speed(expected) and finite_speed(target) and finite_speed(selected_mps) and
abs(expected - selected_mps) <= 0.1 and target > selected_mps + 0.1 and
abs(target - (float(state.acceptedSpeedLimit) + float(state.offset))) <= 0.1 and
target <= 145.0 / 3.6)
+544
View File
@@ -0,0 +1,544 @@
"""One plannerd-owned SLC lifecycle over real, typed replayable observations."""
import math
import uuid
from dataclasses import dataclass, replace
import openpilot.cereal.messaging as messaging
from openpilot.cereal.services import SERVICE_LIST
from openpilot.starpilot.speed_limits import acceptance as acc
from openpilot.starpilot.speed_limits import action_arbitration as arb
from openpilot.starpilot.speed_limits import composition as comp
from openpilot.starpilot.speed_limits import lead_relaxation as lr
from openpilot.starpilot.speed_limits import selection as sel
from openpilot.starpilot.speed_limits import speed_domain as sd
from openpilot.starpilot.speed_limits.runtime_settings import Settings
from openpilot.starpilot.speed_limits.map_observation import MapSource, copy_frame
from openpilot.starpilot.speed_limits.vision.observation import clock_pair_ns, vision_observation
def _number(value, *, positive=False) -> bool:
return type(value) in (int, float) and math.isfinite(value) and (value > 0 if positive else value >= 0)
def _fresh(sm, service: str, now_ns: int) -> bool:
timestamp = sm.logMonoTime.get(service, 0)
frequency = SERVICE_LIST[service].frequency
# SubMaster uses ten expected periods for its alive check. Apply the same
# transport window to replay timestamps, which may be held across frames.
max_age_ns = int(10e9 / frequency) if frequency > 0 else 0
return (bool(sm.valid.get(service, False)) and bool(sm.alive.get(service, False)) and
0 < timestamp <= now_ns and now_ns - timestamp <= max_age_ns)
def _paired_card_frame(sm, source_log_ns: int, now_ns: int, *, max_skew_service: str) -> bool:
"""Accept a card event preceding the latest CarState by at most two producer ticks."""
car_log_ns = int(sm.logMonoTime.get('carState', 0))
max_skew_ns = int(2e9 / SERVICE_LIST[max_skew_service].frequency)
return (_fresh(sm, 'carState', now_ns) and 0 < source_log_ns <= car_log_ns <= now_ns and
car_log_ns - source_log_ns <= max_skew_ns)
def dashboard_observation(evidence, *, car_fingerprint: str, session_id: str, now_ns: int) -> acc.Observation:
"""Project an OEM TSR observation; schema default zero is never absence."""
status = str(evidence.status)
timestamp, expiry = int(evidence.observedMonoTime), int(evidence.validUntilMonoTime)
if status == "unknown" or timestamp <= 0:
return acc.Observation(acc.ObservationKind.UNKNOWN)
if timestamp > now_ns or expiry <= timestamp or now_ns > expiry:
return acc.Observation(acc.ObservationKind.STALE)
if status == "stale":
return acc.Observation(acc.ObservationKind.STALE)
if status == "absent":
return acc.Observation(acc.ObservationKind.ABSENT)
if status != "valid" or not _number(evidence.speedMps, positive=True) or int(evidence.episode) <= 0:
return acc.Observation(acc.ObservationKind.UNKNOWN)
producer_session = str(evidence.producerSessionId)
if not producer_session:
return acc.Observation(acc.ObservationKind.UNKNOWN)
identity = acc.ObservationIdentity(acc.IdentityKind.PRODUCER_EPISODE,
value=f"{session_id}:{producer_session}:{car_fingerprint}:{int(evidence.episode)}")
return acc.Observation(acc.ObservationKind.VALID,
acc.Candidate(sel.Source.DASHBOARD.value, identity, float(evidence.speedMps)))
def _lead(sm, now_ns: int) -> lr.LeadEvidence:
if not _fresh(sm, "radarState", now_ns):
return lr.LeadEvidence(lr.LeadKind.STALE if sm.logMonoTime.get("radarState", 0) else lr.LeadKind.UNKNOWN)
lead = sm["radarState"].leadOne
if not lead.present:
return lr.LeadEvidence(lr.LeadKind.VALID, False, False, 0.0, 0.0, 0.0)
if not all(_number(value, positive=(name == "distance")) for name, value in
(("distance", lead.dRel), ("speed", lead.vLeadK))):
return lr.LeadEvidence(lr.LeadKind.UNKNOWN)
if type(lead.aLeadK) not in (int, float) or not math.isfinite(lead.aLeadK):
return lr.LeadEvidence(lr.LeadKind.UNKNOWN)
return lr.LeadEvidence(lr.LeadKind.VALID, True, True, float(lead.dRel), float(lead.vLeadK), float(lead.aLeadK))
def _authority(CP, sm, host_long: bool, lateral: bool) -> acc.Authority:
car_state = sm["carState"]
stock = not bool(CP.openpilotLongitudinalControl) and bool(car_state.cruiseState.enabled)
if bool(CP.openpilotLongitudinalControl) and host_long:
return acc.Authority(acc.Mode.COMBINED if lateral else acc.Mode.LONGITUDINAL_ONLY,
acc.LongitudinalOwner.SYSTEM, lateral, True, False, False)
if stock:
return acc.Authority(acc.Mode.LATERAL_ONLY if lateral else acc.Mode.OFF,
acc.LongitudinalOwner.STOCK, lateral, False, True, False)
return acc.Authority(acc.Mode.LATERAL_ONLY if lateral else acc.Mode.OFF,
acc.LongitudinalOwner.NONE, lateral, False, False, not lateral)
@dataclass(frozen=True)
class Action:
session_id: str
sequence_id: int
decision_id: int
presentation_id: int
kind: str
@dataclass(frozen=True)
class CruiseEvent:
event_id: int
observed_ns: int
previous_mps: float
selected_mps: float
button: str
producer_session: str = 'replay-card'
car_state_log_ns: int = 0
kind: str = 'driverChange'
session_id: str = ''
decision_id: int = 0
presentation_id: int = 0
command_id: int = 0
@dataclass(frozen=True)
class Output:
result: comp.Result
message: object
action_result: arb.Result | None = None
command: object | None = None
settings: Settings | None = None
class Runtime:
"""Own a unique drive session, action ledger and current-frame UI receipt."""
def __init__(self, settings: Settings, *, session_id: str | None = None, vision_enabled: bool = False,
vision_control_qualified: bool = False, vision_display_qualified: bool = False):
self.settings = settings
self.map_source = MapSource()
self.vision_enabled = vision_enabled
# Exact saved Vision opt-in may grant control; source identity and current
# driver confirmation remain independent requirements for each episode.
self.vision_control_qualified = vision_enabled and vision_control_qualified
self.vision_display_qualified = bool(vision_enabled and vision_display_qualified and settings.display and not settings.enabled)
self.vision_diagnostic = acc.Observation(acc.ObservationKind.UNKNOWN)
self._last_vision_producer = ''
self._last_vision_frame = 0
self._last_vision_eof_boot = 0
self._last_vision_episode = 0
self.session_id = session_id or uuid.uuid4().hex
self.state = comp.new_session(self.session_id)
self.ledger = arb.new_ledger(self.session_id)
self.last_action_result: arb.Result | None = None
self.last_ui_sequence = -1
self.last_cruise_event = -1
self.last_cruise_producer: str | None = None
self.last_command_id = 0
self.command_status = ''
self._command_ui_sequence = 0
self._ui_receipt = None
self.command_issued_ns = 0
self.command_expiry_ns = 0
self.command_expected_mps = 0.0
self.command_target_mps = 0.0
self.command_card_producer = ''
def reset(self) -> None:
self.map_source.reset()
self.vision_diagnostic = acc.Observation(acc.ObservationKind.UNKNOWN)
self._last_vision_producer = ''
self._last_vision_frame = 0
self._last_vision_eof_boot = 0
self._last_vision_episode = 0
self.session_id = uuid.uuid4().hex
self.state = comp.new_session(self.session_id)
self.ledger = arb.new_ledger(self.session_id)
self.last_action_result = None
self.last_ui_sequence = -1
self.last_cruise_event = -1
self.last_cruise_producer = None
self.last_command_id = 0
self.command_status = ''
self._command_ui_sequence = 0
self._ui_receipt = None
self.command_issued_ns = 0
self.command_expiry_ns = 0
self.command_expected_mps = 0.0
self.command_target_mps = 0.0
self.command_card_producer = ''
def _event_current(self, event: CruiseEvent, sm, now_ns: int) -> bool:
return (bool(event.producer_session) and 0 < event.observed_ns <= now_ns and
now_ns - event.observed_ns <= int(2e9 / SERVICE_LIST['modelV2'].frequency) and
_paired_card_frame(sm, event.car_state_log_ns, now_ns, max_skew_service='modelV2') and
(event.producer_session != self.last_cruise_producer or event.event_id > self.last_cruise_event))
def _mark_event(self, event: CruiseEvent) -> None:
if event.producer_session != self.last_cruise_producer:
self.last_cruise_producer = event.producer_session
self.last_cruise_event = -1
self.last_cruise_event = event.event_id
def _cruise(self, event: CruiseEvent | None, sm, now_ns: int) -> arb.Result | None:
if (event is None or event.kind != 'driverChange' or not self._event_current(event, sm, now_ns) or
event.button not in ('accel', 'decel') or not _number(event.previous_mps, positive=True) or
not _number(event.selected_mps, positive=True) or event.previous_mps == event.selected_mps or
abs(float(sm['carState'].vCruise) / 3.6 - event.selected_mps) > 0.1):
return None
self._mark_event(event)
action_id = self.ledger.last_action_id + 1
context = self.state.override.context
if context is not None and context.issued_at_ns > event.observed_ns:
return None
context_id = context.context_id if context is not None else 'no-slc-context'
begun = arb.step(self.ledger, arb.BeginAction(self.session_id, action_id, arb.Origin.DRIVER_CRUISE, context_id), now_ns=now_ns)
self.ledger = begun.state
if begun.errors or begun.status != 'begun':
return None
resolved = arb.step(self.ledger, arb.ResolveAction(self.session_id, action_id, arb.Disposition.DRIVER_INTENT), now_ns=now_ns)
self.ledger = resolved.state
if resolved.errors:
return None
classified = arb.step(self.ledger, arb.CruiseChange(self.session_id, self.ledger.last_effect_id + 1, action_id,
event.previous_mps, event.selected_mps), now_ns=now_ns)
self.ledger = classified.state
completed = arb.step(self.ledger, arb.CompleteAction(self.session_id, action_id), now_ns=now_ns)
self.ledger = completed.state
return classified if not classified.errors and classified.change is not None else None
def _physical_action(self, event: CruiseEvent, sm, now_ns: int) -> tuple[acc.DriverAction | None, int | None]:
if (event.kind not in ('confirmationAccept', 'confirmationReject') or
not self._event_current(event, sm, now_ns)):
return None, None
self._mark_event(event)
pending = self.state.acceptance.pending
shown = self.state.acceptance.presentation
if (event.session_id != self.session_id or pending is None or shown is None or
event.producer_session != str(sm['slcDashboardObservation'].producerSessionId) or
event.decision_id != pending.decision_id or event.presentation_id != shown.presentation_id or
pending.candidate != shown.candidate or
event.button != ('accel' if event.kind == 'confirmationAccept' else 'decel')):
return None, None
action_id = self.ledger.last_action_id + 1
begun = arb.step(self.ledger, arb.BeginAction(self.session_id, action_id, arb.Origin.DRIVER_CRUISE,
f'pending:{pending.decision_id}'), now_ns=now_ns)
self.ledger = begun.state
if begun.errors or begun.status != 'begun':
return None, None
kind = acc.ActionKind.ACCEPT if event.kind == 'confirmationAccept' else acc.ActionKind.REJECT
return acc.DriverAction(self.session_id, action_id, pending.decision_id, kind), action_id
def _command_receipt(self, event: CruiseEvent, sm, now_ns: int) -> None:
if (event.kind not in ('commandApplied', 'commandRejected') or
not self._event_current(event, sm, now_ns)):
return
self._mark_event(event)
if (event.session_id == self.session_id and event.command_id == self.last_command_id and
event.producer_session == self.command_card_producer and self.command_status in ('issued', 'expired') and
self.command_issued_ns <= event.observed_ns <= self.command_expiry_ns):
if event.kind == 'commandApplied':
if (not _number(event.previous_mps, positive=True) or
abs(event.previous_mps - self.command_expected_mps) > 0.1 or
not _number(event.selected_mps, positive=True) or
abs(event.selected_mps - self.command_target_mps) > 0.5):
return
self.command_status = 'applied'
else:
self.command_status = 'rejected'
def _observations(self, sm, CP, now_ns: int, now_boot_ns: int | None = None) -> dict[sel.Source, acc.Observation]:
unknown = acc.Observation(acc.ObservationKind.UNKNOWN)
values = dict.fromkeys(sel.Source, unknown)
if (_fresh(sm, "carState", now_ns) and not sm["carState"].canTimeout and
_fresh(sm, 'slcDashboardObservation', now_ns)):
evidence = sm['slcDashboardObservation']
source_log_ns = int(evidence.carStateLogMonoTime)
if (int(sm.logMonoTime['slcDashboardObservation']) == source_log_ns and
_paired_card_frame(sm, source_log_ns, now_ns, max_skew_service='carState')):
values[sel.Source.DASHBOARD] = dashboard_observation(
evidence, car_fingerprint=str(CP.carFingerprint), session_id=self.session_id, now_ns=now_ns)
elif sm.logMonoTime.get("carState", 0) or sm.logMonoTime.get('slcDashboardObservation', 0):
values[sel.Source.DASHBOARD] = acc.Observation(acc.ObservationKind.STALE)
if self.vision_enabled and _fresh(sm, 'slcVisionObservation', now_ns):
pair = clock_pair_ns() if now_boot_ns is None else (now_ns, now_boot_ns)
if pair is not None:
evidence = sm['slcVisionObservation'].vision
self.vision_diagnostic = vision_observation(
evidence, receipt_mono_ns=int(sm.logMonoTime['slcVisionObservation']),
now_mono_ns=pair[0], now_boot_ns=pair[1], car_fingerprint=str(CP.carFingerprint),
drive_session=self.session_id)
if self.vision_diagnostic.kind is acc.ObservationKind.VALID:
producer = str(evidence.producerSessionId)
frame_id, eof_boot, episode = int(evidence.frameId), int(evidence.cameraFrameEofBootTime), int(evidence.episode)
if (producer == self._last_vision_producer and
(frame_id < self._last_vision_frame or eof_boot < self._last_vision_eof_boot or
episode < self._last_vision_episode)):
self.vision_diagnostic = acc.Observation(acc.ObservationKind.STALE)
else:
self._last_vision_producer = producer
self._last_vision_frame = frame_id
self._last_vision_eof_boot = eof_boot
self._last_vision_episode = episode
if self.vision_control_qualified or self.vision_display_qualified:
values[sel.Source.VISION] = self.vision_diagnostic
else:
self.vision_diagnostic = acc.Observation(acc.ObservationKind.STALE)
elif self.vision_enabled and sm.logMonoTime.get('slcVisionObservation', 0):
self.vision_diagnostic = acc.Observation(acc.ObservationKind.STALE)
else:
self.vision_diagnostic = acc.Observation(acc.ObservationKind.UNKNOWN)
if (sel.Source.MAP in self.settings.selection.slots or
self.settings.selection.mode is not sel.SelectionMode.ORDERED):
pair = clock_pair_ns() if now_boot_ns is None else (now_ns, now_boot_ns)
if pair is not None:
received = bool(sm.updated.get('mapdOut', False))
try:
frame = (copy_frame(sm['mapdOut'], event_valid=bool(sm.valid.get('mapdOut', False)),
event_boot_ns=int(sm.logMonoTime.get('mapdOut', 0))) if received else None)
except (AttributeError, ValueError, TypeError, OverflowError):
frame = None # malformed current message revokes held map evidence
values[sel.Source.MAP] = self.map_source.step(frame, received=received,
now_boot_ns=pair[1], transport_alive=bool(sm.alive.get('mapdOut', False)),
drive_session=self.session_id, car_fingerprint=str(CP.carFingerprint))
else:
self.map_source.reset()
values[sel.Source.MAP] = acc.Observation(acc.ObservationKind.STALE)
# Vision still requires its independent source-specific qualification.
return values
def _frame(self, sm, CP, now_ns: int, now_boot_ns: int | None, action: acc.DriverAction | None, adopt: acc.AdoptRequest | None,
classified: arb.Result | None) -> comp.Frame:
car_fresh = _fresh(sm, "carState", now_ns) and not sm["carState"].canTimeout
controls_fresh = _fresh(sm, "controlsState", now_ns)
selfdrive_fresh = _fresh(sm, "selfdriveState", now_ns)
car_control_fresh = _fresh(sm, 'carControl', now_ns)
car_state, controls, selfdrive = sm["carState"], sm["controlsState"], sm["selfdriveState"]
long_active = str(controls.longControlState) != "off" if controls_fresh else False
host_long = bool(CP.openpilotLongitudinalControl and long_active and car_control_fresh and sm['carControl'].longActive and
selfdrive.enabled and car_fresh and selfdrive_fresh)
lateral = bool(car_control_fresh and sm['carControl'].latActive and selfdrive_fresh)
authority = _authority(CP, sm, host_long, lateral)
selected = float(car_state.vCruise) if car_fresh and _number(car_state.vCruise) else 0.0
selected_cluster = float(car_state.vCruiseCluster) if car_fresh and _number(car_state.vCruiseCluster) else None
ego_valid = car_fresh and _number(car_state.vEgo) and _number(car_state.vEgoCluster)
ego = (sd.SpeedPair(sd.DomainStatus.VALID, float(car_state.vEgo), float(car_state.vEgoCluster), self.session_id, now_ns)
if ego_valid else sd.SpeedPair(sd.DomainStatus.STALE if sm.logMonoTime.get("carState", 0) else sd.DomainStatus.UNKNOWN))
host = comp.HostState(bool(CP.openpilotLongitudinalControl), host_long, bool(selfdrive.enabled and selfdrive_fresh),
bool(controls.forceDecel) if controls_fresh else True, False, selected, selected_cluster)
acceptance_policy = (replace(self.settings.acceptance, vision_driver_confirm=True)
if self.vision_control_qualified else self.settings.acceptance)
return comp.Frame(now_ns, self._observations(sm, CP, now_ns, now_boot_ns), self.settings.selection, authority,
acceptance_policy, self.settings.offsets,
sd.DomainStatus.VALID if car_fresh and selected_cluster is not None else sd.DomainStatus.STALE,
ego, bool(car_state.gasPressed) if car_fresh and self.settings.manual_override else False,
_lead(sm, now_ns), self.settings.lead_policy, host, action=action, adopt=adopt,
classified_cruise=classified)
def _action(self, request: Action | None, now_ns: int) -> tuple[acc.DriverAction | None, acc.AdoptRequest | None, int | None]:
if request is None or request.session_id != self.session_id or request.kind not in ("accept", "reject", "adopt"):
return None, None, None
if request.sequence_id <= self.last_ui_sequence:
return None, None, None
self.last_ui_sequence = request.sequence_id
shown = self.state.acceptance.presentation
if request.kind == 'adopt' and (shown is None or shown.presentation_id != request.presentation_id):
return None, None, None
action_id = self.ledger.last_action_id + 1
context = self.state.override.context
context_id = context.context_id if context is not None else "slc-presentation"
begun = arb.step(self.ledger, arb.BeginAction(self.session_id, action_id, arb.Origin.DRIVER_OTHER, context_id),
now_ns=now_ns)
self.ledger = begun.state
if begun.errors:
return None, None, None
if request.kind == "adopt":
assert shown is not None
return None, acc.AdoptRequest(self.session_id, action_id, request.presentation_id, shown.candidate), action_id
kind = acc.ActionKind.ACCEPT if request.kind == "accept" else acc.ActionKind.REJECT
return acc.DriverAction(self.session_id, action_id, request.decision_id, kind), None, action_id
def step(self, sm, CP, *, now_ns: int, now_boot_ns: int | None = None, request: Action | None = None,
cruise_event: CruiseEvent | None = None) -> Output:
old_accepted = self.state.acceptance.accepted
old_pending = self.state.acceptance.pending
if self.command_status == 'issued' and now_ns > self.command_expiry_ns:
self.command_status = 'expired'
classified = self._cruise(cruise_event, sm, now_ns) if request is None else None
if cruise_event is not None and request is None:
self._command_receipt(cruise_event, sm, now_ns)
action, adopt, action_id = self._action(request, now_ns)
if cruise_event is not None and request is None:
action, action_id = self._physical_action(cruise_event, sm, now_ns)
frame = self._frame(sm, CP, now_ns, now_boot_ns, action, adopt, classified)
result = comp.step(self.state, frame)
self.state = result.state
action_result = None
if action_id is not None:
if result.acceptance is not None:
action_result = arb.resolve_acceptance(self.ledger, action_id, result.acceptance, now_ns=now_ns)
self.ledger = action_result.state
self.last_action_result = action_result
completed = arb.step(self.ledger, arb.CompleteAction(self.session_id, action_id), now_ns=now_ns)
self.ledger = completed.state
command = self._command(result, sm, CP, now_ns, old_accepted, old_pending, action, adopt)
message = self._message(now_ns, result, sm, frame.observations)
if command is not None:
self._command_ui_sequence = request.sequence_id if request is not None else 0
message.slcState.commandActionSequenceId = getattr(self, '_command_ui_sequence', 0)
if action_result is not None and action_result.status == 'resolved' and request is not None:
self._ui_receipt = (request.sequence_id, str(message.slcState.actionStatus))
receipt = getattr(self, '_ui_receipt', None)
if receipt is not None:
message.slcState.actionSequenceId, message.slcState.actionStatus = receipt
return Output(result, message, action_result, command, self.settings)
def _command(self, result: comp.Result, sm, CP, now_ns: int, old_accepted, old_pending,
action: acc.DriverAction | None, adopt: acc.AdoptRequest | None):
decision = result.acceptance
if (decision is None or (decision.history_write is None and decision.adoption is None) or result.domain is None or
result.domain.context is None or result.selection is None or
result.selection.selected_source is not sel.Source.DASHBOARD or
(action is None and adopt is None) or
(action is not None and (old_pending is None or action.kind is not acc.ActionKind.ACCEPT))):
return None
target = result.domain.context.effective_cluster_mps
selected_kph = float(sm['carState'].vCruise)
previous_limit = old_accepted.candidate.speed_mps if old_accepted is not None else 0.0
if (not _number(target, positive=True) or not _number(selected_kph, positive=True) or
selected_kph >= 255 or target <= selected_kph / 3.6 + 0.1 or
(action is not None and decision.history_write.candidate.speed_mps <= previous_limit)):
return None
evidence = sm['slcDashboardObservation']
if dashboard_observation(evidence, car_fingerprint=str(CP.carFingerprint),
session_id=self.session_id, now_ns=now_ns).kind is not acc.ObservationKind.VALID:
return None
expiry = min(int(evidence.validUntilMonoTime), now_ns + int(2e9 / SERVICE_LIST['slcState'].frequency))
if expiry <= now_ns:
return None
self.last_command_id += 1
self.command_status = 'issued'
self.command_issued_ns = now_ns
self.command_expiry_ns = expiry
self.command_expected_mps = selected_kph / 3.6
self.command_target_mps = target
self.command_card_producer = str(evidence.producerSessionId)
command = messaging.new_message('slcCruiseCommand')
command.valid = True
command.logMonoTime = now_ns
record = command.slcCruiseCommand
record.kind = 'adoptAcceptedHigherLimit'
record.sessionId = self.session_id
record.commandId = self.last_command_id
record.actionId = action.sequence_id if action is not None else adopt.sequence_id
record.decisionId = old_pending.decision_id if old_pending is not None else 0
record.presentationId = self.state.acceptance.presentation.presentation_id if self.state.acceptance.presentation is not None else 0
record.sourceProducerSessionId = str(evidence.producerSessionId)
record.sourceEpisode = int(evidence.episode)
record.sourceObservedMonoTime = int(evidence.observedMonoTime)
record.sourceValidUntilMonoTime = int(evidence.validUntilMonoTime)
record.issuedMonoTime = now_ns
record.expiresMonoTime = expiry
record.expectedSelectedMps = selected_kph / 3.6
record.targetMps = target
return command
def _message(self, now_ns: int, result: comp.Result, sm, observations=None):
message = messaging.new_message("slcState")
message.logMonoTime = now_ns
message.valid = not result.errors
state = message.slcState
state.sessionId = self.session_id
# Display metadata is copied from the same qualified inputs used by selection.
enabled_sources = (set(self.settings.selection.slots) if self.settings.selection.mode == sel.SelectionMode.ORDERED
else set(sel.PRIMARY_ORDER if sel.Source.VISION in self.settings.selection.slots else sel.PRIMARY_ORDER[:2]))
if self.settings.selection.online_fallback:
enabled_sources.add(sel.Source.ONLINE)
rows = state.init('sourceReadings', len(sel.Source))
for row, source in zip(rows, sel.Source, strict=True):
observation = (observations or {}).get(source)
row.source = source.value
row.enabled = bool(self.settings.display and source in enabled_sources)
row.observationKind = observation.kind.value if observation is not None else 'unknown'
if observation is not None and observation.kind == acc.ObservationKind.VALID and observation.candidate is not None:
row.speedLimit = observation.candidate.speed_mps
state.frameMonoTime = now_ns
state.enabled = self.settings.enabled
state.displayOnly = self.settings.display and not self.settings.enabled
state.observationKind = result.selection.observation.kind.value if result.selection is not None else "unknown"
state.source = result.selection.selected_source.value if result.selection is not None and result.selection.selected_source is not None else "none"
if result.selection is not None and result.selection.observation.candidate is not None:
state.speedLimit = result.selection.observation.candidate.speed_mps
accepted = result.state.acceptance.accepted
if accepted is not None:
state.hasAccepted = True
state.acceptedSpeedLimit = accepted.candidate.speed_mps
state.acceptedSource = accepted.candidate.source
if result.domain is not None and result.domain.context is not None:
state.offset = result.domain.context.offset_mps
if result.ceiling is not None and self.settings.enabled:
state.hasCeiling = True
state.effectiveCap = result.ceiling.speed_mps
# Publish the qualified domain coordinate; a raw cap is not a cluster value.
context = result.domain.context if result.domain is not None else None
if context is not None:
state.hasEffectiveClusterTarget = True
state.effectiveClusterTarget = context.effective_cluster_mps
state.isLimitingMaxSet = bool(state.hasCeiling and result.ceiling.speed_mps < context.selected_raw_mps)
# Override state.active means the override context is eligible, even with no
# driver override. Only pedal/persistent contributions actually replace the cap.
state.driverOverrideActive = bool(result.override is not None and result.override.contribution_mps is not None)
pending = result.state.acceptance.pending
if pending is not None:
state.hasPending = True
state.pendingSpeedLimit = pending.candidate.speed_mps
state.pendingSource = pending.candidate.source
state.decisionId = pending.decision_id
state.overrideBasis = (result.override.basis if state.driverOverrideActive else 'none')
state.retainedOverride = bool(result.state.override.persistent_selected_mps is not None)
shown = result.state.acceptance.presentation
if state.observationKind == 'valid':
for candidate, value_field, flag_field in (
(accepted.candidate if accepted is not None else None, 'acceptedAdjustedLimit', 'hasAcceptedAdjustedLimit'),
(pending.candidate if pending is not None else None, 'pendingAdjustedLimit', 'hasPendingAdjustedLimit'),
(shown.candidate if shown is not None else None, 'presentationAdjustedLimit', 'hasPresentationAdjustedLimit'),
):
value = sd.adjusted_limit(candidate.speed_mps, self.settings.offsets) if candidate is not None else None
if value is not None:
setattr(state, value_field, value)
setattr(state, flag_field, True)
if shown is not None:
state.presentationId = shown.presentation_id
state.status = result.status
if state.source == 'dashboard' and state.observationKind == 'valid':
evidence = sm['slcDashboardObservation']
state.sourceProducerSessionId = str(evidence.producerSessionId)
state.sourceEpisode = int(evidence.episode)
state.sourceObservedMonoTime = int(evidence.observedMonoTime)
state.sourceValidUntilMonoTime = int(evidence.validUntilMonoTime)
elif state.source == 'vision' and state.observationKind == 'valid' and (self.vision_control_qualified or self.vision_display_qualified):
evidence = sm['slcVisionObservation'].vision
state.sourceProducerSessionId = str(evidence.producerSessionId)
state.sourceEpisode = int(evidence.episode)
state.sourceObservedMonoTime = int(evidence.observedMonoTime)
state.sourceValidUntilMonoTime = int(evidence.validUntilMonoTime)
state.commandId = self.last_command_id
state.commandStatus = self.command_status
receipt = result.acceptance.action_receipt or result.acceptance.adoption_receipt if result.acceptance else None
if receipt is not None:
state.actionSequenceId = receipt.sequence_id
state.actionStatus = receipt.status
return message
@@ -0,0 +1,141 @@
"""Reviewed saved-setting mapping for the opt-in system-longitudinal SLC runtime."""
import math
from collections.abc import Mapping
from dataclasses import dataclass
from pathlib import Path
from openpilot.starpilot.speed_limits import acceptance as acc
from openpilot.starpilot.speed_limits import lead_relaxation as lr
from openpilot.starpilot.speed_limits import offset_document as od
from openpilot.starpilot.speed_limits import selection as sel
from openpilot.starpilot.speed_limits import speed_domain as sd
MPH_TO_MPS = od.MPH_TO_MPS
KPH_TO_MPS = od.KPH_TO_MPS
IMPERIAL_BOUNDS = od.IMPERIAL_BOUNDS
METRIC_BOUNDS = od.METRIC_BOUNDS
LABELS = {"Dashboard": sel.Source.DASHBOARD, "Map Data": sel.Source.MAP, "Maps": sel.Source.MAP, "Vision": sel.Source.VISION}
@dataclass(frozen=True)
class Settings:
enabled: bool
display: bool
selection: sel.SelectionPolicy
acceptance: acc.Policy
offsets: sd.OffsetSchedule
manual_override: bool
set_speed_override: bool
lead_policy: lr.Policy
errors: tuple[str, ...] = ()
# Preserve the validated saved choice for same-drive advisory consumers.
# Append after errors so existing positional Settings construction is stable.
fallback_choice: int = 2
def _bool(values: Mapping[str, object], key: str, default: bool = False) -> bool:
value = values.get(key, default)
if type(value) is not bool:
raise ValueError(f"{key} must be Boolean")
return value
def _number(values: Mapping[str, object], key: str, default: float = 0.0) -> float:
value = values.get(key, default)
if (type(value) is not int and type(value) is not float) or not math.isfinite(value):
raise ValueError(f"{key} must be finite")
return float(value)
def _disabled(reason: str) -> Settings:
selection = sel.SelectionPolicy(sel.SelectionMode.ORDERED, (sel.Source.DASHBOARD, sel.Source.MAP), False)
return Settings(False, False, selection, acc.Policy(display_only=True),
sd.OffsetSchedule((sd.OffsetBand(0.0, None, 0.0),)), False, False,
lr.Policy(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (0.0, 1.0), (1.0, 1.0)), (reason,))
def parse(values: Mapping[str, object]) -> Settings:
"""Translate saved labels without changing their meaning or enabling unsupported output."""
try:
control = _bool(values, "SpeedLimitController")
display = control or _bool(values, "ShowSpeedLimits", True)
document = od.validate(values['SLCOffsetSchedule']) if 'SLCOffsetSchedule' in values else None
if document is od.NEEDS_REVIEW:
raise ValueError('saved SLC offsets need review')
metric = _bool(values, "IsMetric") if document is None else False # presentation units do not own adopted control
first = values.get("SLCPriority1", "Vision")
second = values.get("SLCPriority2", "Dashboard")
if type(first) is not str or type(second) is not str:
raise ValueError("invalid saved SLC source priority")
mode = {"Highest": sel.SelectionMode.HIGHEST, "Lowest": sel.SelectionMode.LOWEST}.get(first, sel.SelectionMode.ORDERED)
if first not in (*LABELS, "Highest", "Lowest") or second not in LABELS:
raise ValueError("invalid saved SLC source priority")
slots = (LABELS.get(first), LABELS[second])
fallback = values.get("SLCFallback", 2)
override = values.get("SLCOverride", 1)
if type(fallback) is not int or fallback not in (0, 1, 2):
raise ValueError("invalid saved SLC fallback")
if type(override) is not int or override not in (0, 1, 2):
raise ValueError("invalid saved SLC override")
# Only accepted-history fallback belongs to this control domain. Saved set
# speed and experimental fallback choices never create a source observation.
confirmation = _bool(values, "SLCConfirmation")
policy = acc.Policy(confirm_lower=confirmation and _bool(values, "SLCConfirmationLower"),
confirm_higher=confirmation and _bool(values, "SLCConfirmationHigher"),
fallback_previous=fallback == 2, display_only=not control)
offsets = (document if isinstance(document, od.OffsetDocument) else
od.adopt_legacy(metric, tuple(_number(values, f"Offset{i + 1}") for i in range(7)))).schedule()
lead_policy = lr.Policy(20 * MPH_TO_MPS, 30.0, 1.2, 0.35, 0.25, 0.001, 0.05,
tuple(v * MPH_TO_MPS for v in (0.0, 5.0, 10.0, 15.0)),
(0.7, 0.9, 1.15, 1.35))
# SLCOverride was display-only; both pedal and selected-speed overrides remain active.
return Settings(control, display, sel.SelectionPolicy(mode, slots, False), policy, offsets,
control, control, lead_policy, fallback_choice=fallback)
except (ValueError, OverflowError) as error:
return _disabled(str(error))
SAVED_KEYS = ("SpeedLimitController", "ShowSpeedLimits", "IsMetric", "SLCPriority1", "SLCPriority2",
"SLCFallback", "SLCOverride", "SLCConfirmation", "SLCConfirmationHigher", "SLCConfirmationLower",
*(f"Offset{i}" for i in range(1, 8)))
def read_params(params) -> Settings:
values = {}
try:
with Path(params.get_param_path('SLCOffsetSchedule')).open('rb') as stream:
raw_document = stream.read(od.MAX_DOCUMENT_BYTES + 1)
except FileNotFoundError:
raw_document = None
except OSError:
return _disabled('Invalid saved SLCOffsetSchedule')
if raw_document is not None:
try:
values['SLCOffsetSchedule'] = od.decode(raw_document)
except (ValueError, OverflowError):
return _disabled('Invalid saved SLCOffsetSchedule')
for key in SAVED_KEYS:
if raw_document is not None and (key == 'IsMetric' or key.startswith('Offset')):
continue
# Typed Params reads turn malformed numbers/strings into absence and accept
# any non-"1" Boolean as false. Neither may silently alter confirmation or
# units. Decode the saved bytes explicitly; only an absent file uses defaults.
try:
raw = Path(params.get_param_path(key)).read_bytes()
if key.startswith('Offset'):
value = float(raw)
elif key in ('SLCFallback', 'SLCOverride'):
value = int(raw)
elif key in ('SLCPriority1', 'SLCPriority2'):
value = raw.decode('utf-8')
else:
if raw not in (b'0', b'1'):
raise ValueError('invalid Boolean')
value = raw == b'1'
values[key] = value
except FileNotFoundError:
continue
except (OSError, ValueError, OverflowError):
return _disabled(f'Invalid saved {key}')
return parse(values)
@@ -0,0 +1,129 @@
"""Select an eligible observation without accepting it or authorizing control."""
from collections.abc import Mapping
from dataclasses import dataclass
from enum import StrEnum
from openpilot.starpilot.speed_limits.acceptance import Observation, ObservationKind, observation_is_valid
class Source(StrEnum):
DASHBOARD = "dashboard"
MAP = "map"
VISION = "vision"
ONLINE = "online"
class SelectionMode(StrEnum):
ORDERED = "ordered"
HIGHEST = "highest"
LOWEST = "lowest"
@dataclass(frozen=True)
class SelectionPolicy:
mode: SelectionMode
slots: tuple[Source | None, Source | None]
online_fallback: bool
@dataclass(frozen=True)
class Selection:
observation: Observation
selected_source: Source | None
reason: str
considered: tuple[Source, ...] = ()
unavailable: tuple[tuple[Source, ObservationKind], ...] = ()
below_minimum: tuple[Source, ...] = ()
errors: tuple[str, ...] = ()
MINIMUM_LIMIT_MPS = 1.0
PRIMARY_ORDER = (Source.DASHBOARD, Source.MAP, Source.VISION)
def select_limit(observations: Mapping[Source, Observation], policy: SelectionPolicy) -> Selection:
"""Use valid configured sources, then an optional online fallback.
The caller supplies normalized, eligible, source-specific observations. A
missing producer is UNKNOWN, not ABSENT. Selection does not infer freshness,
vision support, map lookahead, road identity or provider request permission.
"""
errors = _input_errors(observations, policy)
if errors:
return Selection(Observation(ObservationKind.UNKNOWN), None, "invalid_input", errors=tuple(errors))
if policy.mode == SelectionMode.ORDERED:
considered = tuple(dict.fromkeys(source for source in policy.slots if source is not None))
else:
considered = PRIMARY_ORDER if Source.VISION in policy.slots else PRIMARY_ORDER[:2]
unavailable = []
below_minimum = []
eligible = []
def consider(source):
observation = observations[source]
if observation.kind == ObservationKind.VALID:
if observation.candidate.speed_mps >= MINIMUM_LIMIT_MPS:
eligible.append(source)
return
below_minimum.append(source)
unavailable.append((source, ObservationKind.ABSENT))
else:
unavailable.append((source, observation.kind))
for source in considered:
consider(source)
if eligible:
if policy.mode == SelectionMode.ORDERED:
selected = eligible[0]
else:
# Stable source ordering makes equal-limit ties independent of input map order.
choose = max if policy.mode == SelectionMode.HIGHEST else min
selected = choose(eligible, key=lambda source: observations[source].candidate.speed_mps)
reason = "primary"
elif policy.online_fallback:
considered += (Source.ONLINE,)
consider(Source.ONLINE)
selected = eligible[0] if eligible else None
reason = "online_fallback" if selected is not None else "no_candidate"
else:
selected = None
reason = "no_candidate"
if selected is not None:
observation = observations[selected]
else:
kinds = {kind for _, kind in unavailable}
# Preserve incomplete evidence so an acceptance reducer cannot silently use
# previous-limit fallback after a stale/unknown producer was discarded here.
kind = (ObservationKind.UNKNOWN if ObservationKind.UNKNOWN in kinds else
ObservationKind.STALE if ObservationKind.STALE in kinds else ObservationKind.ABSENT)
observation = Observation(kind)
return Selection(observation, selected, reason, considered, tuple(unavailable), tuple(below_minimum))
def _input_errors(observations, policy):
errors = []
if not isinstance(policy, SelectionPolicy):
return ["policy must be SelectionPolicy"]
if not isinstance(policy.mode, SelectionMode):
errors.append("selection mode must be explicit")
if (not isinstance(policy.slots, tuple) or len(policy.slots) != 2 or
any(source is not None and (not isinstance(source, Source) or source == Source.ONLINE) for source in policy.slots)):
errors.append("two primary source slots are required; online is fallback only")
if type(policy.online_fallback) is not bool:
errors.append("online fallback must be Boolean")
if not isinstance(observations, Mapping):
return errors + ["observations must be a source mapping"]
if len(observations) != len(Source) or any(not isinstance(source, Source) for source in observations) or set(observations) != set(Source):
return errors + ["every source requires an explicit observation"]
for source, observation in observations.items():
if not observation_is_valid(observation):
errors.append(f"{source.value}: invalid observation")
continue
candidate = observation.candidate
if candidate is not None and candidate.source != source.value:
errors.append(f"{source.value}: candidate source mismatch")
return errors
@@ -0,0 +1,219 @@
"""Pure offset selection and qualified raw/cluster coordinate conversion for SLC."""
import math
from dataclasses import dataclass
from enum import Enum
from openpilot.starpilot.speed_limits import acceptance as acc
class DomainStatus(Enum):
VALID = "valid"
ABSENT = "absent"
UNKNOWN = "unknown"
STALE = "stale"
UNAVAILABLE = "unavailable"
INVALID = "invalid"
@dataclass(frozen=True)
class SpeedPair:
status: DomainStatus
raw_mps: float | None = None
cluster_mps: float | None = None
session_id: str | None = None
timestamp_ns: int | None = None
@dataclass(frozen=True)
class OffsetBand:
lower_mps: float
upper_mps: float | None
offset_mps: float
@dataclass(frozen=True)
class OffsetSchedule:
bands: tuple[OffsetBand, ...]
@dataclass(frozen=True)
class DomainContext:
session_id: str
timestamp_ns: int
accepted_raw_mps: float
offset_mps: float
effective_cluster_mps: float
selected_raw_mps: float
selected_delta_mps: float
ego_raw_mps: float
ego_delta_mps: float
@dataclass(frozen=True)
class DomainResolution:
status: DomainStatus
context: DomainContext | None = None
reason: str = ""
@dataclass(frozen=True)
class PlannerCoordinate:
cap_mps: float | None
basis: str
cluster_target_mps: float | None = None
ego_delta_mps: float | None = None
def _number(value: object, *, positive: bool = False) -> bool:
if type(value) not in (int, float):
return False
assert isinstance(value, (int, float))
try:
return math.isfinite(value) and (value > 0 if positive else value >= 0)
except OverflowError:
return False
def _signed_number(value: object) -> bool:
if type(value) not in (int, float):
return False
assert isinstance(value, (int, float))
try:
return math.isfinite(value)
except OverflowError:
return False
def _pair_valid(pair: object, *, positive_raw: bool) -> bool:
if not isinstance(pair, SpeedPair) or not isinstance(pair.status, DomainStatus):
return False
if pair.status is not DomainStatus.VALID:
return pair.raw_mps is None and pair.cluster_mps is None
return (_number(pair.raw_mps, positive=positive_raw) and _number(pair.cluster_mps) and
pair.cluster_mps is not None and pair.raw_mps is not None and
type(pair.session_id) is str and bool(pair.session_id) and
type(pair.timestamp_ns) is int and pair.timestamp_ns >= 0)
def selected_pair_from_kph(raw_mps: float, cluster_kph: float, *, session_id: str, timestamp_ns: int) -> SpeedPair:
"""Convert a qualified dashboard-selected cluster value from km/h to m/s."""
if not _number(raw_mps, positive=True) or not _number(cluster_kph):
return SpeedPair(DomainStatus.INVALID)
return SpeedPair(DomainStatus.VALID, raw_mps, cluster_kph / 3.6, session_id, timestamp_ns)
def context_is_valid(context: object) -> bool:
if not isinstance(context, DomainContext):
return False
return (type(context.session_id) is str and bool(context.session_id) and type(context.timestamp_ns) is int and
context.timestamp_ns >= 0 and _number(context.accepted_raw_mps, positive=True) and
_signed_number(context.offset_mps) and _number(context.effective_cluster_mps, positive=True) and
context.effective_cluster_mps == context.accepted_raw_mps + context.offset_mps and
_number(context.selected_raw_mps, positive=True) and _number(context.selected_delta_mps) and
_number(context.ego_raw_mps) and _number(context.ego_delta_mps))
def _schedule_valid(schedule: object) -> bool:
if not isinstance(schedule, OffsetSchedule) or not isinstance(schedule.bands, tuple) or not schedule.bands:
return False
previous_upper = None
for i, band in enumerate(schedule.bands):
if not isinstance(band, OffsetBand) or not _number(band.lower_mps) or not _signed_number(band.offset_mps):
return False
if band.upper_mps is not None and (not _number(band.upper_mps, positive=True) or band.upper_mps <= band.lower_mps):
return False
if i and (previous_upper is None or band.lower_mps < previous_upper):
return False
previous_upper = band.upper_mps
return True
def offset_for_limit(speed_mps: float, schedule: OffsetSchedule) -> float | None:
if not _number(speed_mps, positive=True) or not _schedule_valid(schedule):
return None
for band in schedule.bands:
if band.lower_mps <= speed_mps and (band.upper_mps is None or speed_mps < band.upper_mps):
return band.offset_mps
return None
def adjusted_limit(speed_mps: float, schedule: OffsetSchedule) -> float | None:
"""Informational cluster limit using the same validated bands as control."""
offset = offset_for_limit(speed_mps, schedule)
if offset is None:
return None
value = speed_mps + offset
return value if _number(value, positive=True) else None
def resolve(decision: acc.Decision, schedule: OffsetSchedule, selected: SpeedPair, ego: SpeedPair) -> DomainResolution:
"""Bind an explicit offset and contemporaneous speed pairs to an acceptance output."""
if (not isinstance(decision, acc.Decision) or not isinstance(decision.state, acc.State) or decision.errors or
not _schedule_valid(schedule) or not _pair_valid(selected, positive_raw=True) or
not _pair_valid(ego, positive_raw=False) or
type(decision.state.session_id) is not str or not decision.state.session_id or
type(decision.state.last_timestamp_ns) is not int or decision.state.last_timestamp_ns < 0 or
not isinstance(decision.authority, acc.Authority) or
not acc.authority_is_valid(decision.authority) or not isinstance(decision.policy, acc.Policy) or
any(type(value) is not bool for value in (decision.policy.confirm_lower, decision.policy.confirm_higher,
decision.policy.fallback_previous, decision.policy.display_only)) or
not acc.observation_is_valid(decision.state.observation, session_id=decision.state.session_id)):
return DomainResolution(DomainStatus.INVALID, reason="invalid acceptance, schedule or speed pair")
accepted = decision.state.accepted
if accepted is not None and (not isinstance(accepted, acc.AcceptedLimit) or
type(accepted.session_id) is not str or not accepted.session_id or
type(accepted.accepted_at_ns) is not int or accepted.accepted_at_ns < 0 or
not acc.observation_is_valid(acc.Observation(acc.ObservationKind.VALID, accepted.candidate),
session_id=decision.state.session_id)):
return DomainResolution(DomainStatus.INVALID, reason="invalid accepted limit")
if selected.status is DomainStatus.INVALID or ego.status is DomainStatus.INVALID:
return DomainResolution(DomainStatus.INVALID, reason="invalid speed coordinate")
if ((selected.status is DomainStatus.VALID and (selected.session_id != decision.state.session_id or
selected.timestamp_ns != decision.state.last_timestamp_ns)) or
(ego.status is DomainStatus.VALID and (ego.session_id != decision.state.session_id or
ego.timestamp_ns != decision.state.last_timestamp_ns))):
return DomainResolution(DomainStatus.INVALID, reason="speed pair session or timestamp mismatch")
if decision.control_target_mps is None or selected.status is not DomainStatus.VALID or ego.status is not DomainStatus.VALID:
return DomainResolution(DomainStatus.UNAVAILABLE, reason="target or coordinate evidence unavailable")
target = decision.control_target_mps
if (not _number(target, positive=True) or accepted is None or
accepted.candidate.speed_mps != target or not decision.authority.can_control or
decision.policy.display_only or
decision.state.observation.kind in (acc.ObservationKind.UNKNOWN, acc.ObservationKind.STALE) or
(decision.state.observation.kind is acc.ObservationKind.ABSENT and
(not decision.policy.fallback_previous or decision.basis != "previous_accepted"))):
return DomainResolution(DomainStatus.INVALID, reason="accepted target mismatch")
assert selected.raw_mps is not None and selected.cluster_mps is not None
assert ego.raw_mps is not None and ego.cluster_mps is not None
offset = offset_for_limit(target, schedule)
if offset is None:
return DomainResolution(DomainStatus.UNAVAILABLE, reason="no configured offset band")
effective = target + offset
if not _number(effective, positive=True):
return DomainResolution(DomainStatus.INVALID, reason="nonpositive effective target")
return DomainResolution(DomainStatus.VALID, DomainContext(
decision.state.session_id, decision.state.last_timestamp_ns, target, offset, effective,
selected.raw_mps, max(0.0, selected.cluster_mps - selected.raw_mps),
ego.raw_mps, max(0.0, ego.cluster_mps - ego.raw_mps)))
def to_planner_coordinate(context: DomainContext, contribution_raw_mps: float | None, basis: str) -> PlannerCoordinate:
"""Apply a raw override in cluster space, then remove the ego delta exactly once."""
if (not context_is_valid(context) or basis not in ("none", "adoption_clear", "pedal", "persistent") or
(basis in ("none", "adoption_clear")) != (contribution_raw_mps is None)):
return PlannerCoordinate(None, "invalid")
if contribution_raw_mps is not None and not _number(contribution_raw_mps, positive=True):
return PlannerCoordinate(None, "invalid")
cluster_target = context.effective_cluster_mps
if basis == "pedal":
if contribution_raw_mps != context.ego_raw_mps:
return PlannerCoordinate(None, "invalid")
cluster_target = max(cluster_target, context.ego_raw_mps + context.ego_delta_mps)
elif basis == "persistent":
assert contribution_raw_mps is not None
cluster_target = max(cluster_target, contribution_raw_mps + context.selected_delta_mps)
cap = cluster_target - context.ego_delta_mps
if not _number(cap, positive=True):
return PlannerCoordinate(None, "nonpositive", cluster_target, context.ego_delta_mps)
return PlannerCoordinate(cap, basis, cluster_target, context.ego_delta_mps)
@@ -0,0 +1,690 @@
import itertools
import math
import unittest
from dataclasses import FrozenInstanceError, replace
from typing import Any, cast
from openpilot.starpilot.speed_limits.acceptance import (
CONFIRMATION_NS, MAX_REJECTIONS, AcceptedLimit, ActionKind, AdoptRequest, Authority, Candidate, DriverAction,
IdentityKind, LongitudinalOwner, Mode, Observation, ObservationIdentity, ObservationKind, Policy, State, new_session, step,
)
NS = 1_000_000_000
def geographic(value: str) -> ObservationIdentity:
return ObservationIdentity(IdentityKind.GEOGRAPHIC, value=value)
HIGH = Candidate("dashboard", geographic("road-1"), 29.0576)
LOW = Candidate("dashboard", geographic("road-2"), 20.1168)
ACTIVE = Authority(Mode.LONGITUDINAL_ONLY, LongitudinalOwner.SYSTEM, False, True, False, False)
PAUSED = replace(ACTIVE, longitudinal_active=False)
DISENGAGED = replace(PAUSED, fully_disengaged=True)
ABSENT = Observation(ObservationKind.ABSENT)
DEFAULT_POLICY = Policy()
def valid(candidate: Candidate) -> Observation:
return Observation(ObservationKind.VALID, candidate)
def saved(candidate: Candidate = HIGH) -> State:
return new_session("drive", AcceptedLimit(candidate, "previous-drive", 0))
class TestAcceptance(unittest.TestCase):
def test_vision_requires_current_driver_confirmation_across_disengage_and_source_loss(self):
policy = Policy(confirm_lower=False, confirm_higher=False, fallback_previous=True,
vision_driver_confirm=True)
first_sign = Candidate('vision', ObservationIdentity(IdentityKind.PRODUCER_EPISODE,
value='drive:ioniq:camera:1'), 22.0)
next_sign = replace(first_sign, observation_identity=ObservationIdentity(
IdentityKind.PRODUCER_EPISODE, value='drive:ioniq:camera:2'), speed_mps=17.0)
stopped = step(new_session('drive'), valid(first_sign), DISENGAGED, policy, now_ns=0)
self.assertIsNone(stopped.history_write)
self.assertIsNone(stopped.state.accepted)
engaged = step(stopped.state, valid(first_sign), ACTIVE, policy, now_ns=NS)
self.assertEqual(engaged.basis, 'pending')
self.assertIsNone(engaged.control_target_mps)
action = DriverAction('drive', 1, engaged.state.pending.decision_id, ActionKind.ACCEPT)
confirmed = step(engaged.state, valid(first_sign), ACTIVE, policy, now_ns=2 * NS, action=action)
self.assertEqual(confirmed.basis, 'driver_accept')
self.assertEqual(confirmed.control_target_mps, 22.0)
continued = step(confirmed.state, valid(first_sign), ACTIVE, policy, now_ns=3 * NS)
self.assertEqual(continued.control_target_mps, 22.0)
changed = step(continued.state, valid(next_sign), ACTIVE, policy, now_ns=4 * NS)
self.assertIsNone(changed.control_target_mps)
self.assertIsNone(changed.state.accepted)
lost = step(changed.state, Observation(ObservationKind.STALE), ACTIVE, policy, now_ns=5 * NS)
returned = step(lost.state, valid(first_sign), ACTIVE, policy, now_ns=6 * NS)
self.assertEqual(returned.basis, 'pending')
self.assertIsNone(returned.control_target_mps)
def pending(self, candidate=LOW, authority=ACTIVE, policy=DEFAULT_POLICY, state=None, now_ns=0):
decision = step(saved() if state is None else state, valid(candidate), authority, policy, now_ns=now_ns)
self.assertEqual(decision.errors, ())
self.assertIsNotNone(decision.state.pending)
return decision
def act(self, state, kind, *, now_ns=NS, sequence=0, authority=ACTIVE, policy=DEFAULT_POLICY):
self.assertIsNotNone(state.pending)
action = DriverAction(state.session_id, sequence, state.pending.decision_id, kind)
return step(state, state.observation, authority, policy, now_ns=now_ns, action=action)
def adopt_request(self, state, sequence=0):
self.assertIsNotNone(state.presentation)
return AdoptRequest(state.session_id, sequence, state.presentation.presentation_id, state.presentation.candidate)
def test_reject_then_source_loss_never_activates_rejected_limit(self):
policy = Policy(fallback_previous=True)
initial = step(new_session("drive"), valid(HIGH), DISENGAGED, policy, now_ns=0)
self.assertEqual(initial.history_write.candidate, HIGH)
proposed = self.pending(state=initial.state, policy=policy, now_ns=NS)
self.assertEqual(proposed.control_target_mps, HIGH.speed_mps)
rejected = self.act(proposed.state, ActionKind.REJECT, policy=policy, now_ns=2 * NS)
lost = step(rejected.state, ABSENT, ACTIVE, policy, now_ns=3 * NS)
returned = step(lost.state, valid(LOW), ACTIVE, policy, now_ns=4 * NS)
for decision in (rejected, lost, returned):
self.assertEqual(decision.control_target_mps, HIGH.speed_mps)
self.assertEqual(decision.state.accepted, initial.history_write)
self.assertIsNone(decision.state.pending)
self.assertIsNone(decision.history_write)
self.assertIn(LOW, decision.state.rejected)
self.assertTrue(rejected.action_receipt.consumed)
self.assertEqual(returned.basis, "rejected")
def test_absence_fallback_is_explicit_unknown_and_stale_suppress_it(self):
for kind, fallback in itertools.product((ObservationKind.ABSENT, ObservationKind.UNKNOWN, ObservationKind.STALE), (False, True)):
with self.subTest(kind=kind, fallback=fallback):
result = step(saved(), Observation(kind), ACTIVE, Policy(fallback_previous=fallback), now_ns=0)
self.assertEqual(result.control_target_mps, HIGH.speed_mps if kind is ObservationKind.ABSENT and fallback else None)
self.assertIsNone(result.display_candidate)
self.assertIsNone(result.history_write)
result = step(new_session("drive"), ABSENT, ACTIVE, Policy(fallback_previous=True), now_ns=0)
self.assertIsNone(result.control_target_mps)
def test_unknown_stale_and_absence_clear_pending_and_restart_decision_identity(self):
for kind in (ObservationKind.UNKNOWN, ObservationKind.STALE, ObservationKind.ABSENT):
with self.subTest(kind=kind):
before = self.pending()
missing = step(before.state, Observation(kind), ACTIVE, Policy(), now_ns=NS)
after = self.pending(state=missing.state, now_ns=2 * NS)
self.assertIsNone(missing.state.pending)
self.assertNotEqual(before.state.pending.decision_id, after.state.pending.decision_id)
self.assertEqual(after.state.pending.active_elapsed_ns, 0)
late = DriverAction("drive", 1, before.state.pending.decision_id, ActionKind.ACCEPT)
result = step(after.state, valid(LOW), ACTIVE, Policy(), now_ns=3 * NS, action=late)
self.assertFalse(result.action_receipt.consumed)
self.assertEqual(result.action_receipt.status, "different_decision")
self.assertEqual(result.state.accepted.candidate, HIGH)
def test_rejection_survives_other_sources_and_fully_disengaged_return(self):
rejected = self.act(self.pending().state, ActionKind.REJECT)
other = Candidate("map", geographic("another-road"), 25.0)
accepted_other = step(rejected.state, valid(other), DISENGAGED, Policy(), now_ns=2 * NS)
returned = step(accepted_other.state, valid(LOW), DISENGAGED, Policy(), now_ns=3 * NS)
self.assertIn(LOW, returned.state.rejected)
self.assertEqual(returned.state.accepted.candidate, other)
self.assertEqual(returned.basis, "rejected")
self.assertIsNone(returned.history_write)
self.assertIsNone(returned.control_target_mps)
def test_same_speed_new_zone_does_not_reuse_rejection(self):
rejected = self.act(self.pending().state, ActionKind.REJECT)
next_zone = replace(LOW, observation_identity=geographic("road-3"))
result = self.pending(candidate=next_zone, state=rejected.state, now_ns=2 * NS)
self.assertEqual(result.state.pending.candidate, next_zone)
self.assertIn(LOW, result.state.rejected)
def test_rejected_limit_from_another_source_still_requires_confirmation(self):
for identity in (LOW.observation_identity, geographic("map-road")):
with self.subTest(identity=identity):
rejected = self.act(self.pending().state, ActionKind.REJECT)
mapped = replace(LOW, source="map", observation_identity=identity)
proposed = self.pending(candidate=mapped, state=rejected.state, now_ns=2 * NS)
self.assertEqual(proposed.control_target_mps, HIGH.speed_mps)
self.assertEqual(proposed.state.accepted.candidate, HIGH)
self.assertIsNone(proposed.history_write)
returned = step(proposed.state, valid(LOW), ACTIVE, Policy(), now_ns=3 * NS)
self.assertEqual(returned.basis, "rejected")
self.assertIsNone(returned.state.pending)
self.assertEqual(returned.control_target_mps, HIGH.speed_mps)
self.assertEqual(returned.state.accepted.candidate, HIGH)
def test_changed_pending_candidate_starts_new_clock_and_id(self):
first = self.pending()
progressed = step(first.state, valid(LOW), ACTIVE, Policy(), now_ns=29 * NS)
changed = self.pending(candidate=replace(LOW, speed_mps=19.0), state=progressed.state, now_ns=40 * NS)
self.assertNotEqual(changed.state.pending.decision_id, first.state.pending.decision_id)
self.assertEqual(changed.state.pending.active_elapsed_ns, 0)
def test_confirmation_expires_at_thirty_active_seconds(self):
pending = self.pending()
before = step(pending.state, valid(LOW), ACTIVE, Policy(), now_ns=CONFIRMATION_NS - 1)
self.assertIsNotNone(before.state.pending)
expired = step(before.state, valid(LOW), ACTIVE, Policy(), now_ns=CONFIRMATION_NS)
self.assertEqual(expired.basis, "timeout_reject")
self.assertIn(LOW, expired.state.rejected)
self.assertIsNone(expired.state.pending)
self.assertIsNone(expired.history_write)
self.assertEqual(expired.control_target_mps, HIGH.speed_mps)
def test_paused_confirmation_does_not_accumulate_inactive_time(self):
start = self.pending()
active = step(start.state, valid(LOW), ACTIVE, Policy(), now_ns=10 * NS)
pause = step(active.state, valid(LOW), PAUSED, Policy(), now_ns=20 * NS)
held = step(pause.state, valid(LOW), PAUSED, Policy(), now_ns=1000 * NS)
resume = step(held.state, valid(LOW), ACTIVE, Policy(), now_ns=2000 * NS)
for result in (active, pause, held, resume):
self.assertEqual(result.state.pending.active_elapsed_ns, 10 * NS)
self.assertIsNone(pause.control_target_mps)
self.assertIsNone(held.control_target_mps)
expired = step(resume.state, valid(LOW), ACTIVE, Policy(), now_ns=2020 * NS)
self.assertEqual(expired.basis, "timeout_reject")
def test_no_implicit_ttl_or_maximum_active_gap(self):
# Freshness belongs to the adapter: an explicitly valid active interval counts.
start = self.pending()
result = step(start.state, valid(LOW), ACTIVE, Policy(), now_ns=1000 * NS)
self.assertEqual(result.basis, "timeout_reject")
def test_action_at_deadline_precedes_automatic_timeout(self):
for kind in ActionKind:
with self.subTest(kind=kind):
result = self.act(self.pending().state, kind, now_ns=CONFIRMATION_NS)
self.assertEqual(result.basis, "driver_accept" if kind is ActionKind.ACCEPT else "driver_reject")
self.assertTrue(result.action_receipt.consumed)
def test_directional_confirmation_and_first_limit_policy(self):
for lower, higher, candidate in itertools.product((False, True), (False, True), (LOW, HIGH, replace(HIGH, speed_mps=31.0))):
with self.subTest(lower=lower, higher=higher, candidate=candidate):
result = step(saved(), valid(candidate), ACTIVE, Policy(lower, higher), now_ns=0)
expected_pending = lower if candidate.speed_mps < HIGH.speed_mps else higher if candidate.speed_mps > HIGH.speed_mps else False
self.assertEqual(result.state.pending is not None, expected_pending)
self.assertEqual(result.control_target_mps, HIGH.speed_mps if expected_pending else candidate.speed_mps)
for higher in (False, True):
result = step(new_session("drive"), valid(LOW), ACTIVE, Policy(confirm_higher=higher), now_ns=0)
self.assertEqual(result.state.pending is not None, higher)
self.assertEqual(result.control_target_mps, None if higher else LOW.speed_mps)
self.assertEqual(result.history_write is None, higher)
def test_near_equal_values_are_exact_and_source_identity_updates_history(self):
for toward in (0.0, math.inf):
near = replace(HIGH, speed_mps=math.nextafter(HIGH.speed_mps, toward))
result = self.pending(candidate=near)
self.assertNotEqual(result.state.pending.candidate.speed_mps, HIGH.speed_mps)
same_speed = replace(HIGH, source="map", observation_identity=geographic("mapped-road"))
result = step(saved(), valid(same_speed), ACTIVE, Policy(), now_ns=0)
self.assertIsNone(result.state.pending)
self.assertEqual(result.history_write.candidate, same_speed)
unchanged = step(result.state, valid(same_speed), ACTIVE, Policy(), now_ns=NS)
self.assertIsNone(unchanged.history_write)
def test_disengaged_acceptance_has_no_control_and_needs_no_button(self):
for mode, owner in itertools.product(Mode, LongitudinalOwner):
with self.subTest(mode=mode, owner=owner):
authority = Authority(mode, owner, False, False, False, True)
result = step(saved(), valid(LOW), authority, Policy(), now_ns=0)
self.assertEqual(result.state.accepted.candidate, LOW)
self.assertIsNotNone(result.history_write)
self.assertIsNone(result.state.pending)
self.assertIsNone(result.control_target_mps)
def test_zero_active_axes_does_not_imply_fully_disengaged(self):
result = self.pending(authority=PAUSED)
self.assertEqual(result.state.accepted.candidate, HIGH)
self.assertIsNone(result.history_write)
self.assertIsNone(result.control_target_mps)
def test_modes_stock_owner_and_inactive_long_never_gain_control_or_consume_action(self):
contexts = [
Authority(Mode.OFF, LongitudinalOwner.NONE, False, False, False, False),
Authority(Mode.LATERAL_ONLY, LongitudinalOwner.SYSTEM, True, False, False, False),
Authority(Mode.LATERAL_ONLY, LongitudinalOwner.STOCK, True, False, True, False),
Authority(Mode.COMBINED, LongitudinalOwner.STOCK, True, False, True, False),
Authority(Mode.LONGITUDINAL_ONLY, LongitudinalOwner.STOCK, False, False, True, False),
PAUSED,
]
for authority in contexts:
with self.subTest(authority=authority):
pending = self.pending(authority=authority)
acted = self.act(pending.state, ActionKind.ACCEPT, authority=authority)
lost = step(acted.state, ABSENT, authority, Policy(fallback_previous=True), now_ns=2 * NS)
for decision in (pending, acted, lost):
self.assertIsNone(decision.control_target_mps)
self.assertIsNone(decision.history_write)
self.assertTrue(acted.action_receipt.acknowledged)
self.assertFalse(acted.action_receipt.consumed)
self.assertEqual(acted.state.last_action_sequence, 0)
def test_combined_and_long_only_have_equal_longitudinal_decisions(self):
for authority in (ACTIVE, replace(ACTIVE, mode=Mode.COMBINED, lateral_active=True)):
pending = self.pending(authority=authority)
accepted = self.act(pending.state, ActionKind.ACCEPT, authority=authority)
self.assertEqual(accepted.control_target_mps, LOW.speed_mps)
self.assertEqual(accepted.history_write.candidate, LOW)
def test_all_authority_combinations_reject_contradictions(self):
combinations = itertools.product(Mode, LongitudinalOwner, (False, True), (False, True), (False, True), (False, True))
for mode, owner, lateral, longitudinal, stock, disengaged in combinations:
authority = Authority(mode, owner, lateral, longitudinal, stock, disengaged)
invalid = ((lateral and mode not in (Mode.LATERAL_ONLY, Mode.COMBINED)) or
(longitudinal and (mode not in (Mode.LONGITUDINAL_ONLY, Mode.COMBINED) or owner is not LongitudinalOwner.SYSTEM)) or
(stock and owner is not LongitudinalOwner.STOCK) or (disengaged and (lateral or longitudinal or stock)))
with self.subTest(authority=authority):
result = step(saved(), valid(LOW), authority, Policy(False, False, True), now_ns=0)
self.assertEqual(bool(result.errors), bool(invalid))
if invalid:
self.assertIsNone(result.control_target_mps)
self.assertIsNone(result.history_write)
elif not longitudinal:
self.assertIsNone(result.control_target_mps)
def test_display_only_clears_pending_and_never_accepts_or_falls_back(self):
pending = self.pending()
action = DriverAction("drive", 1, pending.state.pending.decision_id, ActionKind.ACCEPT)
for authority, observation in itertools.product((ACTIVE, DISENGAGED), (valid(LOW), ABSENT)):
with self.subTest(authority=authority, observation=observation):
result = step(pending.state, observation, authority, Policy(fallback_previous=True, display_only=True), now_ns=NS, action=action)
self.assertIsNone(result.state.pending)
self.assertEqual(result.state.accepted, pending.state.accepted)
self.assertIsNone(result.history_write)
self.assertIsNone(result.control_target_mps)
self.assertEqual(result.display_candidate, observation.candidate)
self.assertFalse(result.action_receipt.consumed)
self.assertEqual(result.state.last_action_sequence, 1)
def test_repeated_and_out_of_order_actions_are_seen_but_not_consumed(self):
pending = self.pending()
ineligible = self.act(pending.state, ActionKind.ACCEPT, sequence=10, authority=PAUSED)
for sequence in (10, 9):
result = self.act(ineligible.state, ActionKind.ACCEPT, sequence=sequence, now_ns=2 * NS)
self.assertFalse(result.action_receipt.consumed)
self.assertEqual(result.action_receipt.status, "already_seen")
self.assertEqual(result.state.last_action_sequence, 10)
self.assertEqual(result.state.accepted.candidate, HIGH)
accepted = self.act(ineligible.state, ActionKind.ACCEPT, sequence=11, now_ns=2 * NS)
self.assertTrue(accepted.action_receipt.consumed)
self.assertEqual(accepted.state.accepted.candidate, LOW)
def test_wrong_session_does_not_poison_sequence_watermark(self):
pending = self.pending()
foreign = DriverAction("other-drive", 10_000, pending.state.pending.decision_id, ActionKind.ACCEPT)
result = step(pending.state, valid(LOW), ACTIVE, Policy(), now_ns=NS, action=foreign)
self.assertEqual(result.action_receipt.status, "foreign_session")
self.assertFalse(result.action_receipt.consumed)
self.assertEqual(result.state.last_action_sequence, -1)
accepted = self.act(result.state, ActionKind.ACCEPT, sequence=0, now_ns=2 * NS)
self.assertTrue(accepted.action_receipt.consumed)
def test_unrelated_action_is_acknowledged_and_watermarked_without_consuming_it(self):
pending = self.pending()
unrelated = DriverAction("drive", 3, 987, ActionKind.REJECT)
result = step(pending.state, valid(LOW), ACTIVE, Policy(), now_ns=NS, action=unrelated)
self.assertTrue(result.action_receipt.acknowledged)
self.assertFalse(result.action_receipt.consumed)
self.assertEqual(result.action_receipt.status, "different_decision")
self.assertEqual(result.state.last_action_sequence, 3)
self.assertEqual(result.state.rejected, frozenset())
def test_event_cannot_accept_a_decision_not_previously_presented(self):
action = DriverAction("drive", 0, 1, ActionKind.ACCEPT)
result = step(saved(), valid(LOW), ACTIVE, Policy(), now_ns=0, action=action)
self.assertEqual(result.state.pending.decision_id, 1)
self.assertEqual(result.action_receipt.status, "not_presented")
self.assertFalse(result.action_receipt.consumed)
self.assertEqual(result.state.last_action_sequence, 0)
self.assertEqual(result.state.accepted.candidate, HIGH)
def test_invalid_input_fails_closed_and_valid_action_is_still_seen(self):
pending = self.pending(now_ns=10 * NS)
action = DriverAction("drive", 2, pending.state.pending.decision_id, ActionKind.ACCEPT)
result = step(pending.state, valid(LOW), ACTIVE, Policy(), now_ns=9 * NS, action=action)
self.assertTrue(result.errors)
self.assertIsNone(result.control_target_mps)
self.assertIsNone(result.state.pending)
self.assertFalse(result.state.timer_running)
self.assertEqual(result.state.last_timestamp_ns, 10 * NS)
self.assertIsNone(result.history_write)
self.assertFalse(result.action_receipt.consumed)
self.assertEqual(result.action_receipt.status, "invalid_input")
self.assertEqual(result.state.last_action_sequence, 2)
recovered = self.pending(state=result.state, now_ns=20 * NS)
self.assertEqual(recovered.state.pending.active_elapsed_ns, 0)
def test_clock_requires_nonnegative_monotonic_integer_nanoseconds(self):
invalid_times: tuple[Any, ...] = (-1, 0.0, True, math.nan, math.inf, "10", None)
for now_ns in invalid_times:
with self.subTest(now_ns=now_ns):
result = step(saved(), valid(LOW), ACTIVE, Policy(), now_ns=now_ns)
self.assertTrue(result.errors)
self.assertIsNone(result.control_target_mps)
pending = self.pending(now_ns=NS)
same = step(pending.state, valid(LOW), ACTIVE, Policy(), now_ns=NS)
self.assertEqual(same.state.pending.active_elapsed_ns, 0)
def test_candidate_requires_finite_positive_speed_and_explicit_bounded_identity(self):
malformed = [replace(LOW, speed_mps=value) for value in (0, -1, True, math.nan, math.inf, "20", None, 10 ** 1000)]
malformed += [replace(LOW, source=""), replace(LOW, observation_identity=geographic("")), replace(LOW, source=" "),
replace(LOW, observation_identity=geographic("x" * 257)),
replace(LOW, source="a\nb")]
for candidate in malformed:
with self.subTest(candidate=candidate):
result = step(saved(), valid(candidate), ACTIVE, Policy(False, False), now_ns=0)
self.assertTrue(result.errors)
self.assertIsNone(result.control_target_mps)
self.assertIsNone(result.history_write)
def test_observation_classification_is_explicit_and_consistent(self):
malformed: list[Any] = [Observation(ObservationKind.VALID), Observation(cast(Any, "valid"), LOW), None]
malformed += [Observation(kind, LOW) for kind in (ObservationKind.ABSENT, ObservationKind.UNKNOWN, ObservationKind.STALE)]
for observation in malformed:
with self.subTest(observation=observation):
result = step(saved(), observation, ACTIVE, Policy(fallback_previous=True), now_ns=0)
self.assertTrue(result.errors)
self.assertIsNone(result.control_target_mps)
def test_policy_authority_and_actions_reject_coerced_types(self):
for field in ("confirm_lower", "confirm_higher", "fallback_previous", "display_only"):
result = step(saved(), valid(LOW), ACTIVE, replace(Policy(), **{field: 1}), now_ns=0)
self.assertTrue(result.errors)
for authority in (replace(ACTIVE, mode="longitudinal_only"), replace(ACTIVE, owner="system"), replace(ACTIVE, fully_disengaged=0)):
result = step(saved(), valid(LOW), authority, Policy(), now_ns=0)
self.assertTrue(result.errors)
pending = self.pending()
action = DriverAction("drive", 1, pending.state.pending.decision_id, ActionKind.ACCEPT)
for invalid in (replace(action, sequence_id=True), replace(action, sequence_id=-1), replace(action, decision_id=0),
replace(action, session_id=""), replace(action, kind="accept")):
result = step(pending.state, valid(LOW), ACTIVE, Policy(), now_ns=NS, action=invalid)
self.assertTrue(result.errors)
self.assertIsNone(result.control_target_mps)
self.assertIsNone(result.action_receipt)
self.assertEqual(result.state.last_action_sequence, -1)
def test_invalid_state_never_returns_a_control_target(self):
malformed = [replace(saved(), session_id=""), replace(saved(), rejected=set()), replace(saved(), last_timestamp_ns=-1),
replace(saved(), next_decision_id=0), replace(saved(), timer_running=True), replace(saved(), accepted=LOW)]
for state in malformed:
with self.subTest(state=state):
result = step(state, valid(LOW), ACTIVE, Policy(False, False, True), now_ns=0)
self.assertTrue(result.errors)
self.assertIsNone(result.control_target_mps)
self.assertIsNone(result.history_write)
def test_accepted_or_pending_candidate_cannot_also_be_rejected(self):
accepted_conflict = replace(saved(), rejected=frozenset({HIGH}))
pending_conflict = replace(self.pending().state, rejected=frozenset({LOW}))
for state in (accepted_conflict, pending_conflict):
with self.subTest(state=state):
result = step(state, valid(HIGH), ACTIVE, Policy(False, False, True), now_ns=NS)
self.assertTrue(result.errors)
self.assertIsNone(result.control_target_mps)
self.assertIsNone(result.history_write)
self.assertIs(result.state, state)
self.assertEqual(result.state.accepted.candidate, HIGH)
def test_rejection_capacity_latches_closed_instead_of_evicting(self):
state = saved()
for index in range(MAX_REJECTIONS + 1):
candidate = replace(LOW, observation_identity=geographic(f"zone-{index}"))
pending = self.pending(candidate=candidate, state=state, now_ns=2 * index)
result = self.act(pending.state, ActionKind.REJECT, sequence=index, now_ns=2 * index + 1)
state = result.state
self.assertEqual(len(state.rejected), MAX_REJECTIONS)
self.assertTrue(state.rejection_capacity_exhausted)
self.assertIsNone(result.control_target_mps)
self.assertIn(replace(LOW, observation_identity=geographic("zone-0")), state.rejected)
after = step(state, ABSENT, ACTIVE, Policy(fallback_previous=True), now_ns=2 * MAX_REJECTIONS + 2)
self.assertEqual(after.basis, "session_reset_required")
self.assertIsNone(after.control_target_mps)
shown = step(after.state, valid(replace(LOW, observation_identity=geographic("zone-0"))), ACTIVE, Policy(),
now_ns=2 * MAX_REJECTIONS + 3)
blocked_adopt = step(shown.state, shown.state.observation, ACTIVE, Policy(), now_ns=2 * MAX_REJECTIONS + 4,
adopt=self.adopt_request(shown.state, MAX_REJECTIONS + 1))
self.assertFalse(blocked_adopt.adoption_receipt.consumed)
self.assertEqual(blocked_adopt.adoption_receipt.status, "session_reset_required")
self.assertIsNone(blocked_adopt.adoption)
self.assertIsNone(blocked_adopt.history_write)
self.assertIsNone(blocked_adopt.control_target_mps)
self.assertTrue(blocked_adopt.state.rejection_capacity_exhausted)
self.assertEqual(blocked_adopt.state.rejected, state.rejected)
reset = new_session("next-drive", state.accepted)
self.assertFalse(reset.rejection_capacity_exhausted)
self.assertEqual(reset.rejected, frozenset())
self.assertEqual(reset.accepted, state.accepted)
self.assertEqual(reset.last_action_sequence, -1)
def test_state_and_inputs_are_immutable_and_new_session_validates_history(self):
original = saved()
step(original, valid(LOW), ACTIVE, Policy(), now_ns=0)
self.assertIsNone(original.pending)
self.assertIsNone(original.last_timestamp_ns)
with self.assertRaises(FrozenInstanceError):
cast(Any, original).session_id = "changed"
for session, history in (("", None), ("drive", LOW), ("drive", AcceptedLimit(replace(HIGH, speed_mps=0), "old", 0))):
with self.assertRaises(ValueError):
new_session(session, cast(Any, history))
def test_identity_kinds_are_explicit_and_not_interchangeable(self):
identities = (geographic("road"), ObservationIdentity(IdentityKind.PRODUCER_EPISODE, value="episode"),
ObservationIdentity(IdentityKind.SESSION_VALUE, session_id="drive"))
for identity in identities:
candidate = replace(HIGH, observation_identity=identity)
result = step(saved(), valid(candidate), ACTIVE, Policy(), now_ns=0)
self.assertEqual(result.errors, ())
self.assertEqual(result.state.accepted.candidate, candidate)
self.assertEqual(len({replace(HIGH, observation_identity=identity) for identity in identities}), 3)
def test_session_value_retains_rejection_across_dropout_and_source_switching(self):
identity = ObservationIdentity(IdentityKind.SESSION_VALUE, session_id="drive")
candidate = replace(LOW, observation_identity=identity)
pending = self.pending(candidate=candidate)
rejected = self.act(pending.state, ActionKind.REJECT)
absent = step(rejected.state, ABSENT, ACTIVE, Policy(), now_ns=2 * NS)
alternate = step(absent.state, valid(replace(candidate, source="map")), ACTIVE, Policy(), now_ns=3 * NS)
returned = step(alternate.state, valid(Candidate("dashboard", ObservationIdentity(IdentityKind.SESSION_VALUE, session_id="drive"),
LOW.speed_mps)), ACTIVE, Policy(), now_ns=4 * NS)
self.assertIn(candidate, returned.state.rejected)
self.assertEqual(returned.basis, "rejected")
self.assertEqual(returned.state.accepted.candidate, HIGH)
self.assertIsNotNone(alternate.state.pending)
self.assertIsNone(returned.state.pending)
def test_session_value_has_no_alternate_token_and_cannot_cross_sessions(self):
malformed = [ObservationIdentity(IdentityKind.SESSION_VALUE, value="invented", session_id="drive"),
ObservationIdentity(IdentityKind.SESSION_VALUE), ObservationIdentity(IdentityKind.SESSION_VALUE, session_id="other"),
ObservationIdentity(IdentityKind.GEOGRAPHIC, value="road", session_id="drive"),
ObservationIdentity(IdentityKind.PRODUCER_EPISODE, value=""),
ObservationIdentity(cast(Any, "session_value"), session_id="drive"), cast(Any, "road")]
for identity in malformed:
with self.subTest(identity=identity):
result = step(saved(), valid(replace(LOW, observation_identity=identity)), ACTIVE, Policy(), now_ns=0)
self.assertTrue(result.errors)
self.assertIsNone(result.control_target_mps)
self.assertIsNone(result.history_write)
candidate = replace(HIGH, observation_identity=ObservationIdentity(IdentityKind.SESSION_VALUE, session_id="old"))
history = AcceptedLimit(candidate, "old", 0)
with self.assertRaises(ValueError):
new_session("drive", history)
with self.assertRaises(ValueError):
new_session("drive", replace(history, session_id="drive"))
# No implicit rekeying: independently qualified geographic history retains its original accepted session.
qualified = AcceptedLimit(HIGH, "old", 0)
self.assertIs(new_session("drive", qualified).accepted, qualified)
def test_explicit_adopt_clears_only_matching_rejection(self):
first_reject = self.act(self.pending().state, ActionKind.REJECT)
other = replace(LOW, source="map", speed_mps=19.0)
second_pending = self.pending(candidate=other, state=first_reject.state, now_ns=2 * NS)
second_reject = self.act(second_pending.state, ActionKind.REJECT, sequence=1, now_ns=3 * NS)
shown = step(second_reject.state, valid(LOW), ACTIVE, Policy(), now_ns=4 * NS)
request = self.adopt_request(shown.state, sequence=2)
adopted = step(shown.state, valid(LOW), ACTIVE, Policy(), now_ns=5 * NS, adopt=request)
self.assertEqual(adopted.basis, "driver_adopt")
self.assertTrue(adopted.adoption_receipt.consumed)
self.assertIsNone(adopted.action_receipt)
self.assertEqual(adopted.state.rejected, frozenset({other}))
self.assertEqual(adopted.history_write.candidate, LOW)
self.assertEqual(adopted.control_target_mps, LOW.speed_mps)
self.assertIsNone(adopted.state.pending)
self.assertTrue(adopted.adoption.clear_override)
self.assertEqual(adopted.adoption.session_id, "drive")
self.assertEqual(adopted.adoption.action_sequence_id, 2)
self.assertEqual(adopted.adoption.presentation_id, request.presentation_id)
self.assertEqual(adopted.adoption.accepted, adopted.state.accepted)
self.assertEqual(adopted.adoption.reconciliation_speed_mps, LOW.speed_mps)
still_rejected = step(adopted.state, valid(other), ACTIVE, Policy(), now_ns=6 * NS)
self.assertEqual(still_rejected.basis, "rejected")
self.assertEqual(still_rejected.control_target_mps, LOW.speed_mps)
def test_adopt_already_accepted_candidate_still_emits_reconciliation(self):
shown = step(saved(), valid(HIGH), ACTIVE, Policy(), now_ns=0)
for authority in (ACTIVE, replace(ACTIVE, mode=Mode.COMBINED, lateral_active=True)):
result = step(shown.state, valid(HIGH), authority, Policy(), now_ns=NS, adopt=self.adopt_request(shown.state))
self.assertTrue(result.adoption_receipt.consumed)
self.assertIsNone(result.history_write)
self.assertEqual(result.state.accepted, shown.state.accepted)
self.assertTrue(result.adoption.clear_override)
self.assertEqual(result.adoption.reconciliation_speed_mps, HIGH.speed_mps)
def test_adopt_precedes_confirmation_timeout(self):
pending = self.pending()
result = step(pending.state, valid(LOW), ACTIVE, Policy(), now_ns=CONFIRMATION_NS, adopt=self.adopt_request(pending.state))
self.assertEqual(result.basis, "driver_adopt")
self.assertEqual(result.state.accepted.candidate, LOW)
self.assertNotIn(LOW, result.state.rejected)
def test_adopt_requires_prior_presentation_even_when_new_id_is_guessed(self):
guessed = AdoptRequest("drive", 0, 1, LOW)
unseen = step(saved(), valid(LOW), ACTIVE, Policy(), now_ns=0, adopt=guessed)
self.assertEqual(unseen.state.presentation.presentation_id, guessed.presentation_id)
self.assertFalse(unseen.adoption_receipt.consumed)
self.assertEqual(unseen.adoption_receipt.status, "different_presentation")
self.assertIsNone(unseen.adoption)
self.assertIsNone(unseen.history_write)
replay = step(unseen.state, valid(LOW), ACTIVE, Policy(), now_ns=NS, adopt=guessed)
self.assertEqual(replay.adoption_receipt.status, "already_seen")
self.assertIsNone(replay.adoption)
fresh = step(replay.state, valid(LOW), ACTIVE, Policy(), now_ns=2 * NS, adopt=replace(guessed, sequence_id=1))
self.assertTrue(fresh.adoption_receipt.consumed)
def test_adopt_context_expires_on_source_change_or_loss(self):
shown = self.pending()
request = self.adopt_request(shown.state)
for observation in (valid(replace(LOW, source="map")), ABSENT, Observation(ObservationKind.UNKNOWN), Observation(ObservationKind.STALE)):
with self.subTest(observation=observation):
changed = step(shown.state, observation, ACTIVE, Policy(fallback_previous=True), now_ns=NS, adopt=request)
self.assertFalse(changed.adoption_receipt.consumed)
self.assertIsNone(changed.adoption)
self.assertIsNone(changed.history_write)
returned = step(changed.state, valid(LOW), ACTIVE, Policy(), now_ns=2 * NS, adopt=replace(request, sequence_id=1))
self.assertFalse(returned.adoption_receipt.consumed)
self.assertIsNone(returned.adoption)
self.assertNotEqual(returned.state.presentation.presentation_id, request.presentation_id)
def test_adopt_exact_candidate_cannot_be_swapped_under_existing_context(self):
shown = self.pending()
for other in (replace(LOW, source="map"), replace(LOW, speed_mps=19.0),
replace(LOW, observation_identity=geographic("other-road"))):
request = replace(self.adopt_request(shown.state), candidate=other)
result = step(shown.state, valid(LOW), ACTIVE, Policy(), now_ns=NS, adopt=request)
self.assertFalse(result.adoption_receipt.consumed)
self.assertEqual(result.adoption_receipt.status, "different_presentation")
self.assertIsNone(result.adoption)
def test_adopt_requires_active_system_long_authority_and_control_policy(self):
rejected = self.act(self.pending().state, ActionKind.REJECT)
request = self.adopt_request(rejected.state, sequence=1)
contexts = [PAUSED, DISENGAGED, Authority(Mode.OFF, LongitudinalOwner.NONE, False, False, False, False),
Authority(Mode.LATERAL_ONLY, LongitudinalOwner.NONE, True, False, False, False),
Authority(Mode.LATERAL_ONLY, LongitudinalOwner.STOCK, True, False, True, False),
Authority(Mode.COMBINED, LongitudinalOwner.STOCK, True, False, True, False)]
for authority in contexts:
with self.subTest(authority=authority):
result = step(rejected.state, valid(LOW), authority, Policy(), now_ns=2 * NS, adopt=request)
self.assertFalse(result.adoption_receipt.consumed)
self.assertEqual(result.adoption_receipt.status, "no_longitudinal_authority")
self.assertIn(LOW, result.state.rejected)
self.assertIsNone(result.adoption)
self.assertIsNone(result.history_write)
self.assertIsNone(result.control_target_mps)
self.assertEqual(result.state.last_action_sequence, 1)
display = step(rejected.state, valid(LOW), ACTIVE, Policy(display_only=True), now_ns=2 * NS, adopt=request)
self.assertFalse(display.adoption_receipt.consumed)
self.assertEqual(display.adoption_receipt.status, "display_only")
self.assertIn(LOW, display.state.rejected)
self.assertIsNone(display.adoption)
self.assertIsNone(display.history_write)
def test_adopt_foreign_session_does_not_advance_watermark(self):
shown = self.pending()
foreign = replace(self.adopt_request(shown.state, sequence=999), session_id="other")
result = step(shown.state, valid(LOW), ACTIVE, Policy(), now_ns=NS, adopt=foreign)
self.assertEqual(result.adoption_receipt.status, "foreign_session")
self.assertEqual(result.state.last_action_sequence, -1)
self.assertIsNone(result.adoption)
accepted = step(result.state, valid(LOW), ACTIVE, Policy(), now_ns=2 * NS, adopt=self.adopt_request(result.state))
self.assertTrue(accepted.adoption_receipt.consumed)
def test_adoption_and_confirmation_share_seen_sequence_watermark(self):
shown = self.pending()
request = self.adopt_request(shown.state, sequence=5)
paused = step(shown.state, valid(LOW), PAUSED, Policy(), now_ns=NS, adopt=request)
repeated_action = DriverAction("drive", 5, paused.state.pending.decision_id, ActionKind.ACCEPT)
repeated = step(paused.state, valid(LOW), ACTIVE, Policy(), now_ns=2 * NS, action=repeated_action)
self.assertEqual(repeated.action_receipt.status, "already_seen")
self.assertFalse(repeated.action_receipt.consumed)
fresh = step(repeated.state, valid(LOW), ACTIVE, Policy(), now_ns=3 * NS, action=replace(repeated_action, sequence_id=6))
self.assertTrue(fresh.action_receipt.consumed)
old_adopt = step(fresh.state, valid(LOW), ACTIVE, Policy(), now_ns=4 * NS, adopt=request)
self.assertFalse(old_adopt.adoption_receipt.consumed)
self.assertIsNone(old_adopt.adoption)
def test_ambiguous_action_and_adopt_fail_closed_and_watermark_both(self):
shown = self.pending()
for ordinary_sequence, adopt_sequence in ((3, 7), (7, 3), (7, 7)):
with self.subTest(ordinary=ordinary_sequence, adopt=adopt_sequence):
action = DriverAction("drive", ordinary_sequence, shown.state.pending.decision_id, ActionKind.ACCEPT)
adopt = self.adopt_request(shown.state, sequence=adopt_sequence)
result = step(shown.state, valid(LOW), ACTIVE, Policy(), now_ns=NS, action=action, adopt=adopt)
self.assertIn("ambiguous action and adopt request", result.errors)
self.assertFalse(result.action_receipt.consumed)
self.assertFalse(result.adoption_receipt.consumed)
self.assertEqual(result.state.last_action_sequence, 7)
self.assertIsNone(result.control_target_mps)
self.assertIsNone(result.adoption)
self.assertIsNone(result.history_write)
self.assertEqual(result.state.accepted.candidate, HIGH)
self.assertIsNone(result.state.presentation)
def test_invalid_adopt_or_clock_never_produces_an_effect(self):
shown = self.pending(now_ns=10 * NS)
request = self.adopt_request(shown.state, sequence=1)
for malformed in (replace(request, sequence_id=True), replace(request, presentation_id=0),
replace(request, session_id=""), replace(request, candidate=replace(LOW, speed_mps=0))):
result = step(shown.state, valid(LOW), ACTIVE, Policy(), now_ns=11 * NS, adopt=malformed)
self.assertTrue(result.errors)
self.assertIsNone(result.adoption_receipt)
self.assertIsNone(result.adoption)
self.assertIsNone(result.control_target_mps)
self.assertEqual(result.state.last_action_sequence, -1)
reversed_clock = step(shown.state, valid(LOW), ACTIVE, Policy(), now_ns=9 * NS, adopt=request)
self.assertTrue(reversed_clock.errors)
self.assertFalse(reversed_clock.adoption_receipt.consumed)
self.assertEqual(reversed_clock.state.last_action_sequence, 1)
self.assertIsNone(reversed_clock.adoption)
def test_invalid_presentation_state_is_inert(self):
shown = self.pending()
for state in (replace(shown.state, presentation=None), replace(shown.state, next_presentation_id=1),
replace(shown.state, presentation=replace(shown.state.presentation, candidate=HIGH))):
result = step(state, valid(LOW), ACTIVE, Policy(), now_ns=NS, adopt=self.adopt_request(shown.state))
self.assertTrue(result.errors)
self.assertIsNone(result.control_target_mps)
self.assertIsNone(result.adoption)
self.assertIsNone(result.history_write)
if __name__ == "__main__":
unittest.main()
@@ -0,0 +1,242 @@
"""Causal effect contracts, including delayed confirmation/adoption speed changes."""
from dataclasses import FrozenInstanceError, replace
from typing import Any, cast
import unittest
from openpilot.starpilot.speed_limits import action_arbitration as arb
from openpilot.starpilot.speed_limits import acceptance as accept
SESSION = "drive"
AUTHORITY = accept.Authority(accept.Mode.LONGITUDINAL_ONLY, accept.LongitudinalOwner.SYSTEM, False, True, False, False)
CANDIDATE = accept.Candidate("dashboard", accept.ObservationIdentity(accept.IdentityKind.SESSION_VALUE, session_id=SESSION), 20.0)
OBSERVATION = accept.Observation(accept.ObservationKind.VALID, CANDIDATE)
def begin(ledger=None, action_id=1, origin=arb.Origin.DRIVER_CRUISE, context="accepted-context-1", now=1):
return arb.step(ledger or arb.new_ledger(SESSION), arb.BeginAction(SESSION, action_id, origin, context), now_ns=now).state
def resolve(ledger, disposition=arb.Disposition.DRIVER_INTENT, action_id=1, now=2):
return arb.step(ledger, arb.ResolveAction(SESSION, action_id, disposition), now_ns=now)
def change(ledger, effect_id=1, action_id=1, previous=20.0, selected=25.0, now=3):
return arb.step(ledger, arb.CruiseChange(SESSION, effect_id, action_id, previous, selected), now_ns=now)
class TestActionArbitration(unittest.TestCase):
def test_confirmation_consumption_survives_release_and_long_delay(self):
pending = accept.step(accept.new_session(SESSION), OBSERVATION, AUTHORITY, accept.Policy(), now_ns=0)
ledger = begin()
decision = accept.step(pending.state, OBSERVATION, AUTHORITY, accept.Policy(), now_ns=2,
action=accept.DriverAction(SESSION, 1, pending.state.pending.decision_id, accept.ActionKind.ACCEPT))
bound = arb.resolve_acceptance(ledger, 1, decision, now_ns=2)
self.assertEqual(bound.status, "resolved")
# Button release does not complete a transaction whose cruise effect is pending.
delayed = change(bound.state, now=60_000_000_000)
self.assertEqual(delayed.change.disposition, arb.Disposition.SLC_CONSUMED)
self.assertEqual(delayed.change.action_id, 1)
self.assertEqual(delayed.change.started_at_ns, 1)
self.assertEqual(delayed.change.context_id, "accepted-context-1")
def test_adoption_proposal_and_effect_keep_originating_action(self):
shown = accept.step(accept.new_session(SESSION), OBSERVATION, AUTHORITY, accept.Policy(), now_ns=0)
request = accept.AdoptRequest(SESSION, 1, shown.state.presentation.presentation_id, CANDIDATE)
adopted = accept.step(shown.state, OBSERVATION, AUTHORITY, accept.Policy(), now_ns=2, adopt=request)
ledger = arb.resolve_acceptance(begin(), 1, adopted, now_ns=2).state
result = change(ledger, previous=25.0, selected=adopted.adoption.reconciliation_speed_mps, now=10_000_000_000)
self.assertEqual(result.change.disposition, arb.Disposition.SLC_CONSUMED)
self.assertEqual(result.change.action_id, adopted.adoption.action_sequence_id)
def test_final_unconsumed_receipt_does_not_swallow_cruise_input(self):
shown = accept.step(accept.new_session(SESSION), OBSERVATION, AUTHORITY, accept.Policy(), now_ns=0)
decision = accept.step(shown.state, OBSERVATION, AUTHORITY, accept.Policy(), now_ns=2,
action=accept.DriverAction(SESSION, 1, 999, accept.ActionKind.ACCEPT))
self.assertFalse(decision.action_receipt.consumed)
ledger = arb.resolve_acceptance(begin(), 1, decision, now_ns=2).state
result = change(ledger)
self.assertEqual(result.change.disposition, arb.Disposition.DRIVER_INTENT)
def test_deliberate_increase_and_decrease_remain_distinct_from_policy(self):
ledger = resolve(begin()).state
for selected in (15.0, 25.0):
result = change(ledger, selected=selected)
self.assertEqual(result.change.disposition, arb.Disposition.DRIVER_INTENT)
self.assertEqual(result.change.selected_mps, selected)
# Above/below-limit eligibility belongs to the separate override/stock policy.
def test_initial_or_steady_scalar_sample_cannot_establish_fresh_intent(self):
ledger = resolve(begin()).state
for previous in (None, 25.0):
result = change(ledger, previous=previous, selected=25.0)
self.assertEqual(result.change.disposition, arb.Disposition.UNRESOLVED)
self.assertEqual(result.state.last_effect_id, 1)
def test_missing_unknown_and_future_origins_never_infer_intent(self):
for action_id in (None, 0, 999):
result = change(resolve(begin()).state, action_id=action_id)
self.assertEqual(result.change.disposition, arb.Disposition.UNRESOLVED)
self.assertIsNone(result.change.context_id)
def test_early_effect_is_not_replayed_after_later_resolution(self):
early = change(begin(), now=2)
self.assertEqual(early.change.disposition, arb.Disposition.UNRESOLVED)
resolved = resolve(early.state, now=3)
repeated = change(resolved.state, now=4)
self.assertIsNone(repeated.change)
self.assertEqual(repeated.status, "already_seen_effect")
self.assertEqual(change(repeated.state, effect_id=2, now=5).change.disposition, arb.Disposition.DRIVER_INTENT)
def test_automatic_and_unrelated_effects_never_become_driver_intent(self):
for origin, disposition in ((arb.Origin.AUTOMATIC, arb.Disposition.AUTOMATIC),
(arb.Origin.DRIVER_OTHER, arb.Disposition.UNRELATED)):
ledger = resolve(begin(origin=origin), disposition).state
self.assertEqual(change(ledger).change.disposition, disposition)
def test_multiple_pending_actions_resolve_and_deliver_independently(self):
ledger = begin(begin(), action_id=2, context="accepted-context-2", now=2)
ledger = resolve(ledger, action_id=2, now=3).state
ledger = resolve(ledger, arb.Disposition.SLC_CONSUMED, action_id=1, now=4).state
second = change(ledger, action_id=2, now=5)
first = change(second.state, effect_id=2, action_id=1, now=6)
self.assertEqual(second.change.disposition, arb.Disposition.DRIVER_INTENT)
self.assertEqual(second.change.context_id, "accepted-context-2")
self.assertEqual(first.change.disposition, arb.Disposition.SLC_CONSUMED)
self.assertEqual(first.change.context_id, "accepted-context-1")
def test_completion_prevents_late_effect_from_rearming_or_reopening(self):
ledger = resolve(begin()).state
completed = arb.step(ledger, arb.CompleteAction(SESSION, 1), now_ns=3)
self.assertEqual(completed.state.transactions, ())
late = change(completed.state, now=4)
self.assertEqual(late.change.disposition, arb.Disposition.UNRESOLVED)
reopened = arb.step(late.state, arb.BeginAction(SESSION, 1, arb.Origin.DRIVER_CRUISE, "new-context"), now_ns=5)
self.assertEqual(reopened.status, "already_seen_action")
self.assertEqual(reopened.state.transactions, ())
def test_duplicate_and_out_of_order_effects_never_emit_twice(self):
first = change(resolve(begin()).state, effect_id=10)
for effect_id in (9, 10):
repeated = change(first.state, effect_id=effect_id, now=4)
self.assertIsNone(repeated.change)
self.assertEqual(repeated.status, "already_seen_effect")
self.assertEqual(change(first.state, effect_id=11, now=4).change.disposition, arb.Disposition.DRIVER_INTENT)
def test_foreign_session_cannot_watermark_or_complete_current_actions(self):
ledger = resolve(begin()).state
for event in (arb.BeginAction("other", 999, arb.Origin.DRIVER_CRUISE, "ctx"),
arb.ResolveAction("other", 1, arb.Disposition.SLC_CONSUMED),
arb.CompleteAction("other", 1), arb.CruiseChange("other", 999, 1, 20.0, 25.0)):
result = arb.step(ledger, event, now_ns=100)
self.assertIs(result.state, ledger)
self.assertIsNone(result.change)
self.assertEqual(result.status, "foreign_session")
def test_conflicting_final_resolution_latches_closed(self):
ledger = resolve(begin(), arb.Disposition.SLC_CONSUMED).state
conflict = resolve(ledger, arb.Disposition.DRIVER_INTENT, now=3)
self.assertTrue(conflict.errors)
self.assertTrue(conflict.state.reset_required)
self.assertIsNone(change(conflict.state, now=4).change)
self.assertEqual(conflict.state.transactions[0].disposition, arb.Disposition.SLC_CONSUMED)
def test_repeated_matching_resolution_is_idempotent(self):
ledger = resolve(begin()).state
repeated = resolve(ledger, now=3)
self.assertEqual(repeated.status, "already_resolved")
self.assertFalse(repeated.state.reset_required)
def test_origin_cannot_be_reclassified_as_driver_cruise(self):
for origin in (arb.Origin.AUTOMATIC, arb.Origin.DRIVER_OTHER):
conflict = resolve(begin(origin=origin))
self.assertTrue(conflict.errors)
self.assertIsNone(change(conflict.state).change)
def test_capacity_exhaustion_preserves_consumption_and_latches(self):
ledger = arb.new_ledger(SESSION)
for action_id in range(arb.MAX_TRANSACTIONS):
ledger = begin(ledger, action_id=action_id, now=action_id)
ledger = resolve(ledger, arb.Disposition.SLC_CONSUMED, action_id=0, now=arb.MAX_TRANSACTIONS).state
full = arb.step(ledger, arb.BeginAction(SESSION, arb.MAX_TRANSACTIONS, arb.Origin.DRIVER_CRUISE, "ctx"),
now_ns=arb.MAX_TRANSACTIONS + 1)
self.assertEqual(full.status, "session_reset_required")
self.assertEqual(len(full.state.transactions), arb.MAX_TRANSACTIONS)
self.assertEqual(full.state.transactions[0].disposition, arb.Disposition.SLC_CONSUMED)
self.assertIsNone(change(full.state, now=arb.MAX_TRANSACTIONS + 2).change)
self.assertFalse(arb.new_ledger("next-drive").reset_required)
def test_explicit_completion_releases_capacity(self):
ledger = arb.new_ledger(SESSION)
for action_id in range(arb.MAX_TRANSACTIONS + 10):
ledger = begin(ledger, action_id=action_id, now=2 * action_id)
ledger = arb.step(ledger, arb.CompleteAction(SESSION, action_id), now_ns=2 * action_id + 1).state
self.assertFalse(ledger.reset_required)
self.assertEqual(ledger.transactions, ())
def test_invalid_speeds_and_ids_latch_without_classifying(self):
event = arb.CruiseChange(SESSION, 1, 1, 20.0, 25.0)
invalid = [replace(event, selected_mps=v) for v in (True, 0, -1, float("nan"), float("inf"), 10 ** 1000)]
invalid += [replace(event, effect_id=True), replace(event, action_id=-1), replace(event, previous_mps=0)]
for supplied in invalid:
result = arb.step(resolve(begin()).state, supplied, now_ns=3)
self.assertTrue(result.errors)
self.assertIsNone(result.change)
self.assertTrue(result.state.reset_required)
def test_reversed_clock_latches_and_invalid_state_is_inert(self):
ledger = resolve(begin()).state
reversed_time = change(ledger, now=0)
self.assertTrue(reversed_time.errors)
self.assertTrue(reversed_time.state.reset_required)
for invalid in (replace(ledger, transactions=cast(Any, [])), replace(ledger, last_action_id=-1),
replace(ledger, transactions=ledger.transactions * 2), replace(ledger, session_id="")):
result = change(invalid)
self.assertIs(result.state, invalid)
self.assertIsNone(result.change)
self.assertEqual(result.status, "invalid_state")
def test_receipt_replay_errors_and_wrong_action_do_not_resolve(self):
pending = accept.step(accept.new_session(SESSION), OBSERVATION, AUTHORITY, accept.Policy(), now_ns=0)
accepted = accept.step(pending.state, OBSERVATION, AUTHORITY, accept.Policy(), now_ns=2,
action=accept.DriverAction(SESSION, 1, pending.state.pending.decision_id, accept.ActionKind.ACCEPT))
assert accepted.action_receipt is not None
for decision in (replace(accepted, errors=("invalid input",)), replace(accepted, action_receipt=None),
replace(accepted, action_receipt=replace(accepted.action_receipt, sequence_id=2)),
replace(accepted, action_receipt=replace(accepted.action_receipt, consumed=False, status="seen")),
replace(accepted, action_receipt=replace(accepted.action_receipt, consumed=False, status="already_seen"))):
result = arb.resolve_acceptance(begin(), 1, decision, now_ns=2)
self.assertEqual(result.status, "unresolved_acceptance")
self.assertEqual(change(result.state).change.disposition, arb.Disposition.UNRESOLVED)
def test_malformed_acceptance_result_cannot_classify_driver_intent(self):
decision = accept.Decision(accept.new_session(SESSION))
for malformed in (replace(decision, state=cast(Any, None)),
replace(decision, action_receipt=cast(Any, "consumed")),
replace(decision, action_receipt=accept.ActionReceipt(1, True, cast(Any, 0), "accepted"))):
result = arb.resolve_acceptance(begin(), 1, malformed, now_ns=2)
self.assertTrue(result.errors)
self.assertTrue(result.state.reset_required)
self.assertIsNone(change(result.state).change)
def test_acceptance_clock_must_belong_to_transaction_lifecycle(self):
pending = accept.step(accept.new_session(SESSION), OBSERVATION, AUTHORITY, accept.Policy(), now_ns=0)
accepted = accept.step(pending.state, OBSERVATION, AUTHORITY, accept.Policy(), now_ns=2,
action=accept.DriverAction(SESSION, 1, pending.state.pending.decision_id, accept.ActionKind.ACCEPT))
for timestamp in (0, 100):
invalid = replace(accepted, state=replace(accepted.state, last_timestamp_ns=timestamp))
result = arb.resolve_acceptance(begin(), 1, invalid, now_ns=3)
self.assertTrue(result.errors)
self.assertIsNone(change(result.state, now=4).change)
def test_ledger_is_immutable(self):
ledger = begin()
resolve(ledger)
self.assertEqual(ledger.transactions[0].disposition, arb.Disposition.UNRESOLVED)
with self.assertRaises(FrozenInstanceError):
cast(Any, ledger).session_id = "changed"
if __name__ == "__main__":
unittest.main()
@@ -0,0 +1,269 @@
"""Actual host cereal fixtures through the disabled map-source boundary."""
import base64
import json
import math
import unittest
from dataclasses import replace
from pathlib import Path
from openpilot.starpilot.speed_limits import acceptance as acc
from openpilot.starpilot.speed_limits import map_source as ms
from openpilot.starpilot.speed_limits import selection as sel
ROOT = Path(__file__).resolve().parents[4]
def v2_frame() -> ms.MapFrame:
fixture = json.loads((ROOT / 'mapd_repo/cereal/testdata/mapd_v1_wire.json').read_text())
return replace(ms.decode_event(base64.b64decode(fixture['event_base64'])), version=2)
class MapSourceTests(unittest.TestCase):
def test_diagnostic_metadata_follows_accepted_order_only(self):
frame = v2_frame()
now = 456_100_000_000
tracker = ms.MapTracker()
tracker.step(frame, received=True, now_boot_ns=now)
self.assertEqual(tracker.diagnostic(now_boot_ns=now), ms.MapDiagnostic(frame, 0, 0))
old_loss = replace(frame, status='noGps', event_valid=False, speed_mps=0.0, way_id=0,
tile_loaded=False)
tracker.step(old_loss, received=True, now_boot_ns=now+1)
self.assertEqual(tracker.diagnostic(now_boot_ns=now+1).frame, frame)
switched = replace(frame, source='internal', generation=frame.generation+1,
event_mono_ns=frame.event_mono_ns+2, computed_mono_ns=frame.computed_mono_ns+2)
tracker.step(switched, received=True, now_boot_ns=now+2)
self.assertEqual(tracker.diagnostic(now_boot_ns=now+2), ms.MapDiagnostic(switched, 0, 1))
restarted = replace(switched, producer_session=frame.producer_session+1, generation=1,
event_mono_ns=frame.event_mono_ns+3, computed_mono_ns=frame.computed_mono_ns+3)
tracker.step(restarted, received=True, now_boot_ns=now+3)
self.assertEqual(tracker.diagnostic(now_boot_ns=now+3), ms.MapDiagnostic(restarted, 1, 1))
self.assertIsNone(tracker.diagnostic(now_boot_ns=now+ms.EVENT_MAX_AGE_NS+4).frame)
tracker.step(None, received=False, now_boot_ns=now+ms.EVENT_MAX_AGE_NS+4)
self.assertIsNone(tracker.diagnostic(now_boot_ns=now+ms.EVENT_MAX_AGE_NS+4).frame)
def test_can_gps_retains_source_age_and_unqualified_map_authority(self):
frame = replace(v2_frame(), source='car', gps_mono_ns=453_600_000_000)
now = 456_100_000_000
result = ms.MapTracker().step(frame, received=True, now_boot_ns=now)
self.assertEqual(result.kind, 'matched_limit_unqualified')
self.assertIs(result.observation.kind, acc.ObservationKind.UNKNOWN)
self.assertIsNone(result.observation.candidate)
self.assertEqual(ms.MapTracker().step(frame, received=True, now_boot_ns=now+1).kind, 'stale')
def test_real_provider_and_historical_wire_are_copied_immutably(self):
frame = v2_frame()
self.assertEqual((frame.version, frame.status, frame.source), (2, 'matchedLimit', 'external'))
self.assertEqual((frame.gps_mono_ns, frame.computed_mono_ns, frame.event_mono_ns),
(455_950_000_000, 455_990_000_000, 456_000_000_000))
self.assertEqual((frame.generation, frame.producer_session), (3, 0x1234abcd5011))
result = ms.MapTracker().step(frame, received=True, now_boot_ns=456_100_000_000)
self.assertEqual(result.kind, 'matched_limit_unqualified')
assert result.speed_mps is not None
self.assertAlmostEqual(result.speed_mps, 13.4112, places=4)
self.assertIs(result.observation.kind, acc.ObservationKind.UNKNOWN)
self.assertIsNone(result.observation.candidate)
v1_fixture = json.loads((ROOT / 'mapd_repo/cereal/testdata/mapd_v1_wire.json').read_text())
v1 = ms.decode_event(base64.b64decode(v1_fixture['event_base64']))
self.assertEqual(v1.version, 1)
self.assertEqual(ms.MapTracker().step(v1, received=True, now_boot_ns=456_100_000_000).kind, 'unknown')
fixture = json.loads((ROOT / 'openpilot/starpilot/tests/fixtures/mapd_wire.json').read_text())
old = ms.decode_event(base64.b64decode(fixture['historical']['event_base64']))
self.assertEqual(old.version, 0)
self.assertEqual(ms.MapTracker().step(old, received=True, now_boot_ns=123_100_000_000).kind, 'unknown')
with self.assertRaises(ValueError):
ms.decode_event(b'not capnp')
with self.assertRaises(ValueError):
ms.decode_event(b'\x00' * (ms.MAX_EVENT_BYTES + 1))
def test_time_boundaries_and_malformed_types(self):
frame = v2_frame()
now = frame.computed_mono_ns + ms.COMPUTED_MAX_AGE_NS
self.assertEqual(ms.MapTracker().step(frame, received=True, now_boot_ns=now).kind, 'matched_limit_unqualified')
self.assertEqual(ms.MapTracker().step(frame, received=True, now_boot_ns=now+1).kind, 'stale')
for changed in (
replace(frame, event_mono_ns=now+1), replace(frame, computed_mono_ns=now+1),
replace(frame, gps_mono_ns=now+1), replace(frame, gps_mono_ns=frame.gps_mono_ns-500_000_001),
replace(frame, event_mono_ns=True), replace(frame, computed_mono_ns=True),
replace(frame, gps_mono_ns=True), replace(frame, producer_session=True),
replace(frame, generation=True), replace(frame, version=True),
):
with self.subTest(changed=changed):
self.assertNotIn(ms.MapTracker().step(changed, received=True, now_boot_ns=now).kind,
('matched_limit_unqualified', 'matched_no_limit_unqualified'))
with self.assertRaises(ValueError):
ms.MapTracker().step(frame, received=True, now_boot_ns=True)
def test_matched_no_limit_is_not_absence_or_control(self):
frame = v2_frame()
no_limit = replace(frame, status='matchedNoLimit', speed_mps=0.0)
result = ms.MapTracker().step(no_limit, received=True, now_boot_ns=456_100_000_000)
self.assertEqual(result.kind, 'matched_no_limit_unqualified')
self.assertIsNone(result.speed_mps)
self.assertIs(result.observation.kind, acc.ObservationKind.UNKNOWN)
for changed in (replace(no_limit, speed_mps=13), replace(frame, speed_mps=0),
replace(frame, speed_mps=10 ** 400),
replace(frame, speed_mps=math.nan), replace(frame, event_valid=False),
replace(frame, tile_loaded=False), replace(frame, way_id=0),
replace(frame, way_selection='fail'), replace(frame, way_selection='bogus'),
replace(frame, status='unknown'),
replace(frame, version=1)):
with self.subTest(changed=changed):
self.assertEqual(ms.MapTracker().step(changed, received=True, now_boot_ns=456_100_000_000).kind, 'unknown')
def test_coherent_coverage_and_match_loss_clear_without_becoming_absent(self):
frame = v2_frame()
for status in ('noCoverage', 'noMatch'):
tracker = ms.MapTracker()
tracker.step(frame, received=True, now_boot_ns=456_100_000_000)
loss = replace(frame, status=status, event_valid=False, speed_mps=0.0, way_id=0,
tile_loaded=False, event_mono_ns=frame.event_mono_ns+1,
computed_mono_ns=frame.computed_mono_ns+1)
result = tracker.step(loss, received=True, now_boot_ns=456_100_000_001)
self.assertEqual(result.kind, 'loss')
self.assertIs(result.observation.kind, acc.ObservationKind.UNKNOWN)
self.assertIsNone(result.observation.candidate)
self.assertEqual(tracker.step(frame, received=True, now_boot_ns=456_100_000_002).kind, 'loss')
def test_old_packets_do_not_renew_or_revoke_new_loss_clears(self):
frame = v2_frame()
tracker = ms.MapTracker()
now = 456_100_000_000
tracker.step(frame, received=True, now_boot_ns=now)
self.assertEqual(tracker.step(replace(frame, status='noGps', event_valid=False, speed_mps=0.0, way_id=0,
tile_loaded=False), received=True, now_boot_ns=now+1).kind,
'matched_limit_unqualified') # duplicate timestamp is ignored
loss = replace(frame, status='noGps', event_valid=False, speed_mps=0.0, way_id=0, tile_loaded=False,
event_mono_ns=frame.event_mono_ns+1, computed_mono_ns=frame.computed_mono_ns+1)
self.assertEqual(tracker.step(loss, received=True, now_boot_ns=now+2).kind, 'loss')
self.assertEqual(tracker.step(frame, received=True, now_boot_ns=now+3).kind, 'loss')
self.assertIs(tracker.step(None, received=False, now_boot_ns=now+ms.EVENT_MAX_AGE_NS+2).observation.kind,
acc.ObservationKind.STALE)
self.assertIs(tracker.step(None, received=False, now_boot_ns=now+ms.EVENT_MAX_AGE_NS+3).observation.kind,
acc.ObservationKind.UNKNOWN)
def test_source_generation_restart_and_clock_regression(self):
frame = v2_frame()
now = 456_100_000_000
tracker = ms.MapTracker()
tracker.step(frame, received=True, now_boot_ns=now)
bad_source = replace(frame, source='internal', event_mono_ns=frame.event_mono_ns+1,
computed_mono_ns=frame.computed_mono_ns+1)
self.assertEqual(tracker.step(bad_source, received=True, now_boot_ns=now+1).kind, 'unknown')
self.assertEqual(tracker.step(frame, received=True, now_boot_ns=now+2).kind, 'unknown') # blocked until reset
tracker.reset()
self.assertEqual(tracker.step(frame, received=True, now_boot_ns=now).kind, 'matched_limit_unqualified')
switched = replace(frame, source='internal', generation=4, event_mono_ns=frame.event_mono_ns+1,
computed_mono_ns=frame.computed_mono_ns+1)
self.assertEqual(tracker.step(switched, received=True, now_boot_ns=now+1).kind, 'matched_limit_unqualified')
restarted = replace(switched, producer_session=frame.producer_session+1,
event_mono_ns=frame.event_mono_ns+2, computed_mono_ns=frame.computed_mono_ns+2)
self.assertEqual(tracker.step(restarted, received=True, now_boot_ns=now+2).kind, 'matched_limit_unqualified')
late_old = replace(frame, event_mono_ns=frame.event_mono_ns+3, computed_mono_ns=frame.computed_mono_ns+3)
self.assertEqual(tracker.step(late_old, received=True, now_boot_ns=now+3).kind, 'matched_limit_unqualified')
self.assertEqual(tracker.step(None, received=False, now_boot_ns=now-1).kind, 'unknown')
self.assertEqual(tracker.step(restarted, received=True, now_boot_ns=now+4).kind, 'unknown')
tracker.reset()
self.assertEqual(tracker.step(restarted, received=True, now_boot_ns=now+4).kind, 'matched_limit_unqualified')
def test_future_packet_does_not_poison_order_and_generation_regression_blocks(self):
frame = v2_frame()
now = 456_100_000_000
tracker = ms.MapTracker()
self.assertEqual(tracker.step(frame, received=True, now_boot_ns=now).kind, 'matched_limit_unqualified')
future = replace(frame, event_mono_ns=now+1_000_000_000)
self.assertEqual(tracker.step(future, received=True, now_boot_ns=now+1).kind, 'unknown')
recovered = replace(frame, event_mono_ns=frame.event_mono_ns+1,
computed_mono_ns=frame.computed_mono_ns+1)
self.assertEqual(tracker.step(recovered, received=True, now_boot_ns=now+2).kind, 'matched_limit_unqualified')
newer = replace(recovered, generation=4, event_mono_ns=recovered.event_mono_ns+1,
computed_mono_ns=recovered.computed_mono_ns+1)
self.assertEqual(tracker.step(newer, received=True, now_boot_ns=now+3).kind, 'matched_limit_unqualified')
regressed = replace(newer, generation=3, event_mono_ns=newer.event_mono_ns+1,
computed_mono_ns=newer.computed_mono_ns+1)
self.assertEqual(tracker.step(regressed, received=True, now_boot_ns=now+4).kind, 'unknown')
self.assertEqual(tracker.step(newer, received=True, now_boot_ns=now+5).kind, 'unknown')
def test_bounded_retired_sessions_fail_closed(self):
frame = v2_frame()
tracker = ms.MapTracker()
now = 456_100_000_000
for i in range(ms.MAX_RETIRED_SESSIONS+2):
current = replace(frame, producer_session=100+i, event_mono_ns=frame.event_mono_ns+i,
computed_mono_ns=frame.computed_mono_ns+i)
result = tracker.step(current, received=True, now_boot_ns=now+i)
self.assertEqual(result.kind, 'unknown')
self.assertEqual(tracker.step(frame, received=True, now_boot_ns=now+100).kind, 'unknown')
def test_direct_wire_ranges_are_rejected_without_conversion(self):
frame = v2_frame()
fields = {
'event_mono_ns': (1 << 64), 'gps_mono_ns': (1 << 64),
'computed_mono_ns': (1 << 64), 'generation': (1 << 64),
'producer_session': (1 << 64), 'version': (1 << 16),
'way_id': (1 << 63), 'speed_mps': 1e100,
}
for field, value in fields.items():
with self.subTest(field=field):
result = ms.MapTracker().step(replace(frame, **{field: value}), received=True,
now_boot_ns=456_100_000_000)
self.assertEqual(result.kind, 'unknown')
self.assertEqual(ms.MapTracker().step(replace(frame, way_id=-(1 << 63)-1), received=True,
now_boot_ns=456_100_000_000).kind, 'unknown')
self.assertEqual(ms.MapTracker().step(replace(frame, way_id=-1), received=True,
now_boot_ns=456_100_000_000).kind, 'unknown')
def test_no_gps_retained_source_requires_coherent_timestamp(self):
frame = v2_frame()
loss = replace(frame, status='noGps', event_valid=False, speed_mps=0.0,
tile_loaded=False, way_id=0)
now = 456_100_000_000
self.assertEqual(ms.MapTracker().step(replace(loss, source='none', gps_mono_ns=0),
received=True, now_boot_ns=now).kind, 'loss')
for source in ('external', 'internal'):
self.assertEqual(ms.MapTracker().step(replace(loss, source=source),
received=True, now_boot_ns=now).kind, 'loss')
for gps_mono_ns in (0, loss.computed_mono_ns+1):
with self.subTest(source=source, gps_mono_ns=gps_mono_ns):
self.assertEqual(ms.MapTracker().step(replace(loss, source=source, gps_mono_ns=gps_mono_ns),
received=True, now_boot_ns=now).kind, 'unknown')
self.assertEqual(ms.MapTracker().step(replace(loss, source='none'),
received=True, now_boot_ns=now).kind, 'unknown')
def test_unsupported_wire_does_not_poison_ordering(self):
frame = v2_frame()
now = 456_100_000_000
for changes in ({'version': 1}, {'status': 'unknown'}, {'source': 'unknown'}):
with self.subTest(changes=changes):
tracker = ms.MapTracker()
self.assertEqual(tracker.step(frame, received=True, now_boot_ns=now).kind,
'matched_limit_unqualified')
unsupported = replace(frame, producer_session=frame.producer_session+1, generation=4,
event_mono_ns=frame.event_mono_ns+5,
computed_mono_ns=frame.computed_mono_ns+5, **changes)
self.assertEqual(tracker.step(unsupported, received=True, now_boot_ns=now+5).kind, 'unknown')
valid = replace(frame, producer_session=frame.producer_session+1, generation=4,
event_mono_ns=frame.event_mono_ns+1,
computed_mono_ns=frame.computed_mono_ns+1)
self.assertEqual(tracker.step(valid, received=True, now_boot_ns=now+6).kind,
'matched_limit_unqualified')
def test_selection_and_accepted_fallback_remain_closed(self):
result = ms.MapTracker().step(v2_frame(), received=True, now_boot_ns=456_100_000_000)
observations = {source: acc.Observation(acc.ObservationKind.ABSENT) for source in sel.Source}
observations[sel.Source.MAP] = result.observation
chosen = sel.select_limit(observations, sel.SelectionPolicy(sel.SelectionMode.ORDERED, (sel.Source.MAP, None), False))
self.assertIsNone(chosen.selected_source)
self.assertIs(chosen.observation.kind, acc.ObservationKind.UNKNOWN)
prior_candidate = acc.Candidate('dashboard', acc.ObservationIdentity(acc.IdentityKind.GEOGRAPHIC, value='old-road'), 20.0)
prior = acc.AcceptedLimit(prior_candidate, 'drive', 0)
authority = acc.Authority(acc.Mode.COMBINED, acc.LongitudinalOwner.SYSTEM, True, True, False, False)
decision = acc.step(acc.new_session('drive', prior), chosen.observation, authority,
acc.Policy(fallback_previous=True), now_ns=456_100_000_000)
self.assertIsNone(decision.control_target_mps)
self.assertEqual(decision.basis, 'unknown')
if __name__ == '__main__':
unittest.main()
@@ -0,0 +1,118 @@
"""The adopted SI schedule, legacy zero tail and invalid-source boundaries."""
from pathlib import Path
import tempfile
import unittest
from unittest import mock
from openpilot.common.params import Params
from openpilot.starpilot.speed_limits import offset_document as od
from openpilot.starpilot.speed_limits.runtime_settings import parse, read_params
def offset_at(schedule, speed):
return next((band.offset_mps for band in schedule.bands
if band.lower_mps <= speed and (band.upper_mps is None or speed < band.upper_mps)), None)
class OffsetDocumentTests(unittest.TestCase):
def test_all_legacy_edges_and_frozen_zero_tail(self):
epsilon = 0.00001
for metric, bounds, conversion in ((False, od.IMPERIAL_BOUNDS, od.MPH_TO_MPS),
(True, od.METRIC_BOUNDS, od.KPH_TO_MPS)):
with self.subTest(metric=metric):
values = {'SpeedLimitController': True, 'IsMetric': metric,
**{f'Offset{i}': float(i) for i in range(1, 8)}}
schedule = parse(values).offsets
self.assertEqual(len(schedule.bands), 8)
for index in range(1, 8):
edge = bounds[index]
with self.subTest(edge=edge):
self.assertAlmostEqual(offset_at(schedule, edge - epsilon), index * conversion)
expected = (index + 1) * conversion if index < 7 else 0.0
self.assertAlmostEqual(offset_at(schedule, edge), expected)
self.assertAlmostEqual(offset_at(schedule, edge + epsilon), expected)
self.assertEqual(schedule.bands[-1].lower_mps, bounds[-1])
self.assertEqual(schedule.bands[-1].offset_mps, 0.0)
def test_adopted_document_is_sole_numeric_authority(self):
document = od.adopt_legacy(False, (0, 1, -2, 3, 4, 5, 7))
encoded = od.encode(document)
self.assertEqual(od.decode(encoded), document)
expected = document.schedule()
for metric in (False, True, b'corrupt'):
settings = parse({'SpeedLimitController': True, 'SLCOffsetSchedule': document,
'IsMetric': metric, 'Offset3': float('nan'), 'Offset7': 'bad'})
self.assertTrue(settings.enabled)
self.assertEqual(settings.offsets, expected)
self.assertEqual(offset_at(expected, od.IMPERIAL_BOUNDS[-1]), 0.0)
with tempfile.TemporaryDirectory() as directory:
params = Params(directory)
params.put_bool('SpeedLimitController', True, block=True)
Path(params.get_param_path('SLCOffsetSchedule')).write_bytes(encoded)
Path(params.get_param_path('IsMetric')).write_bytes(b'bad')
Path(params.get_param_path('Offset3')).write_bytes(b'bad')
original_read = Path.read_bytes
def no_legacy_read(path):
if path.name == 'IsMetric' or path.name.startswith('Offset'):
raise AssertionError(f'adopted runtime read legacy numeric source: {path.name}')
return original_read(path)
with mock.patch.object(Path, 'read_bytes', no_legacy_read):
self.assertEqual(read_params(params).offsets, expected)
self.assertTrue(read_params(params).enabled)
def test_needs_review_marker_prevents_legacy_reactivation(self):
self.assertIs(od.decode(od.encode(od.NEEDS_REVIEW)), od.NEEDS_REVIEW)
settings = parse({'SpeedLimitController': True, 'SLCOffsetSchedule': od.NEEDS_REVIEW})
self.assertFalse(settings.enabled)
with tempfile.TemporaryDirectory() as directory:
params = Params(directory)
params.put_bool('SpeedLimitController', True, block=True)
Path(params.get_param_path('SLCOffsetSchedule')).write_bytes(od.encode(od.NEEDS_REVIEW))
params.put_bool('IsMetric', True, block=True)
self.assertFalse(read_params(params).enabled)
def test_registered_json_params_roundtrip_and_preserved_legacy_bytes(self):
with tempfile.TemporaryDirectory() as directory:
params = Params(directory)
params.put_bool('SpeedLimitController', True, block=True)
legacy = Path(params.get_param_path('Offset7'))
legacy.write_bytes(b'5.0000')
document = od.adopt_legacy(False, (0, 1, 2, 3, 4, 5, 5))
params.put('SLCOffsetSchedule', od.to_value(document), block=True)
self.assertEqual(params.get('SLCOffsetSchedule'), od.to_value(document))
self.assertEqual(read_params(params).offsets, document.schedule())
self.assertEqual(legacy.read_bytes(), b'5.0000')
params.put('SLCOffsetSchedule', od.to_value(od.NEEDS_REVIEW), block=True)
self.assertEqual(params.get('SLCOffsetSchedule'), od.to_value(od.NEEDS_REVIEW))
self.assertFalse(read_params(params).enabled)
self.assertEqual(legacy.read_bytes(), b'5.0000')
def test_malformed_documents_never_fall_back_to_legacy(self):
bad = (b'', b'{', b'\xff', b'{}', b'{"version":2}', b'{"version":1,"state":"other"}',
b'{"version":1,"state":"needs_review","extra":1}',
b'{"version":1,"version":1,"state":"needs_review"}',
b'{"version":1,"bounds_mps":[0,1,2,3,4,5,6,7],"offsets_mps":[0,0,0,0,0,0,NaN]}',
b'[' * 1100 + b'0' + b']' * 1100,
b'x' * (od.MAX_DOCUMENT_BYTES + 1),
b'x' * (1024 * 1024))
for raw in bad:
with self.subTest(raw=raw[:30]):
with self.assertRaises(ValueError):
od.decode(raw)
with tempfile.TemporaryDirectory() as directory:
params = Params(directory)
params.put_bool('SpeedLimitController', True, block=True)
Path(params.get_param_path('SLCOffsetSchedule')).write_bytes(raw)
self.assertFalse(read_params(params).enabled)
valid = od.adopt_legacy(True, (0, 0, 0, 0, 0, 0, 0))
for altered in ({'version': True, 'bounds_mps': valid.bounds_mps, 'offsets_mps': valid.offsets_mps},
{'version': 1, 'bounds_mps': (0, 1, 1, 3, 4, 5, 6, 7), 'offsets_mps': valid.offsets_mps},
{'version': 1, 'bounds_mps': valid.bounds_mps, 'offsets_mps': (0, 0, 0, 0, 0, 0, True)}):
with self.assertRaises(ValueError):
od.validate(altered)
if __name__ == '__main__':
unittest.main()
@@ -0,0 +1,339 @@
"""Override intent requires the acceptance decision and causal ledger together."""
import unittest
from dataclasses import replace
from openpilot.starpilot.speed_limits import acceptance as acc
from openpilot.starpilot.speed_limits import action_arbitration as arb
from openpilot.starpilot.speed_limits import overrides as ov
SESSION = "drive"
HIGH = acc.Candidate("dashboard", acc.ObservationIdentity(acc.IdentityKind.SESSION_VALUE, session_id=SESSION), 20.0)
LOW = acc.Candidate("map", acc.ObservationIdentity(acc.IdentityKind.GEOGRAPHIC, value="road-2"), 15.0)
SAME = acc.Candidate("map", acc.ObservationIdentity(acc.IdentityKind.GEOGRAPHIC, value="road-3"), 20.0)
ACTIVE = acc.Authority(acc.Mode.LONGITUDINAL_ONLY, acc.LongitudinalOwner.SYSTEM, False, True, False, False)
POLICY = acc.Policy(confirm_lower=False, confirm_higher=False)
PEDAL_OFF = ov.PedalEvidence(ov.EvidenceKind.VALID, False)
def observation(candidate=HIGH):
return acc.Observation(acc.ObservationKind.VALID, candidate)
class OverrideTests(unittest.TestCase):
def setUp(self):
self.accepted = acc.step(acc.new_session(SESSION), observation(), ACTIVE, POLICY, now_ns=0)
self.state = ov.new_session(SESSION)
self.selected = ov.SelectedEvidence(ov.EvidenceKind.VALID, 20.0)
self.first = self.reduce(0)
def decision(self, now, *, candidate=HIGH, authority=ACTIVE, policy=POLICY, source=None, adopt=None):
obs = observation(candidate) if source is None else acc.Observation(source)
result = acc.step(self.accepted.state, obs, authority, policy, now_ns=now, adopt=adopt)
self.accepted = result
return result
def reduce(self, now, *, acceptance=None, authority=ACTIVE, policy=POLICY, selected=None, pedal=PEDAL_OFF, change=None):
result = ov.step(self.state, self.accepted if acceptance is None else acceptance, authority, policy,
self.selected if selected is None else selected, pedal, now_ns=now, change=change)
self.state = result.state
return result
def classified(self, *, effect=1, now=2, chosen=25.0, context=None, disposition=arb.Disposition.DRIVER_INTENT):
ledger = arb.new_ledger(SESSION)
context = self.first.context if context is None else context
ledger = arb.step(ledger, arb.BeginAction(SESSION, 1, arb.Origin.DRIVER_CRUISE, context.context_id), now_ns=1).state
ledger = arb.step(ledger, arb.ResolveAction(SESSION, 1, disposition), now_ns=1).state
result = arb.step(ledger, arb.CruiseChange(SESSION, effect, 1, 20.0, chosen), now_ns=now)
return result.change
def arm(self, now=2, chosen=25.0):
change = self.classified(now=now, chosen=chosen)
self.decision(now)
return self.reduce(now, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, chosen), change=change)
def test_fresh_causal_intent_persists_and_scalar_does_not_rearm(self):
armed = self.arm()
self.assertEqual(armed.contribution_mps, 25.0)
self.assertEqual(armed.event_receipt.status, "driver_intent")
self.assertTrue(armed.event_receipt.consumed)
self.decision(3)
self.assertEqual(self.reduce(3, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0)).contribution_mps, 25.0)
self.decision(4, candidate=LOW)
self.assertIsNone(self.reduce(4, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0)).contribution_mps)
self.decision(5, candidate=LOW)
self.assertIsNone(self.reduce(5, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0)).contribution_mps)
def test_higher_target_preserves_until_it_catches_up_and_same_speed_source_keeps_it(self):
self.arm(chosen=25.0)
self.decision(3, candidate=SAME)
self.assertEqual(self.reduce(3, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0)).contribution_mps, 25.0)
mid = acc.Candidate("map", acc.ObservationIdentity(acc.IdentityKind.GEOGRAPHIC, value="road-4"), 22.0)
self.decision(4, candidate=mid)
self.assertEqual(self.reduce(4, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0)).contribution_mps, 25.0)
caught = replace(mid, speed_mps=25.0)
self.decision(5, candidate=caught)
self.assertIsNone(self.reduce(5, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0)).contribution_mps)
def test_pedal_tracks_current_ego_and_release_restores_persistence(self):
self.arm()
self.decision(3)
self.assertEqual(self.reduce(3, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0),
pedal=ov.PedalEvidence(ov.EvidenceKind.VALID, True, 28.0)).contribution_mps, 28.0)
self.decision(4)
self.assertEqual(self.reduce(4, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0),
pedal=ov.PedalEvidence(ov.EvidenceKind.VALID, True, 23.0)).contribution_mps, 23.0)
self.decision(5)
self.assertEqual(self.reduce(5, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0)).contribution_mps, 25.0)
def test_temporary_inactivity_invalidates_origin_and_grace_expires(self):
self.arm()
paused = replace(ACTIVE, longitudinal_active=False)
self.decision(3, authority=paused)
self.assertIsNone(self.reduce(3, authority=paused, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0)).context)
self.decision(3 + ov.SUSPENSION_NS - 1, authority=paused)
self.reduce(3 + ov.SUSPENSION_NS - 1, authority=paused, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0))
self.decision(3 + ov.SUSPENSION_NS - 1)
resumed = self.reduce(3 + ov.SUSPENSION_NS - 1, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0))
self.assertEqual(resumed.contribution_mps, 25.0)
self.assertNotEqual(resumed.context.context_id, self.first.context.context_id)
self.decision(ov.SUSPENSION_NS + 10, authority=paused)
self.reduce(ov.SUSPENSION_NS + 10, authority=paused, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0))
self.decision(2 * ov.SUSPENSION_NS + 10)
self.assertIsNone(self.reduce(2 * ov.SUSPENSION_NS + 10, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0)).contribution_mps)
def test_stale_missing_and_display_only_suppress_output_and_watermark(self):
self.arm()
change = self.classified(effect=2, now=3, chosen=26.0)
self.decision(3, source=acc.ObservationKind.STALE)
stale = self.reduce(3, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 26.0), change=change)
self.assertIsNone(stale.contribution_mps)
self.assertEqual(stale.state.last_effect_id, 2)
self.decision(4)
self.assertEqual(self.reduce(4, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0), change=change).event_receipt.status, "already_seen")
display = acc.Policy(confirm_lower=False, confirm_higher=False, display_only=True)
self.decision(5, policy=display)
self.assertIsNone(self.reduce(5, policy=display, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0)).contribution_mps)
def test_absent_uses_only_actual_fallback_decision(self):
self.arm()
fallback = acc.Policy(confirm_lower=False, confirm_higher=False, fallback_previous=True)
self.decision(3, source=acc.ObservationKind.ABSENT, policy=fallback)
self.assertEqual(self.reduce(3, policy=fallback, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0)).contribution_mps, 25.0)
self.decision(4, source=acc.ObservationKind.ABSENT)
self.assertIsNone(self.reduce(4, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0)).contribution_mps)
def test_delayed_acceptance_consumed_effect_cannot_arm_after_release(self):
pending_policy = acc.Policy(confirm_lower=True, confirm_higher=True)
pending = acc.step(acc.new_session(SESSION), observation(LOW), ACTIVE, pending_policy, now_ns=0)
action = acc.DriverAction(SESSION, 1, pending.state.pending.decision_id, acc.ActionKind.ACCEPT)
ledger = arb.step(arb.new_ledger(SESSION), arb.BeginAction(SESSION, 1, arb.Origin.DRIVER_CRUISE,
self.first.context.context_id), now_ns=1).state
accepted = acc.step(pending.state, observation(LOW), ACTIVE, pending_policy, now_ns=2, action=action)
ledger = arb.resolve_acceptance(ledger, 1, accepted, now_ns=2).state
delayed = arb.step(ledger, arb.CruiseChange(SESSION, 1, 1, 20.0, 25.0), now_ns=60_000_000_000).change
self.assertEqual(delayed.disposition, arb.Disposition.SLC_CONSUMED)
self.decision(60_000_000_000)
result = self.reduce(60_000_000_000, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0), change=delayed)
self.assertIsNone(result.contribution_mps)
self.assertEqual(result.state.last_effect_id, 1)
def test_adoption_clears_once_and_current_pedal_still_contributes(self):
self.arm()
shown = self.decision(3, candidate=LOW)
adopt = acc.AdoptRequest(SESSION, 2, shown.state.presentation.presentation_id, LOW)
adopted = self.decision(4, candidate=LOW, adopt=adopt)
result = self.reduce(4, acceptance=adopted, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0),
pedal=ov.PedalEvidence(ov.EvidenceKind.VALID, True, 27.0))
self.assertIsNone(result.state.persistent_selected_mps)
self.assertEqual(result.contribution_mps, 27.0)
self.assertNotEqual(result.context.context_id, self.first.context.context_id)
duplicate = self.reduce(4, acceptance=adopted, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0))
self.assertEqual(duplicate.context, result.context)
def test_mismatched_snapshot_and_inactive_origin_fail_closed(self):
change = self.classified()
self.decision(2)
mismatch = self.reduce(2, policy=replace(POLICY, fallback_previous=True), change=change)
self.assertTrue(mismatch.errors)
self.assertEqual(mismatch.state.last_effect_id, 1)
self.assertTrue(mismatch.state.reset_required)
self.decision(3)
self.assertIsNone(self.reduce(3).context)
self.state = ov.new_session(SESSION)
old = self.reduce(3).context
paused = replace(ACTIVE, longitudinal_active=False)
self.decision(4, authority=paused)
self.reduce(4, authority=paused)
self.decision(5)
late = replace(change, effect_id=2, context_id=old.context_id, started_at_ns=4)
result = self.reduce(5, change=late, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0))
self.assertIsNone(result.contribution_mps)
self.assertEqual(result.event_receipt.status, "unqualified_effect")
def test_pending_lower_and_rejection_keep_accepted_target_and_intent(self):
self.arm()
confirmation = acc.Policy(confirm_lower=True, confirm_higher=True)
pending = self.decision(3, candidate=LOW, policy=confirmation)
self.assertEqual(pending.basis, "pending")
self.assertEqual(self.reduce(3, policy=confirmation, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0)).contribution_mps, 25.0)
action = acc.DriverAction(SESSION, 2, pending.state.pending.decision_id, acc.ActionKind.REJECT)
rejected = acc.step(pending.state, observation(LOW), ACTIVE, confirmation, now_ns=4, action=action)
self.accepted = rejected
self.assertEqual(self.reduce(4, policy=confirmation, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0)).contribution_mps, 25.0)
self.assertIn(LOW, rejected.state.rejected)
def test_mode_owner_change_clears_even_when_brief(self):
self.arm()
stock = acc.Authority(acc.Mode.LONGITUDINAL_ONLY, acc.LongitudinalOwner.STOCK, False, False, True, False)
self.decision(3, authority=stock)
self.reduce(3, authority=stock, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0))
self.assertIsNone(self.state.persistent_selected_mps)
self.decision(4)
self.assertIsNone(self.reduce(4, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0)).contribution_mps)
def test_previous_session_history_can_supply_explicit_absent_fallback(self):
prior = acc.AcceptedLimit(acc.Candidate("map", acc.ObservationIdentity(acc.IdentityKind.GEOGRAPHIC, value="road"), 20.0),
"prior-drive", 0)
fallback = acc.Policy(fallback_previous=True)
decision = acc.step(acc.new_session(SESSION, prior), acc.Observation(acc.ObservationKind.ABSENT), ACTIVE, fallback, now_ns=1)
result = ov.step(ov.new_session(SESSION), decision, ACTIVE, fallback, self.selected, PEDAL_OFF, now_ns=1)
self.assertFalse(result.errors)
self.assertIsNotNone(result.context)
def test_automatic_foreign_and_completed_effects_are_watermarked_without_intent(self):
self.decision(2)
automatic = replace(self.classified(now=2), disposition=arb.Disposition.AUTOMATIC)
got = self.reduce(2, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0), change=automatic)
self.assertIsNone(got.contribution_mps)
self.assertEqual(got.state.last_effect_id, 1)
self.decision(3)
foreign = replace(automatic, session_id="other", effect_id=2, disposition=arb.Disposition.DRIVER_INTENT)
self.assertEqual(self.reduce(3, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0), change=foreign).event_receipt.status,
"foreign_session")
self.decision(4)
completed = replace(automatic, effect_id=2, action_id=1, context_id=None, disposition=arb.Disposition.UNRESOLVED)
result = self.reduce(4, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0), change=completed)
self.assertIsNone(result.contribution_mps)
self.assertEqual(result.state.last_effect_id, 2)
def test_inconsistent_acceptance_target_and_reversed_clock_fail_closed(self):
self.decision(2)
forged = replace(self.accepted, control_target_mps=21.0)
bad = self.reduce(2, acceptance=forged)
self.assertTrue(bad.errors)
self.assertIsNone(bad.contribution_mps)
self.assertTrue(bad.state.reset_required)
reversed_clock = self.reduce(1)
self.assertTrue(reversed_clock.errors)
def test_zero_ego_speed_is_valid_but_negative_bool_and_nan_are_not(self):
self.arm()
for now, pressed in ((3, True), (4, False)):
self.decision(now)
zero = self.reduce(now, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0),
pedal=ov.PedalEvidence(ov.EvidenceKind.VALID, pressed, 0.0))
self.assertFalse(zero.errors)
self.assertEqual(zero.contribution_mps, 25.0)
for invalid in (-0.1, True, float("nan")):
with self.subTest(invalid=invalid):
self.decision(5)
bad = ov.step(self.state, self.accepted, ACTIVE, POLICY, ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0),
ov.PedalEvidence(ov.EvidenceKind.VALID, True, invalid), now_ns=5)
self.assertTrue(bad.errors)
self.assertTrue(bad.state.reset_required)
def test_malformed_effect_latches_session_and_cannot_replay_after_repair(self):
self.arm()
fresh = self.classified(effect=2, now=4, chosen=26.0)
self.decision(3)
malformed = replace(fresh, selected_mps=float("nan"))
bad = self.reduce(3, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 26.0), change=malformed)
self.assertTrue(bad.state.reset_required)
self.assertIsNone(bad.state.persistent_selected_mps)
self.assertIsNone(bad.state.context)
self.decision(4)
retry = self.reduce(4, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 26.0), change=fresh)
self.assertIsNone(retry.contribution_mps)
self.assertEqual(retry.basis, "session_reset_required")
next_candidate = acc.Candidate("map", acc.ObservationIdentity(acc.IdentityKind.GEOGRAPHIC, value="new-road"), 20.0)
next_acceptance = acc.step(acc.new_session("next-drive"), observation(next_candidate), ACTIVE, POLICY, now_ns=0)
next_state = ov.step(ov.new_session("next-drive"), next_acceptance, ACTIVE, POLICY, self.selected, PEDAL_OFF, now_ns=0)
next_acceptance = acc.step(next_acceptance.state, observation(next_candidate), ACTIVE, POLICY, now_ns=1)
replay = ov.step(next_state.state, next_acceptance, ACTIVE, POLICY, ov.SelectedEvidence(ov.EvidenceKind.VALID, 26.0),
PEDAL_OFF, now_ns=1, change=fresh)
self.assertIsNone(replay.contribution_mps)
self.assertEqual(replay.event_receipt.status, "foreign_session")
def test_malformed_state_is_inert(self):
forged = replace(self.state, context=replace(self.first.context, context_id="caller-proof"))
self.decision(2)
result = ov.step(forged, self.accepted, ACTIVE, POLICY, self.selected, PEDAL_OFF, now_ns=2)
self.assertTrue(result.errors)
self.assertIsNone(result.contribution_mps)
self.assertIs(result.state, forged)
def test_reversed_clock_clears_previously_armed_state_and_stays_latched(self):
self.arm()
bad = self.reduce(1)
self.assertTrue(bad.state.reset_required)
self.assertIsNone(bad.state.persistent_selected_mps)
self.assertIsNone(bad.context)
self.decision(3)
recovered_input = self.reduce(3)
self.assertEqual(recovered_input.basis, "session_reset_required")
self.assertIsNone(recovered_input.contribution_mps)
def test_target_change_requires_new_action_from_the_new_eligible_context(self):
self.arm()
old_effect = self.classified(effect=2, now=3, chosen=26.0)
self.decision(3, candidate=LOW)
lower = self.reduce(3, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 26.0), change=old_effect)
self.assertIsNone(lower.contribution_mps)
self.assertFalse(lower.event_receipt.consumed)
self.assertNotEqual(lower.context, self.first.context)
ledger = arb.step(arb.new_ledger(SESSION), arb.BeginAction(SESSION, 2, arb.Origin.DRIVER_CRUISE,
lower.context.context_id), now_ns=4).state
ledger = arb.step(ledger, arb.ResolveAction(SESSION, 2, arb.Disposition.DRIVER_INTENT), now_ns=4).state
fresh = arb.step(ledger, arb.CruiseChange(SESSION, 3, 2, 26.0, 27.0), now_ns=5).change
self.decision(5, candidate=LOW)
rearmed = self.reduce(5, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 27.0), change=fresh)
self.assertTrue(rearmed.event_receipt.consumed)
self.assertEqual(rearmed.contribution_mps, 27.0)
def test_missing_selected_or_pedal_evidence_suppresses_output(self):
self.arm()
for now, selected, pedal in (
(3, ov.SelectedEvidence(ov.EvidenceKind.STALE), PEDAL_OFF),
(4, ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0), ov.PedalEvidence(ov.EvidenceKind.UNKNOWN)),
(5, ov.SelectedEvidence(ov.EvidenceKind.ABSENT), PEDAL_OFF),
):
self.decision(now)
result = self.reduce(now, selected=selected, pedal=pedal)
self.assertIsNone(result.contribution_mps)
self.assertIsNone(result.context)
def test_same_value_adoption_rotates_once_and_delayed_effect_is_consumed(self):
self.arm()
shown = self.decision(3)
request = acc.AdoptRequest(SESSION, 2, shown.state.presentation.presentation_id, HIGH)
ledger = arb.step(arb.new_ledger(SESSION), arb.BeginAction(SESSION, 2, arb.Origin.DRIVER_CRUISE,
self.first.context.context_id), now_ns=3).state
adopted = self.decision(4, adopt=request)
ledger = arb.resolve_acceptance(ledger, 2, adopted, now_ns=4).state
result = self.reduce(4, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0))
self.assertIsNone(result.state.persistent_selected_mps)
self.assertNotEqual(result.context.context_id, self.first.context.context_id)
again = self.reduce(4, acceptance=adopted, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 25.0))
self.assertEqual(again.context, result.context)
delayed = arb.step(ledger, arb.CruiseChange(SESSION, 3, 2, 25.0, 20.0), now_ns=10_000_000_000).change
self.assertEqual(delayed.disposition, arb.Disposition.SLC_CONSUMED)
self.decision(10_000_000_000)
no_rearm = self.reduce(10_000_000_000, selected=ov.SelectedEvidence(ov.EvidenceKind.VALID, 20.0), change=delayed)
self.assertIsNone(no_rearm.contribution_mps)
self.assertEqual(no_rearm.event_receipt.status, "unqualified_effect")
if __name__ == "__main__":
unittest.main()
@@ -0,0 +1,104 @@
"""Persisted SLC corruption must not silently select a less restrictive default."""
from pathlib import Path
import tempfile
import unittest
from openpilot.common.params import Params
from openpilot.starpilot.speed_limits.runtime_settings import MPH_TO_MPS, parse, read_params
class RuntimeSettingsTests(unittest.TestCase):
def test_fresh_vision_display_default_preserves_explicit_saved_choices(self):
from openpilot.starpilot.speed_limits.selection import Source
with tempfile.TemporaryDirectory() as directory:
params = Params(directory)
settings = read_params(params)
self.assertTrue(settings.display)
self.assertFalse(settings.enabled)
self.assertEqual(settings.selection.slots, (Source.VISION, Source.DASHBOARD))
self.assertTrue(settings.acceptance.display_only)
params.put_bool('ShowSpeedLimits', False, block=True)
params.put('SLCPriority1', 'Dashboard', block=True)
params.put('SLCPriority2', 'Map Data', block=True)
saved = read_params(params)
self.assertFalse(saved.display)
self.assertFalse(saved.enabled)
self.assertEqual(saved.selection.slots, (Source.DASHBOARD, Source.MAP))
self.assertEqual(Path(params.get_param_path('ShowSpeedLimits')).read_bytes(), b'0')
self.assertEqual(Path(params.get_param_path('SLCPriority1')).read_bytes(), b'Dashboard')
self.assertEqual(Path(params.get_param_path('SLCPriority2')).read_bytes(), b'Map Data')
def test_corrupt_saved_values_disable_control_without_rewriting(self):
cases = {'SLCConfirmation': b'invalid', 'SLCFallback': b'broken',
'Offset3': b'bad', 'IsMetric': b'2', 'SLCPriority1': b'\xff'}
for key, raw in cases.items():
with self.subTest(key=key), tempfile.TemporaryDirectory() as directory:
params = Params(directory)
params.put_bool('SpeedLimitController', True, block=True)
path = Path(params.get_param_path(key))
path.write_bytes(raw)
settings = read_params(params)
self.assertFalse(settings.enabled)
self.assertTrue(settings.errors)
self.assertEqual(path.read_bytes(), raw)
self.assertTrue(params.get_bool('SpeedLimitController'))
def test_absent_optional_values_keep_existing_defaults(self):
with tempfile.TemporaryDirectory() as directory:
params = Params(directory)
params.put_bool('SpeedLimitController', True, block=True)
settings = read_params(params)
self.assertTrue(settings.enabled)
self.assertFalse(settings.errors)
self.assertTrue(settings.acceptance.fallback_previous)
self.assertEqual(settings.fallback_choice, 2)
self.assertTrue(all(band.offset_mps == 0 for band in settings.offsets.bands))
def test_valid_native_values_preserve_explicit_zero_and_units(self):
with tempfile.TemporaryDirectory() as directory:
params = Params(directory)
params.put_bool('SpeedLimitController', True, block=True)
params.put_bool('SLCConfirmation', True, block=True)
params.put_bool('SLCConfirmationLower', True, block=True)
params.put('SLCFallback', 0, block=True)
params.put('Offset3', -5.0, block=True)
settings = read_params(params)
self.assertTrue(settings.enabled)
self.assertFalse(settings.errors)
self.assertTrue(settings.acceptance.confirm_lower)
self.assertFalse(settings.acceptance.fallback_previous)
self.assertEqual(settings.fallback_choice, 0)
self.assertAlmostEqual(settings.offsets.bands[2].offset_mps, -5 * MPH_TO_MPS)
def test_fallback_choice_is_a_validated_immutable_drive_snapshot(self):
with tempfile.TemporaryDirectory() as directory:
params = Params(directory)
params.put_bool('SpeedLimitController', True, block=True)
snapshots = []
for choice in (0, 1, 2):
params.put('SLCFallback', choice, block=True)
snapshot = read_params(params)
self.assertFalse(snapshot.errors)
self.assertEqual(snapshot.fallback_choice, choice)
self.assertEqual(snapshot.acceptance.fallback_previous, choice == 2)
snapshots.append(snapshot)
self.assertEqual(tuple(snapshot.fallback_choice for snapshot in snapshots), (0, 1, 2))
path = Path(params.get_param_path('SLCFallback'))
path.write_bytes(b'not-an-int')
unavailable = read_params(params)
self.assertFalse(unavailable.enabled)
self.assertTrue(unavailable.errors)
self.assertEqual(path.read_bytes(), b'not-an-int')
self.assertEqual(tuple(snapshot.fallback_choice for snapshot in snapshots), (0, 1, 2))
def test_nonfinite_or_unrepresentable_offset_disables_control(self):
for value in (float('nan'), float('inf'), 10 ** 1000):
with self.subTest(kind=type(value).__name__):
settings = parse({'SpeedLimitController': True, 'Offset3': value})
self.assertFalse(settings.enabled)
self.assertTrue(settings.errors)
if __name__ == '__main__':
unittest.main()
@@ -0,0 +1,249 @@
from itertools import permutations, product
import math
from typing import Any, cast
import unittest
from openpilot.starpilot.speed_limits.acceptance import Candidate, IdentityKind, Observation, ObservationIdentity, ObservationKind
from openpilot.starpilot.speed_limits.acceptance import ActionKind, AdoptRequest, Authority, DriverAction, LongitudinalOwner, Mode, Policy, new_session, step
from openpilot.starpilot.speed_limits.selection import SelectionMode, SelectionPolicy, Source, select_limit
def geographic(value):
return ObservationIdentity(IdentityKind.GEOGRAPHIC, value=value)
def observations(**speeds):
values = {source: Observation(ObservationKind.ABSENT) for source in Source}
values.update({Source(source): Observation(ObservationKind.VALID, Candidate(source, geographic(f"{source}-zone"), speed))
for source, speed in speeds.items()})
return values
def policy(mode=SelectionMode.ORDERED, slots=(Source.VISION, Source.MAP), online=True):
return SelectionPolicy(mode, slots, online)
class TestSourceSelection(unittest.TestCase):
def test_priority_order_and_unused_source(self):
supplied = observations(dashboard=30, map=20, vision=10, online=40)
for first, second in permutations((Source.DASHBOARD, Source.MAP, Source.VISION), 2):
with self.subTest(first=first, second=second):
selected = select_limit(supplied, policy(slots=(first, second)))
self.assertIs(selected.observation, supplied[first])
self.assertEqual(selected.selected_source, first)
missing = dict(supplied)
missing[first] = Observation(ObservationKind.ABSENT)
self.assertEqual(select_limit(missing, policy(slots=(first, second))).selected_source, second)
self.assertEqual(select_limit(supplied, policy(slots=(Source.MAP, Source.MAP))).selected_source, Source.MAP)
def test_extrema_vision_requires_explicit_slot(self):
for mode, vision in ((SelectionMode.HIGHEST, 40), (SelectionMode.LOWEST, 5)):
for slots in ((Source.MAP, Source.DASHBOARD), (None, None), (Source.MAP, Source.MAP)):
with self.subTest(mode=mode, slots=slots):
selected = select_limit(observations(dashboard=30, map=20, vision=vision), policy(mode, slots))
self.assertEqual(selected.selected_source, Source.DASHBOARD if mode == SelectionMode.HIGHEST else Source.MAP)
self.assertNotIn(Source.VISION, selected.considered)
for slots in ((Source.VISION, None), (None, Source.VISION)):
selected = select_limit(observations(dashboard=30, map=20, vision=vision), policy(mode, slots))
self.assertEqual(selected.selected_source, Source.VISION)
def test_extrema_ties_keep_stable_dashboard_map_vision_order(self):
original = observations(dashboard=20, map=20, vision=20)
for mode in (SelectionMode.HIGHEST, SelectionMode.LOWEST):
for order in permutations(Source):
selected = select_limit({source: original[source] for source in order}, policy(mode))
self.assertEqual(selected.selected_source, Source.DASHBOARD)
original_without_dashboard = dict(original)
original_without_dashboard[Source.DASHBOARD] = Observation(ObservationKind.ABSENT)
self.assertEqual(select_limit(original_without_dashboard, policy(mode)).selected_source, Source.MAP)
def test_online_never_preempts_valid_primary(self):
for mode in SelectionMode:
for online in (1.0, 50.0):
selected = select_limit(observations(map=20, online=online), policy(mode))
self.assertEqual(selected.selected_source, Source.MAP)
self.assertNotIn(Source.ONLINE, selected.considered)
def test_online_fallback_and_disabled_provider(self):
# Dashboard is valid but not configured in ordered mode; it cannot preempt fallback.
supplied = observations(dashboard=30, online=20)
selected = select_limit(supplied, policy())
self.assertEqual(selected.selected_source, Source.ONLINE)
self.assertEqual(selected.reason, "online_fallback")
for slots in ((Source.VISION, Source.MAP), (None, None)):
selected = select_limit(supplied, policy(slots=slots, online=False))
self.assertIsNone(selected.selected_source)
self.assertEqual(selected.observation.kind, ObservationKind.ABSENT)
def test_incomplete_sources_do_not_become_absent(self):
missing = (ObservationKind.ABSENT, ObservationKind.UNKNOWN, ObservationKind.STALE)
for first, second, online in product(missing, repeat=3):
supplied = observations()
for source, kind in ((Source.VISION, first), (Source.MAP, second), (Source.ONLINE, online)):
supplied[source] = Observation(kind)
selected = select_limit(supplied, policy())
with self.subTest(first=first, second=second, online=online):
self.assertIsNone(selected.selected_source)
self.assertIsNone(selected.observation.candidate)
if ObservationKind.UNKNOWN in (first, second, online):
self.assertEqual(selected.observation.kind, ObservationKind.UNKNOWN)
elif ObservationKind.STALE in (first, second, online):
self.assertEqual(selected.observation.kind, ObservationKind.STALE)
else:
self.assertEqual(selected.observation.kind, ObservationKind.ABSENT)
self.assertEqual(selected.unavailable, ((Source.VISION, first), (Source.MAP, second), (Source.ONLINE, online)))
def test_healthy_alternative_can_replace_unavailable_source(self):
for unavailable in (ObservationKind.UNKNOWN, ObservationKind.STALE):
supplied = observations(map=20, online=25)
supplied[Source.VISION] = Observation(unavailable)
selected = select_limit(supplied, policy())
self.assertEqual(selected.selected_source, Source.MAP)
self.assertIn((Source.VISION, unavailable), selected.unavailable)
supplied[Source.MAP] = Observation(ObservationKind.ABSENT)
self.assertEqual(select_limit(supplied, policy()).selected_source, Source.ONLINE)
def test_disabled_or_unselected_unknown_sources_do_not_block_absence(self):
supplied = observations()
supplied[Source.VISION] = supplied[Source.ONLINE] = Observation(ObservationKind.UNKNOWN)
selected = select_limit(supplied, policy(slots=(Source.DASHBOARD, Source.MAP), online=False))
self.assertEqual(selected.observation.kind, ObservationKind.ABSENT)
def test_minimum_threshold_is_explicit_and_inclusive(self):
supplied = observations(vision=math.nextafter(1.0, 0.0), map=1.0)
selected = select_limit(supplied, policy())
self.assertEqual(selected.selected_source, Source.MAP)
self.assertEqual(selected.below_minimum, (Source.VISION,))
supplied[Source.MAP] = Observation(ObservationKind.ABSENT)
selected = select_limit(supplied, policy(online=False))
self.assertEqual(selected.observation.kind, ObservationKind.ABSENT)
def test_malformed_candidates_fail_closed(self):
candidates = [Candidate("dashboard", geographic("zone"), 20), Candidate("vision", geographic(""), 20),
Candidate("vision", geographic("zone"), float("nan")), Candidate("vision", geographic("zone"), float("inf")),
Candidate("vision", geographic("zone"), 0), Candidate("vision", geographic("zone"), -1), Candidate("vision", geographic("zone"), True),
Candidate("vision", geographic("zone"), 10 ** 400), Candidate("vision", geographic("zone\x00"), 20)]
for candidate in candidates:
supplied = observations(map=20, online=30)
supplied[Source.VISION] = Observation(ObservationKind.VALID, candidate)
with self.subTest(candidate=candidate):
selected = select_limit(supplied, policy())
self.assertEqual(selected.reason, "invalid_input")
self.assertEqual(selected.observation.kind, ObservationKind.UNKNOWN)
self.assertIsNone(selected.selected_source)
self.assertTrue(selected.errors)
def test_structural_input_errors_fail_closed(self):
supplied = observations(vision=20)
invalid_observations: list[Any] = [None, {}, {source.value: value for source, value in supplied.items()},
{**supplied, Source.MAP: Observation(cast(Any, "valid"), Candidate("map", geographic("zone"), 20))},
{**supplied, Source.MAP: Observation(ObservationKind.STALE, Candidate("map", geographic("zone"), 20))},
{**supplied, Source.MAP: Observation(ObservationKind.VALID)}]
invalid_policies: list[Any] = [None, SelectionPolicy(cast(Any, "ordered"), (Source.VISION, Source.MAP), True),
SelectionPolicy(SelectionMode.ORDERED, (Source.ONLINE, Source.MAP), True),
SelectionPolicy(SelectionMode.ORDERED, cast(Any, (Source.VISION,)), True),
SelectionPolicy(SelectionMode.ORDERED, (Source.VISION, Source.MAP), cast(Any, 1))]
for malformed in invalid_observations:
self.assertEqual(select_limit(malformed, policy()).reason, "invalid_input")
for malformed in invalid_policies:
self.assertEqual(select_limit(supplied, malformed).reason, "invalid_input")
def test_rejected_limit_never_becomes_selected_fallback(self):
for mode in (Mode.LONGITUDINAL_ONLY, Mode.COMBINED):
with self.subTest(mode=mode):
authority = Authority(mode, LongitudinalOwner.SYSTEM, mode == Mode.COMBINED, True, False, False)
acceptance_policy = Policy(confirm_lower=True, confirm_higher=False, fallback_previous=True)
source_policy = policy(slots=(Source.DASHBOARD, None), online=False)
state = new_session("selection-regression")
# Values and sequence reproduce the observed accepted65 / reject45 / lost-source case.
for index, (speed, reject) in enumerate(((29.0576, False), (20.1168, False), (20.1168, True), (None, False))):
chosen = select_limit(observations(dashboard=speed) if speed is not None else observations(), source_policy)
action = DriverAction(state.session_id, 1, state.pending.decision_id, ActionKind.REJECT) if reject else None
decision = step(state, chosen.observation, authority, acceptance_policy, now_ns=index * 50_000_000, action=action)
self.assertFalse(decision.errors)
self.assertEqual(decision.control_target_mps, 29.0576)
self.assertEqual(decision.state.accepted.candidate.speed_mps, 29.0576)
self.assertEqual(decision.state.pending is not None, index == 1)
if index > 0:
self.assertIsNone(decision.history_write)
if reject:
self.assertTrue(decision.action_receipt.consumed)
state = decision.state
returned = select_limit(observations(dashboard=20.1168), source_policy)
decision = step(state, returned.observation, authority, acceptance_policy, now_ns=200_000_000)
self.assertEqual(decision.basis, "rejected")
self.assertEqual(decision.control_target_mps, 29.0576)
def test_uncertainty_and_axis_ownership_survive_composition(self):
system = Authority(Mode.LONGITUDINAL_ONLY, LongitudinalOwner.SYSTEM, False, True, False, False)
acceptance_policy = Policy(confirm_higher=False, fallback_previous=True)
source_policy = policy(slots=(Source.MAP, None), online=False)
accepted = step(new_session("uncertainty"), select_limit(observations(map=20), source_policy).observation,
system, acceptance_policy, now_ns=0)
for kind in (ObservationKind.UNKNOWN, ObservationKind.STALE):
supplied = observations()
supplied[Source.MAP] = Observation(kind)
selected = select_limit(supplied, source_policy)
decision = step(accepted.state, selected.observation, system, acceptance_policy, now_ns=1)
self.assertEqual(decision.state.accepted, accepted.state.accepted)
self.assertIsNone(decision.control_target_mps)
for authority in (Authority(Mode.LATERAL_ONLY, LongitudinalOwner.NONE, True, False, False, False),
Authority(Mode.LATERAL_ONLY, LongitudinalOwner.STOCK, True, False, True, False),
Authority(Mode.OFF, LongitudinalOwner.NONE, False, False, False, True)):
selected = select_limit(observations(), source_policy)
decision = step(accepted.state, selected.observation, authority, acceptance_policy, now_ns=1)
self.assertEqual(decision.state.accepted, accepted.state.accepted)
self.assertIsNone(decision.control_target_mps)
def test_selector_preserves_identity_kind_without_inventing_geography(self):
for identity in (geographic("road-1"), ObservationIdentity(IdentityKind.PRODUCER_EPISODE, value="episode-1"),
ObservationIdentity(IdentityKind.SESSION_VALUE, session_id="drive")):
supplied = observations()
candidate = Candidate("dashboard", identity, 20)
supplied[Source.DASHBOARD] = Observation(ObservationKind.VALID, candidate)
selected = select_limit(supplied, policy(slots=(Source.DASHBOARD, None)))
self.assertIs(selected.observation.candidate, candidate)
self.assertIs(selected.observation.candidate.observation_identity, identity)
self.assertEqual(selected.errors, ())
def test_acceptance_checks_session_after_structural_source_selection(self):
supplied = observations()
supplied[Source.DASHBOARD] = Observation(
ObservationKind.VALID, Candidate("dashboard", ObservationIdentity(IdentityKind.SESSION_VALUE, session_id="previous-drive"), 20))
selected = select_limit(supplied, policy(slots=(Source.DASHBOARD, None)))
self.assertEqual(selected.errors, ())
authority = Authority(Mode.LONGITUDINAL_ONLY, LongitudinalOwner.SYSTEM, False, True, False, False)
decision = step(new_session("current-drive"), selected.observation, authority, Policy(False, False), now_ns=0)
self.assertIn("invalid observation", decision.errors)
self.assertIsNone(decision.control_target_mps)
self.assertIsNone(decision.history_write)
def test_dashboard_session_value_rejection_and_adopt_through_selection(self):
authority = Authority(Mode.LONGITUDINAL_ONLY, LongitudinalOwner.SYSTEM, False, True, False, False)
acceptance_policy = Policy(confirm_higher=False, fallback_previous=True)
selection_policy = policy(slots=(Source.DASHBOARD, None), online=False)
state = new_session("drive")
for index, speed in enumerate((29.0576, 20.1168, 20.1168, None, 20.1168)):
supplied = observations()
if speed is not None:
supplied[Source.DASHBOARD] = Observation(
ObservationKind.VALID, Candidate("dashboard", ObservationIdentity(IdentityKind.SESSION_VALUE, session_id="drive"), speed))
selected = select_limit(supplied, selection_policy)
action = DriverAction("drive", 0, state.pending.decision_id, ActionKind.REJECT) if index == 2 else None
decision = step(state, selected.observation, authority, acceptance_policy, now_ns=index, action=action)
self.assertEqual(decision.errors, ())
self.assertEqual(decision.control_target_mps, 29.0576)
state = decision.state
self.assertEqual(decision.basis, "rejected")
shown = state.presentation
request = AdoptRequest("drive", 1, shown.presentation_id, shown.candidate)
adopted = step(state, selected.observation, authority, acceptance_policy, now_ns=5, adopt=request)
self.assertTrue(adopted.adoption_receipt.consumed)
self.assertEqual(adopted.control_target_mps, 20.1168)
self.assertEqual(adopted.state.rejected, frozenset())
self.assertEqual(adopted.adoption.action_sequence_id, request.sequence_id)
self.assertEqual(adopted.history_write.candidate, shown.candidate)
if __name__ == "__main__":
unittest.main()
@@ -0,0 +1 @@
"""Development-qualified camera speed-limit observations."""
@@ -0,0 +1,15 @@
These two ONNX files are copied byte-for-byte from the frozen StarPilot
`678af783` baseline under `starpilot/assets/vision_models/`:
| File | Bytes | SHA-256 |
| --- | ---: | --- |
| `speed_limit_us_detector.onnx` | 9,605,948 | `82408b68c79c269296f0af942130c5383cace4ee06c78e2a4690e8488720116a` |
| `speed_limit_us_value_classifier.onnx` | 6,229,930 | `07c6696e530eb940d2757d5849b4bc0f1d785cda704e5296e18c0a94959f30a5` |
The frozen tree does not identify the training set, original model publisher, or
an asset-specific redistribution license. These facts remain unresolved before
production/device distribution. The current service is development opt-in.
On the local OpenCV 4.13 host, the detector accepts a 256×256 tensor and emits
`(1, 5, 1344)`; the classifier accepts 128×128 and emits `(1, 19)`. Device
inference compatibility has not been established.
File diff suppressed because it is too large Load Diff
@@ -0,0 +1,60 @@
"""Clock-aware projection of one typed vision event into the SLC domain."""
from __future__ import annotations
import math
import time
from openpilot.starpilot.speed_limits import acceptance as acc
from openpilot.starpilot.speed_limits import selection as sel
MAX_RECEIPT_AGE_NS = 1_000_000_000
MAX_FRAME_AGE_NS = 250_000_000
MAX_OBSERVATION_AGE_NS = 1_000_000_000
MAX_PAIR_SKEW_NS = 5_000_000
MODEL_ID = "82408b68c79c269296f0af942130c5383cace4ee06c78e2a4690e8488720116a:07c6696e530eb940d2757d5849b4bc0f1d785cda704e5296e18c0a94959f30a5"
def clock_pair_ns() -> tuple[int, int] | None:
"""Sample MONOTONIC around BOOTTIME; reject an interrupted sample."""
before = time.monotonic_ns()
boot_clock = getattr(time, "CLOCK_BOOTTIME", time.CLOCK_MONOTONIC)
boot = time.clock_gettime_ns(boot_clock)
after = time.monotonic_ns()
return ((before + after) // 2, boot) if 0 <= after - before <= MAX_PAIR_SKEW_NS else None
def vision_observation(event, *, receipt_mono_ns: int, now_mono_ns: int, now_boot_ns: int,
car_fingerprint: str, drive_session: str) -> acc.Observation:
"""An absent/invalid producer is unknown; a stale producer is never absence."""
unknown = acc.Observation(acc.ObservationKind.UNKNOWN)
stale = acc.Observation(acc.ObservationKind.STALE)
if not car_fingerprint or not drive_session or receipt_mono_ns <= 0:
return unknown
if not 0 <= now_mono_ns - receipt_mono_ns <= MAX_RECEIPT_AGE_NS:
return stale
if str(event.modelId) != MODEL_ID or str(event.stream) not in ("road", "wideRoad"):
return unknown
status = str(event.status)
if status == "unavailable" or status == "unknown":
return unknown
if status == "stale":
return stale
if status != "valid":
return unknown
eof_boot_ns = int(event.cameraFrameEofBootTime)
observed_ns = int(event.observedMonoTime)
expiry_ns = int(event.validUntilMonoTime)
if (eof_boot_ns <= 0 or not 0 <= now_boot_ns - eof_boot_ns <= MAX_FRAME_AGE_NS or
observed_ns <= 0 or not 0 <= now_mono_ns - observed_ns <= MAX_OBSERVATION_AGE_NS or
expiry_ns <= observed_ns or now_mono_ns > expiry_ns):
return stale
speed, confidence = float(event.speedMps), float(event.confidence)
if (not math.isfinite(speed) or not 1.0 <= speed <= 55.0 or
not math.isfinite(confidence) or not 0.0 <= confidence <= 1.0 or
int(event.supportCount) <= 0 or int(event.episode) <= 0 or not str(event.producerSessionId)):
return unknown
identity = acc.ObservationIdentity(acc.IdentityKind.PRODUCER_EPISODE,
value=f"{drive_session}:{car_fingerprint}:{event.producerSessionId}:{int(event.episode)}")
return acc.Observation(acc.ObservationKind.VALID,
acc.Candidate(sel.Source.VISION.value, identity, speed))
@@ -0,0 +1,262 @@
"""Opt-in onroad camera observation producer; no Params mailbox or control writes."""
from __future__ import annotations
import math
import os
import time
import uuid
import numpy as np
from openpilot.starpilot.speed_limits.vision.model import VisionModelCore, cv2
from openpilot.starpilot.speed_limits.vision.observation import MODEL_ID, MAX_FRAME_AGE_NS, MAX_PAIR_SKEW_NS, clock_pair_ns
UNAVAILABLE_RETRY_SECONDS = 1.0
INFERENCE_INTERVAL_NS = 166_666_667
BUSY_INFERENCE_INTERVAL_NS = 1_500_000_000
def _configure_process() -> None:
from openpilot.common.hardware import PC
from openpilot.common.realtime import drop_realtime, set_core_affinity
if PC:
return
for configure in (drop_realtime, lambda: set_core_affinity([0, 1, 2])):
try:
configure()
except OSError:
pass
try:
os.nice(max(0, 10 - os.getpriority(os.PRIO_PROCESS, 0)))
except (AttributeError, OSError):
pass
def _driving_model_ready(sm, now_ns: int) -> bool:
service = 'drivingModelData'
if not sm.seen[service] or not sm.valid[service]:
return False
stamp_ns = sm.logMonoTime[service]
drop = float(sm[service].frameDropPerc)
return (stamp_ns > 0 and 0 <= now_ns - stamp_ns <= MAX_FRAME_AGE_NS and
math.isfinite(drop) and 0 <= drop <= 1)
def _camera_buffer(client):
buffer = client.recv(100)
if buffer is None:
return None
eof_boot_ns, frame_id = int(client.timestamp_eof), int(client.frame_id)
stride, width, height = int(client.stride), int(client.width), int(client.height)
uv_offset = int(client.uv_offset)
if (stride < width or width <= 0 or height <= 0 or width % 2 or height % 2 or
uv_offset < stride * height or uv_offset % stride):
return None
return buffer, eof_boot_ns, frame_id, stride, width, height, uv_offset
def _convert_camera_buffer(info) -> tuple[np.ndarray, int, int] | None:
buffer, eof_boot_ns, frame_id, stride, width, height, uv_offset = info
# VisionBuf borrows client-owned shared memory. Copy before another receive
# or reconnect; only the owned array is used for conversion and inference.
raw = np.frombuffer(buffer.data, dtype=np.uint8).copy()
required = uv_offset + stride * (height // 2)
if raw.size < required:
return None
y = raw[:stride * height].reshape((height, stride))[:, :width]
uv = raw[uv_offset:required].reshape((height // 2, stride))[:, :width]
nv12 = np.concatenate((y, uv), axis=0)
frame = cv2.cvtColor(nv12, cv2.COLOR_YUV2BGR_NV12)
return frame, eof_boot_ns, frame_id
def _camera_frame(client) -> tuple[np.ndarray, int, int] | None:
info = _camera_buffer(client)
return _convert_camera_buffer(info) if info is not None else None
def _send(pm, *, session: str, status: str, stream: str, observed_ns: int = 0,
eof_boot_ns: int = 0, frame_id: int = 0, result: tuple[int, float, int, int] | None = None) -> None:
from openpilot.cereal import messaging
msg = messaging.new_message('slcVisionObservation', valid=True)
event = msg.slcVisionObservation.init('vision')
event.producerSessionId = session
event.status = status
event.modelId = MODEL_ID
event.stream = stream
event.observedMonoTime = observed_ns
event.cameraFrameEofBootTime = eof_boot_ns
event.frameId = frame_id
if result is not None:
mph, confidence, support, episode = result
event.speedMps = mph * 0.44704
event.confidence = confidence
event.supportCount = support
event.episode = episode
event.validUntilMonoTime = observed_ns + MAX_FRAME_AGE_NS
pm.send('slcVisionObservation', msg)
def main() -> None:
import openpilot.cereal.messaging as messaging
from openpilot.cereal.visionipc import VisionStreamType
from msgq.visionipc import VisionIpcClient
from openpilot.common.params import Params
from openpilot.common.swaglog import cloudlog
_configure_process()
pm = messaging.PubMaster(['slcVisionObservation'])
sm = messaging.SubMaster(['drivingModelData'])
session = uuid.uuid4().hex
stream = 'unknown'
client = None
core = None
last_unavailable = 0.0
try:
if cv2 is not None:
cv2.setNumThreads(1)
core = VisionModelCore(is_metric=Params().get_bool('IsMetric'))
except Exception as error:
cloudlog.error('vision SLC model unavailable: %s', error)
last_inference_ns = 0
inference_interval_ns = INFERENCE_INTERVAL_NS
last_clock_offset_ns = None
model_paused = False
while True:
if core is None:
now = time.monotonic()
if now - last_unavailable >= UNAVAILABLE_RETRY_SECONDS:
_send(pm, session=session, status='unavailable', stream=stream)
last_unavailable = now
time.sleep(0.1)
continue
try:
sm.update(0)
available = VisionIpcClient.available_streams('camerad', block=False)
wanted = (VisionStreamType.VISION_STREAM_NARROW_ROAD if VisionStreamType.VISION_STREAM_NARROW_ROAD in available else
VisionStreamType.VISION_STREAM_WIDE_ROAD if VisionStreamType.VISION_STREAM_WIDE_ROAD in available else None)
if wanted is None:
if client is not None:
core.reset()
session = uuid.uuid4().hex
client = None
stream = 'unknown'
_send(pm, session=session, status='unavailable', stream='unknown')
time.sleep(UNAVAILABLE_RETRY_SECONDS)
continue
new_stream = 'road' if wanted == VisionStreamType.VISION_STREAM_NARROW_ROAD else 'wideRoad'
if client is None or stream != new_stream:
core.reset()
session = uuid.uuid4().hex
client = VisionIpcClient('camerad', wanted, True)
stream = new_stream
if not client.is_connected():
# Even a successful reconnect of the same client starts a new camera
# session. Earlier sign support cannot confirm a new producer's frame.
core.reset()
session = uuid.uuid4().hex
last_inference_ns = 0
inference_interval_ns = INFERENCE_INTERVAL_NS
last_clock_offset_ns = None
client.connect(False)
if not client.is_connected():
core.reset()
client = None
time.sleep(0.1)
continue
buffer_info = _camera_buffer(client)
if buffer_info is None:
continue
eof_boot_ns, frame_id = buffer_info[1:3]
admission_pair = clock_pair_ns()
if (admission_pair is None or eof_boot_ns <= 0 or
not 0 <= admission_pair[1] - eof_boot_ns <= MAX_FRAME_AGE_NS):
del buffer_info
_send(pm, session=session, status='stale', stream=stream, eof_boot_ns=eof_boot_ns, frame_id=frame_id)
continue
if not _driving_model_ready(sm, admission_pair[0]):
del buffer_info
if not model_paused:
core.reset()
session = uuid.uuid4().hex
_send(pm, session=session, status='unavailable', stream=stream)
last_unavailable = admission_pair[0] / 1e9
elif admission_pair[0] / 1e9 - last_unavailable >= UNAVAILABLE_RETRY_SECONDS:
_send(pm, session=session, status='unavailable', stream=stream)
last_unavailable = admission_pair[0] / 1e9
model_paused = True
time.sleep(0.1)
continue
model_paused = False
admission_offset_ns = admission_pair[1] - admission_pair[0]
offset_stable = (last_clock_offset_ns is None or
abs(admission_offset_ns - last_clock_offset_ns) <= MAX_PAIR_SKEW_NS)
if offset_stable and admission_pair[0] - last_inference_ns < inference_interval_ns:
del buffer_info
continue
try:
frame_info = _convert_camera_buffer(buffer_info)
finally:
del buffer_info
if frame_info is None:
continue
frame, eof_boot_ns, frame_id = frame_info
pair = clock_pair_ns()
if pair is None or eof_boot_ns <= 0 or not 0 <= pair[1] - eof_boot_ns <= MAX_FRAME_AGE_NS:
_send(pm, session=session, status='stale', stream=stream, eof_boot_ns=eof_boot_ns, frame_id=frame_id)
continue
offset_ns = pair[1] - pair[0]
if last_clock_offset_ns is not None and abs(offset_ns - last_clock_offset_ns) > MAX_PAIR_SKEW_NS:
core.reset()
session = uuid.uuid4().hex
last_inference_ns = 0
inference_interval_ns = INFERENCE_INTERVAL_NS
last_clock_offset_ns = offset_ns
if pair[0] - last_inference_ns < inference_interval_ns:
continue
last_inference_ns = pair[0]
try:
result = core.observe(frame, now=pair[0] / 1e9)
except Exception as error:
cloudlog.error('vision SLC inference unavailable: %s', error)
_send(pm, session=session, status='unavailable', stream=stream)
client = None
core = None
continue
sm.update(0)
observed_pair = clock_pair_ns()
if observed_pair is not None:
processing_ns = observed_pair[0] - last_inference_ns
if processing_ns >= 0:
# Original Dom processing-cost cooldown: keep slow CPU inference from
# immediately starting another pass, including after stale rejection.
inference_interval_ns = max(INFERENCE_INTERVAL_NS,
min(BUSY_INFERENCE_INTERVAL_NS, processing_ns * 5 // 2))
if (observed_pair is None or
abs((observed_pair[1] - observed_pair[0]) - offset_ns) > MAX_PAIR_SKEW_NS or
not 0 <= observed_pair[1] - eof_boot_ns <= MAX_FRAME_AGE_NS):
core.reset()
session = uuid.uuid4().hex
last_clock_offset_ns = None
_send(pm, session=session, status='stale', stream=stream, eof_boot_ns=eof_boot_ns, frame_id=frame_id)
continue
if not _driving_model_ready(sm, observed_pair[0]):
core.reset()
session = uuid.uuid4().hex
model_paused = True
last_unavailable = observed_pair[0] / 1e9
_send(pm, session=session, status='unavailable', stream=stream)
continue
_send(pm, session=session, status='valid' if result is not None else 'unknown', stream=stream,
observed_ns=observed_pair[0], eof_boot_ns=eof_boot_ns, frame_id=frame_id, result=result)
except Exception as error:
cloudlog.error('vision SLC camera unavailable: %s', error)
_send(pm, session=session, status='unavailable', stream=stream)
client = None
core.reset()
session = uuid.uuid4().hex
last_clock_offset_ns = None
time.sleep(0.1)
@@ -0,0 +1,323 @@
"""Exercise the producer loop against the current camera API and typed wire."""
from collections import deque
from types import SimpleNamespace
import unittest
from unittest.mock import MagicMock, Mock, patch
import numpy as np
from openpilot.cereal import messaging
from openpilot.cereal.visionipc import VisionStreamType
from openpilot.starpilot.speed_limits.vision import producer
from openpilot.starpilot.speed_limits.vision.model import Detection, VisionModelCore
class EndProducer(BaseException):
pass
class TestVisionProducer(unittest.TestCase):
def setUp(self):
event = messaging.new_message('drivingModelData', valid=True)
self.model = event.drivingModelData
self.model.frameDropPerc = 0
self.sm = MagicMock()
self.sm.seen = {'drivingModelData': True}
self.sm.valid = {'drivingModelData': True}
self.sm.logMonoTime = {'drivingModelData': 0}
self.sm.__getitem__ = Mock(return_value=self.model)
for patcher in (patch.object(messaging, 'SubMaster', return_value=self.sm),
patch.object(producer, '_configure_process')):
patcher.start()
self.addCleanup(patcher.stop)
def clock_pairs(self, clocks, states=None):
pairs = iter(clocks)
model_states = iter(states) if states is not None else None
def next_pair():
pair = next(pairs)
self.sm.logMonoTime['drivingModelData'] = pair[0]
if model_states is not None:
seen, valid, age_ns, drop = next(model_states)
self.sm.seen['drivingModelData'] = seen
self.sm.valid['drivingModelData'] = valid
self.sm.logMonoTime['drivingModelData'] = pair[0] - age_ns
self.model.frameDropPerc = drop
return pair
return next_pair
def run_frame(self, streams, *, completion_age_ns=10_000_000, model_states=None):
core = Mock()
core.observe.return_value = (55, 0.9, 2, 1)
client = Mock()
client.is_connected.return_value = True
camera = Mock(return_value=client)
camera.available_streams.side_effect = [streams, EndProducer()]
messages = []
def send(service, event):
self.assertEqual(service, 'slcVisionObservation')
messages.append(messaging.log_from_bytes(event.to_bytes()).slcVisionObservation.vision)
clocks = [(1_000_000_000, 5_000_000_000), (1_000_000_000, 5_000_000_000),
(1_000_000_000 + completion_age_ns, 5_000_000_000 + completion_age_ns)]
with patch('msgq.visionipc.VisionIpcClient', camera), \
patch('openpilot.common.params.Params', return_value=SimpleNamespace(get_bool=lambda _: False)), \
patch.object(messaging, 'PubMaster', return_value=SimpleNamespace(send=send)), \
patch.object(producer, 'VisionModelCore', return_value=core), \
patch.object(producer, '_camera_buffer', return_value=(object(), 5_000_000_000, 12)), \
patch.object(producer, '_convert_camera_buffer', return_value=(np.zeros((2, 2, 3)), 5_000_000_000, 12)) as convert, \
patch.object(producer, 'clock_pair_ns', side_effect=self.clock_pairs(clocks, model_states)), \
patch.object(producer.time, 'sleep'), \
self.assertRaises(EndProducer):
producer.main()
if model_states is not None and len(model_states) == 1:
convert.assert_not_called()
return camera, core, messages
def test_actual_narrow_camera_api_and_compatible_road_wire_tag(self):
for streams in ({VisionStreamType.VISION_STREAM_NARROW_ROAD},
{VisionStreamType.VISION_STREAM_NARROW_ROAD, VisionStreamType.VISION_STREAM_WIDE_ROAD}):
with self.subTest(streams=streams):
camera, core, messages = self.run_frame(streams)
camera.assert_called_once_with('camerad', VisionStreamType.VISION_STREAM_NARROW_ROAD, True)
core.observe.assert_called_once()
self.assertEqual(len(messages), 1)
self.assertEqual(str(messages[0].status), 'valid')
self.assertEqual(str(messages[0].stream), 'road')
self.assertEqual(messages[0].frameId, 12)
self.assertEqual(messages[0].cameraFrameEofBootTime, 5_000_000_000)
def test_actual_wide_camera_fallback_and_no_camera_unavailability(self):
camera, _, messages = self.run_frame({VisionStreamType.VISION_STREAM_WIDE_ROAD})
camera.assert_called_once_with('camerad', VisionStreamType.VISION_STREAM_WIDE_ROAD, True)
self.assertEqual(str(messages[0].status), 'valid')
self.assertEqual(str(messages[0].stream), 'wideRoad')
camera, core, messages = self.run_frame({VisionStreamType.VISION_STREAM_CABIN})
camera.assert_not_called()
core.observe.assert_not_called()
self.assertEqual(str(messages[0].status), 'unavailable')
self.assertEqual(str(messages[0].stream), 'unknown')
def test_slow_inference_remains_stale_after_camera_api_fix(self):
_, core, messages = self.run_frame({VisionStreamType.VISION_STREAM_NARROW_ROAD},
completion_age_ns=270_000_000)
core.observe.assert_called_once()
self.assertEqual(len(messages), 1)
self.assertEqual(str(messages[0].status), 'stale')
self.assertEqual(messages[0].validUntilMonoTime, 0)
self.assertEqual(messages[0].speedMps, 0)
def test_unhealthy_driving_model_skips_inference_and_does_not_publish_speed(self):
road = {VisionStreamType.VISION_STREAM_NARROW_ROAD}
for state in ((False, True, 0, 0), (True, False, 0, 0),
(True, True, 250_000_001, 0), (True, True, -1, 0),
(True, True, 0, 1.01), (True, True, 0, float('nan'))):
with self.subTest(state=state):
_, core, messages = self.run_frame(road, model_states=[state])
core.observe.assert_not_called()
self.assertEqual([str(message.status) for message in messages], ['unavailable'])
self.assertEqual(messages[0].speedMps, 0)
self.assertEqual(messages[0].validUntilMonoTime, 0)
def test_model_loss_during_inference_cannot_renew_result(self):
healthy = (True, True, 0, 0)
_, core, messages = self.run_frame({VisionStreamType.VISION_STREAM_NARROW_ROAD},
model_states=[healthy, healthy, (True, True, 0, 1.01)])
core.observe.assert_called_once()
self.assertEqual([str(message.status) for message in messages], ['unavailable'])
self.assertEqual(messages[0].validUntilMonoTime, 0)
self.assertEqual(messages[0].speedMps, 0)
def test_borrowed_camera_buffers_do_not_survive_conversion_or_skip(self):
received, released = [], []
sources = iter(((12, 5_000_000_000), (13, 5_050_000_000), (14, 5_000_000_000),
(15, 5_400_000_000), (16, 5_500_000_000), (17, 5_700_000_000)))
class Buffer:
def __init__(self, frame_id):
self.frame_id = frame_id
self.data = bytes(6)
def __del__(self):
released.append(self.frame_id)
client = Mock()
client.is_connected.return_value = True
client.width = client.height = client.stride = 2
client.uv_offset = 4
def receive(timeout):
self.assertEqual(timeout, 100)
self.assertEqual(released, received)
frame_id, stamp = next(sources)
client.frame_id, client.timestamp_eof = frame_id, stamp
received.append(frame_id)
return Buffer(frame_id)
client.recv.side_effect = receive
core = Mock()
def observe(_frame, *, now):
self.assertEqual(released, [12] if core.observe.call_count == 1 else [12, 13, 14, 15, 16, 17])
self.assertEqual(now, 1. if core.observe.call_count == 1 else 1.7)
return None
core.observe.side_effect = observe
road = {VisionStreamType.VISION_STREAM_NARROW_ROAD}
camera = Mock(return_value=client)
camera.available_streams.side_effect = [road] * 6 + [EndProducer()]
clocks = [(1_000_000_000, 5_000_000_000), (1_000_000_000, 5_000_000_000),
(1_010_000_000, 5_010_000_000), (1_050_000_000, 5_050_000_000),
(1_300_000_000, 5_300_000_000), (1_400_000_000, 5_400_000_000),
(1_500_000_000, 5_500_000_000), (1_700_000_000, 5_700_000_000),
(1_700_000_000, 5_700_000_000), (1_710_000_000, 5_710_000_000)]
healthy = (True, True, 0, 0)
states = [healthy] * 5 + [(True, True, 0, 1.1)] * 2 + [healthy] * 3
messages = []
def send(_service, event):
messages.append(messaging.log_from_bytes(event.to_bytes()).slcVisionObservation.vision)
opencv = Mock()
opencv.cvtColor.side_effect = lambda array, _format: array.copy()
with patch('msgq.visionipc.VisionIpcClient', camera), \
patch('openpilot.common.params.Params', return_value=SimpleNamespace(get_bool=lambda _: False)), \
patch.object(messaging, 'PubMaster', return_value=SimpleNamespace(send=send)), \
patch.object(producer, 'VisionModelCore', return_value=core), \
patch.object(producer, 'cv2', opencv), \
patch.object(producer, 'clock_pair_ns', side_effect=self.clock_pairs(clocks, states)), \
patch.object(producer.time, 'sleep'), \
self.assertRaises(EndProducer):
producer.main()
self.assertEqual(released, [12, 13, 14, 15, 16, 17])
self.assertEqual(opencv.cvtColor.call_count, 2)
self.assertEqual(core.observe.call_count, 2)
self.assertEqual([str(message.status) for message in messages], ['unknown', 'stale', 'unavailable', 'unknown'])
self.assertEqual(messages[2].validUntilMonoTime, 0)
self.assertEqual(messages[2].producerSessionId, messages[3].producerSessionId)
def test_throttled_and_already_stale_frames_do_not_convert(self):
core = Mock()
core.observe.return_value = None
client = Mock()
client.is_connected.return_value = True
camera = Mock(return_value=client)
road = {VisionStreamType.VISION_STREAM_NARROW_ROAD}
camera.available_streams.side_effect = [road, road, road, EndProducer()]
buffers = [(object(), 5_000_000_000, 12), (object(), 5_050_000_000, 13),
(object(), 5_000_000_000, 14)]
clocks = [(1_000_000_000, 5_000_000_000), (1_000_000_000, 5_000_000_000),
(1_010_000_000, 5_010_000_000), (1_050_000_000, 5_050_000_000),
(1_300_000_000, 5_300_000_000)]
messages = []
def send(service, event):
self.assertEqual(service, 'slcVisionObservation')
messages.append(messaging.log_from_bytes(event.to_bytes()).slcVisionObservation.vision)
with patch('msgq.visionipc.VisionIpcClient', camera), \
patch('openpilot.common.params.Params', return_value=SimpleNamespace(get_bool=lambda _: False)), \
patch.object(messaging, 'PubMaster', return_value=SimpleNamespace(send=send)), \
patch.object(producer, 'VisionModelCore', return_value=core), \
patch.object(producer, '_camera_buffer', side_effect=buffers), \
patch.object(producer, '_convert_camera_buffer', return_value=(np.zeros((2, 2, 3)), 5_000_000_000, 12)) as convert, \
patch.object(producer, 'clock_pair_ns', side_effect=self.clock_pairs(clocks)), \
patch.object(producer.time, 'sleep'), \
self.assertRaises(EndProducer):
producer.main()
convert.assert_called_once_with(buffers[0])
core.observe.assert_called_once()
self.assertEqual([str(message.status) for message in messages], ['unknown', 'stale'])
self.assertEqual([message.frameId for message in messages], [12, 14])
def test_expired_inference_cools_down_without_accepting_old_result(self):
core = Mock()
core.observe.return_value = None
client = Mock()
client.is_connected.return_value = True
camera = Mock(return_value=client)
road = {VisionStreamType.VISION_STREAM_NARROW_ROAD}
camera.available_streams.side_effect = [road, road, road, EndProducer()]
buffers = [(object(), 5_000_000_000, 12), (object(), 5_300_000_000, 13),
(object(), 5_700_000_000, 14)]
frames = [(np.zeros((2, 2, 3)), item[1], item[2]) for item in (buffers[0], buffers[2])]
clocks = [(1_000_000_000, 5_000_000_000), (1_000_000_000, 5_000_000_000),
(1_270_000_000, 5_270_000_000), (1_300_000_000, 5_300_000_000),
(1_700_000_000, 5_700_000_000), (1_700_000_000, 5_700_000_000),
(1_710_000_000, 5_710_000_000)]
messages = []
def send(service, event):
self.assertEqual(service, 'slcVisionObservation')
messages.append(messaging.log_from_bytes(event.to_bytes()).slcVisionObservation.vision)
with patch('msgq.visionipc.VisionIpcClient', camera), \
patch('openpilot.common.params.Params', return_value=SimpleNamespace(get_bool=lambda _: False)), \
patch.object(messaging, 'PubMaster', return_value=SimpleNamespace(send=send)), \
patch.object(producer, 'VisionModelCore', return_value=core), \
patch.object(producer, '_camera_buffer', side_effect=buffers), \
patch.object(producer, '_convert_camera_buffer', side_effect=frames) as convert, \
patch.object(producer, 'clock_pair_ns', side_effect=self.clock_pairs(clocks)), \
patch.object(producer.time, 'sleep'), \
self.assertRaises(EndProducer):
producer.main()
self.assertEqual([call.args[0] for call in convert.call_args_list], [buffers[0], buffers[2]])
self.assertEqual(core.observe.call_count, 2)
self.assertEqual([str(message.status) for message in messages], ['stale', 'unknown'])
self.assertEqual([message.frameId for message in messages], [12, 14])
self.assertEqual(messages[0].validUntilMonoTime, 0)
self.assertEqual(messages[0].speedMps, 0)
def test_same_client_reconnect_starts_new_consensus_and_session(self):
# Use the real temporal state machine without loading ONNX assets. A sign
# seen once before a reconnect cannot confirm a single post-reconnect read.
core = VisionModelCore.__new__(VisionModelCore)
core.history = deque()
core.reset()
core.is_metric = False
client = Mock()
client.is_connected.side_effect = [True, False, True, True]
camera = Mock(return_value=client)
road = {VisionStreamType.VISION_STREAM_NARROW_ROAD}
camera.available_streams.side_effect = [road, road, road, EndProducer()]
frames = [(np.zeros((2, 2, 3)), 5_000_000_000, 12),
(np.zeros((2, 2, 3)), 5_300_000_000, 1),
(np.zeros((2, 2, 3)), 5_600_000_000, 2)]
clocks = [(1_000_000_000, 5_000_000_000), (1_000_000_000, 5_000_000_000), (1_010_000_000, 5_010_000_000),
(1_300_000_000, 5_300_000_000), (1_300_000_000, 5_300_000_000), (1_310_000_000, 5_310_000_000),
(1_600_000_000, 5_600_000_000), (1_600_000_000, 5_600_000_000), (1_610_000_000, 5_610_000_000)]
messages = []
def send(service, event):
self.assertEqual(service, 'slcVisionObservation')
messages.append(messaging.log_from_bytes(event.to_bytes()).slcVisionObservation.vision)
with patch('msgq.visionipc.VisionIpcClient', camera), \
patch('openpilot.common.params.Params', return_value=SimpleNamespace(get_bool=lambda _: False)), \
patch.object(messaging, 'PubMaster', return_value=SimpleNamespace(send=send)), \
patch.object(producer, 'VisionModelCore', return_value=core), \
patch.object(producer, '_camera_buffer', side_effect=[(object(), item[1], item[2]) for item in frames]), \
patch.object(producer, '_convert_camera_buffer', side_effect=frames), \
patch.object(producer, 'clock_pair_ns', side_effect=self.clock_pairs(clocks)), \
patch.object(producer.time, 'sleep'), \
patch.object(core, 'infer', side_effect=[Detection(55, 0.60)] * 3), \
self.assertRaises(EndProducer):
producer.main()
camera.assert_called_once_with('camerad', VisionStreamType.VISION_STREAM_NARROW_ROAD, True)
client.connect.assert_called_once_with(False)
self.assertEqual([str(message.status) for message in messages], ['unknown', 'unknown', 'valid'])
self.assertEqual([message.frameId for message in messages], [12, 1, 2])
self.assertNotEqual(messages[0].producerSessionId, messages[1].producerSessionId)
self.assertEqual(messages[1].producerSessionId, messages[2].producerSessionId)
self.assertEqual(messages[2].supportCount, 2)
if __name__ == '__main__':
unittest.main()
@@ -0,0 +1,11 @@
"""Availability of the optional camera speed-limit source."""
from collections.abc import Mapping
from openpilot.starpilot.car.hyundai.aol import ioniq6_settings_capable
from opendbc.car.gm.feature_capabilities import display_supported as gm_display_supported
def diagnostic_choice_enabled(environment: Mapping[str, str], cp=None) -> bool:
# A saved source choice does not itself authorize Vision speed control.
return ((environment.get("SLC_REPLAY_RUNTIME") == "1" and environment.get("SLC_VISION_DEVELOPMENT") == "1") or
(cp is not None and (ioniq6_settings_capable(cp) or gm_display_supported(cp))))