diff --git a/mapd_repo/LICENSE b/mapd_repo/LICENSE new file mode 100644 index 0000000000..018aed2c31 --- /dev/null +++ b/mapd_repo/LICENSE @@ -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. diff --git a/mapd_repo/cereal/testdata/mapd_v1_wire.json b/mapd_repo/cereal/testdata/mapd_v1_wire.json new file mode 100644 index 0000000000..734784d139 --- /dev/null +++ b/mapd_repo/cereal/testdata/mapd_v1_wire.json @@ -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==" +} diff --git a/openpilot/starpilot/longitudinal/cruise_ceiling.py b/openpilot/starpilot/longitudinal/cruise_ceiling.py new file mode 100644 index 0000000000..5cc111376a --- /dev/null +++ b/openpilot/starpilot/longitudinal/cruise_ceiling.py @@ -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") diff --git a/openpilot/starpilot/maps/shadow_lifecycle.py b/openpilot/starpilot/maps/shadow_lifecycle.py index 566dd15ee8..c0b0b89940 100644 --- a/openpilot/starpilot/maps/shadow_lifecycle.py +++ b/openpilot/starpilot/maps/shadow_lifecycle.py @@ -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): diff --git a/openpilot/starpilot/speed_limits/ACTIONS.md b/openpilot/starpilot/speed_limits/ACTIONS.md new file mode 100644 index 0000000000..1a200cede8 --- /dev/null +++ b/openpilot/starpilot/speed_limits/ACTIONS.md @@ -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. diff --git a/openpilot/starpilot/speed_limits/COMPOSITION.md b/openpilot/starpilot/speed_limits/COMPOSITION.md new file mode 100644 index 0000000000..8aec63e974 --- /dev/null +++ b/openpilot/starpilot/speed_limits/COMPOSITION.md @@ -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. diff --git a/openpilot/starpilot/speed_limits/LEAD_RELAXATION.md b/openpilot/starpilot/speed_limits/LEAD_RELAXATION.md new file mode 100644 index 0000000000..5621407b0d --- /dev/null +++ b/openpilot/starpilot/speed_limits/LEAD_RELAXATION.md @@ -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. diff --git a/openpilot/starpilot/speed_limits/MAP_SOURCE.md b/openpilot/starpilot/speed_limits/MAP_SOURCE.md new file mode 100644 index 0000000000..95b93ec60e --- /dev/null +++ b/openpilot/starpilot/speed_limits/MAP_SOURCE.md @@ -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. diff --git a/openpilot/starpilot/speed_limits/OVERRIDES.md b/openpilot/starpilot/speed_limits/OVERRIDES.md new file mode 100644 index 0000000000..bb2f92a761 --- /dev/null +++ b/openpilot/starpilot/speed_limits/OVERRIDES.md @@ -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. diff --git a/openpilot/starpilot/speed_limits/README.md b/openpilot/starpilot/speed_limits/README.md new file mode 100644 index 0000000000..3369a1d71a --- /dev/null +++ b/openpilot/starpilot/speed_limits/README.md @@ -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. diff --git a/openpilot/starpilot/speed_limits/SELECTION.md b/openpilot/starpilot/speed_limits/SELECTION.md new file mode 100644 index 0000000000..8a3b2ccb13 --- /dev/null +++ b/openpilot/starpilot/speed_limits/SELECTION.md @@ -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. diff --git a/openpilot/starpilot/speed_limits/SPEED_DOMAIN.md b/openpilot/starpilot/speed_limits/SPEED_DOMAIN.md new file mode 100644 index 0000000000..ad85a2f9d1 --- /dev/null +++ b/openpilot/starpilot/speed_limits/SPEED_DOMAIN.md @@ -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. diff --git a/openpilot/starpilot/speed_limits/__init__.py b/openpilot/starpilot/speed_limits/__init__.py new file mode 100644 index 0000000000..1e2a51a935 --- /dev/null +++ b/openpilot/starpilot/speed_limits/__init__.py @@ -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"] diff --git a/openpilot/starpilot/speed_limits/acceptance.py b/openpilot/starpilot/speed_limits/acceptance.py new file mode 100644 index 0000000000..c158dec932 --- /dev/null +++ b/openpilot/starpilot/speed_limits/acceptance.py @@ -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()) diff --git a/openpilot/starpilot/speed_limits/action_arbitration.py b/openpilot/starpilot/speed_limits/action_arbitration.py new file mode 100644 index 0000000000..c976d5f01d --- /dev/null +++ b/openpilot/starpilot/speed_limits/action_arbitration.py @@ -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) diff --git a/openpilot/starpilot/speed_limits/composition.py b/openpilot/starpilot/speed_limits/composition.py new file mode 100644 index 0000000000..dfe8cb1a26 --- /dev/null +++ b/openpilot/starpilot/speed_limits/composition.py @@ -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") diff --git a/openpilot/starpilot/speed_limits/history.py b/openpilot/starpilot/speed_limits/history.py new file mode 100644 index 0000000000..4189afd709 --- /dev/null +++ b/openpilot/starpilot/speed_limits/history.py @@ -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 diff --git a/openpilot/starpilot/speed_limits/lead_relaxation.py b/openpilot/starpilot/speed_limits/lead_relaxation.py new file mode 100644 index 0000000000..b92ab9adf0 --- /dev/null +++ b/openpilot/starpilot/speed_limits/lead_relaxation.py @@ -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") diff --git a/openpilot/starpilot/speed_limits/map_observation.py b/openpilot/starpilot/speed_limits/map_observation.py new file mode 100644 index 0000000000..9e252cb70c --- /dev/null +++ b/openpilot/starpilot/speed_limits/map_observation.py @@ -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)) diff --git a/openpilot/starpilot/speed_limits/map_source.py b/openpilot/starpilot/speed_limits/map_source.py new file mode 100644 index 0000000000..99a10053b9 --- /dev/null +++ b/openpilot/starpilot/speed_limits/map_source.py @@ -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 diff --git a/openpilot/starpilot/speed_limits/offset_document.py b/openpilot/starpilot/speed_limits/offset_document.py new file mode 100644 index 0000000000..124cbd127f --- /dev/null +++ b/openpilot/starpilot/speed_limits/offset_document.py @@ -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') diff --git a/openpilot/starpilot/speed_limits/overrides.py b/openpilot/starpilot/speed_limits/overrides.py new file mode 100644 index 0000000000..8e2f3d9d92 --- /dev/null +++ b/openpilot/starpilot/speed_limits/overrides.py @@ -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) diff --git a/openpilot/starpilot/speed_limits/physical_actions.py b/openpilot/starpilot/speed_limits/physical_actions.py new file mode 100644 index 0000000000..5871e078d4 --- /dev/null +++ b/openpilot/starpilot/speed_limits/physical_actions.py @@ -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) diff --git a/openpilot/starpilot/speed_limits/runtime.py b/openpilot/starpilot/speed_limits/runtime.py new file mode 100644 index 0000000000..349012e2ca --- /dev/null +++ b/openpilot/starpilot/speed_limits/runtime.py @@ -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 diff --git a/openpilot/starpilot/speed_limits/runtime_settings.py b/openpilot/starpilot/speed_limits/runtime_settings.py new file mode 100644 index 0000000000..2e5449db83 --- /dev/null +++ b/openpilot/starpilot/speed_limits/runtime_settings.py @@ -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) diff --git a/openpilot/starpilot/speed_limits/selection.py b/openpilot/starpilot/speed_limits/selection.py new file mode 100644 index 0000000000..0b325696f9 --- /dev/null +++ b/openpilot/starpilot/speed_limits/selection.py @@ -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 diff --git a/openpilot/starpilot/speed_limits/speed_domain.py b/openpilot/starpilot/speed_limits/speed_domain.py new file mode 100644 index 0000000000..7d29f743a2 --- /dev/null +++ b/openpilot/starpilot/speed_limits/speed_domain.py @@ -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) diff --git a/openpilot/starpilot/speed_limits/tests/__init__.py b/openpilot/starpilot/speed_limits/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/starpilot/speed_limits/tests/test_acceptance.py b/openpilot/starpilot/speed_limits/tests/test_acceptance.py new file mode 100644 index 0000000000..bb4831a1fe --- /dev/null +++ b/openpilot/starpilot/speed_limits/tests/test_acceptance.py @@ -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() diff --git a/openpilot/starpilot/speed_limits/tests/test_action_arbitration.py b/openpilot/starpilot/speed_limits/tests/test_action_arbitration.py new file mode 100644 index 0000000000..b41299b031 --- /dev/null +++ b/openpilot/starpilot/speed_limits/tests/test_action_arbitration.py @@ -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() diff --git a/openpilot/starpilot/speed_limits/tests/test_map_source.py b/openpilot/starpilot/speed_limits/tests/test_map_source.py new file mode 100644 index 0000000000..5e81ba082e --- /dev/null +++ b/openpilot/starpilot/speed_limits/tests/test_map_source.py @@ -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() diff --git a/openpilot/starpilot/speed_limits/tests/test_offset_document.py b/openpilot/starpilot/speed_limits/tests/test_offset_document.py new file mode 100644 index 0000000000..1206591bf6 --- /dev/null +++ b/openpilot/starpilot/speed_limits/tests/test_offset_document.py @@ -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() diff --git a/openpilot/starpilot/speed_limits/tests/test_overrides.py b/openpilot/starpilot/speed_limits/tests/test_overrides.py new file mode 100644 index 0000000000..8c47067ca1 --- /dev/null +++ b/openpilot/starpilot/speed_limits/tests/test_overrides.py @@ -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() diff --git a/openpilot/starpilot/speed_limits/tests/test_runtime_settings.py b/openpilot/starpilot/speed_limits/tests/test_runtime_settings.py new file mode 100644 index 0000000000..9d1bad71fb --- /dev/null +++ b/openpilot/starpilot/speed_limits/tests/test_runtime_settings.py @@ -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() diff --git a/openpilot/starpilot/speed_limits/tests/test_selection.py b/openpilot/starpilot/speed_limits/tests/test_selection.py new file mode 100644 index 0000000000..b409ee5553 --- /dev/null +++ b/openpilot/starpilot/speed_limits/tests/test_selection.py @@ -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() diff --git a/openpilot/starpilot/speed_limits/vision/__init__.py b/openpilot/starpilot/speed_limits/vision/__init__.py new file mode 100644 index 0000000000..561ab6dd43 --- /dev/null +++ b/openpilot/starpilot/speed_limits/vision/__init__.py @@ -0,0 +1 @@ +"""Development-qualified camera speed-limit observations.""" diff --git a/openpilot/starpilot/speed_limits/vision/assets/PROVENANCE.md b/openpilot/starpilot/speed_limits/vision/assets/PROVENANCE.md new file mode 100644 index 0000000000..eac591bdb1 --- /dev/null +++ b/openpilot/starpilot/speed_limits/vision/assets/PROVENANCE.md @@ -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. diff --git a/openpilot/starpilot/speed_limits/vision/assets/speed_limit_us_detector.onnx b/openpilot/starpilot/speed_limits/vision/assets/speed_limit_us_detector.onnx new file mode 100644 index 0000000000..fbfaaaa405 Binary files /dev/null and b/openpilot/starpilot/speed_limits/vision/assets/speed_limit_us_detector.onnx differ diff --git a/openpilot/starpilot/speed_limits/vision/assets/speed_limit_us_value_classifier.onnx b/openpilot/starpilot/speed_limits/vision/assets/speed_limit_us_value_classifier.onnx new file mode 100644 index 0000000000..c3b2eb51ed Binary files /dev/null and b/openpilot/starpilot/speed_limits/vision/assets/speed_limit_us_value_classifier.onnx differ diff --git a/openpilot/starpilot/speed_limits/vision/model.py b/openpilot/starpilot/speed_limits/vision/model.py new file mode 100644 index 0000000000..d70f2c41d8 --- /dev/null +++ b/openpilot/starpilot/speed_limits/vision/model.py @@ -0,0 +1,1024 @@ +"""Frozen active US sign inference, isolated from camera/Params and debug collection.""" +from __future__ import annotations + +from collections import Counter, deque +from dataclasses import dataclass +from pathlib import Path +import hashlib +import math +import time + +import numpy as np + +try: + import cv2 +except ImportError: # Keep inference unavailable when OpenCV is absent. + cv2 = None + +MIN_DETECTION_CONFIDENCE = 0.2 +PUBLISHED_CHANGE_COOLDOWN_SECONDS = 1.4 +PUBLISHED_REVERT_CONFIDENCE = 0.97 + +TEMPORAL_TRACKING_ENABLED = False + +TRACK_MIN_PROPOSAL_CONFIDENCE = 0.10 + +TRACK_UNREADABLE_MIN_PROPOSAL_CONFIDENCE = 0.22 + +DETECTOR_CLASSIFIER_REGION_MODE = "right_roi" # full, right_roi, full_and_right_roi + +STRONG_DETECTION_CONFIDENCE = 0.72 + +HISTORY_SECONDS = 2.0 + +CONSISTENT_DETECTIONS = 2 + +CHANGE_CONSISTENT_DETECTIONS = 2 + +CHANGE_SINGLE_READ_MIN_CONFIDENCE = 0.83 + +CHANGE_REPEAT_MIN_CONFIDENCE = 0.70 + +LOW_SPEED_CHANGE_CONSISTENT_DETECTIONS = 2 + +LOW_SPEED_CHANGE_MIN_CONFIDENCE = 0.90 + +LOW_SPEED_CHANGE_ALLOW_STRONG_CONSENSUS = True + +MODEL_DETECTION_SHORT_CIRCUIT_CONFIDENCE = 0.65 + +MODEL_PROPOSAL_MAX_COUNT = 4 + +MODEL_PROPOSAL_MAX_AREA_RATIO = 0.18 + +MODEL_PROPOSAL_MIN_WIDTH = 10 + +MODEL_PROPOSAL_MIN_HEIGHT = 18 + +MODEL_PROPOSAL_MIN_X_RATIO = 0.35 + +MODEL_PROPOSAL_MAX_Y_RATIO = 0.82 + +ROI_WINDOWS = ( + {"bounds": (0.48, 0.00, 0.98, 0.42), "min_confidence": MIN_DETECTION_CONFIDENCE}, + {"bounds": (0.52, 0.02, 0.97, 0.58), "min_confidence": 0.22}, + {"bounds": (0.62, 0.02, 0.99, 0.68), "min_confidence": 0.18}, + {"bounds": (0.45, 0.00, 1.00, 0.82), "min_confidence": 0.06}, +) + +REGULATORY_WHITE_VALUE_MIN = 135 + +REGULATORY_WHITE_SAT_MAX = 70 + +REGULATORY_DARK_VALUE_MAX = 115 + +REGULATORY_DARK_SAT_MAX = 110 + +REGULATORY_YELLOW_HUE_MIN = 12 + +REGULATORY_YELLOW_HUE_MAX = 45 + +REGULATORY_YELLOW_SAT_MIN = 70 + +REGULATORY_YELLOW_VALUE_MIN = 85 + +REGULATORY_RED_LOW_HUE_MAX = 12 + +REGULATORY_RED_HIGH_HUE_MIN = 168 + +REGULATORY_RED_SAT_MIN = 80 + +REGULATORY_RED_VALUE_MIN = 60 + +REGULATORY_GREEN_HUE_MIN = 45 + +REGULATORY_GREEN_HUE_MAX = 90 + +REGULATORY_BLUE_HUE_MIN = 90 + +REGULATORY_BLUE_HUE_MAX = 135 + +REGULATORY_COLORED_SAT_MIN = 70 + +REGULATORY_COLORED_VALUE_MIN = 70 + +REGULATORY_MIN_WHITE_RATIO = 0.08 + +REGULATORY_MIN_DARK_RATIO = 0.01 + +REGULATORY_MAX_YELLOW_RATIO = 0.12 + +REGULATORY_MAX_RED_RATIO = 0.10 + +REGULATORY_MAX_GREEN_RATIO = 0.35 + +REGULATORY_MAX_BLUE_RATIO = 0.35 + +REGULATORY_MIN_WHITE_COMPONENT_RATIO = 0.012 + +REGULATORY_MIN_COMPONENT_FILL = 0.36 + +REGULATORY_MIN_COMPONENT_HEIGHT_RATIO = 0.2 + +REGULATORY_MIN_COMPONENT_WIDTH_RATIO = 0.12 + +REGULATORY_MIN_ASPECT_RATIO = 0.28 + +REGULATORY_MAX_ASPECT_RATIO = 1.25 + +MIN_PUBLISHABLE_SPEED_LIMIT_MPH = 5 + +MAX_IMPERIAL_PUBLISHABLE_SPEED_LIMIT_MPH = 80 + +US_DETECTOR_CLASSES = { + 0: "regulatory_speed_limit", + 1: "advisory_speed_limit", + 2: "school_zone_speed_limit", +} + +US_CLASSIFIER_SPEED_VALUES = (10, 100, 15, 20, 25, 30, 35, 40, 45, 5, 50, 55, 60, 65, 70, 75, 80, 90) + +EXTENDED_CLASSIFIER_SPEED_VALUES = frozenset((5, 10, 80, 90, 100)) + +SCHOOL_ZONE_SPEED_VALUES = frozenset((15, 20, 25)) + +US_DETECTOR_MIN_CONFIDENCE = 0.06 + +US_CLASSIFIER_MIN_CONFIDENCE = 0.60 + +EXTENDED_CLASSIFIER_MIN_CONFIDENCE = 0.90 + +US_CLASSIFIER_REJECT_MIN_CONFIDENCE = 0.85 + +DETECTOR_CLASSIFIER_EXPANSIONS = ( + (0.00, 0.00, 0.00, 0.00, 1.10), + (0.10, 0.06, 0.10, 0.12, 1.00), + (0.00, 0.00, 0.18, 0.18, 0.55), +) + +SCHOOL_ZONE_DIRECT_EXPANSIONS = ( + (0.00, 0.00, 0.18, 0.18), + (0.00, 0.00, 0.22, 0.18), +) + +SCHOOL_ZONE_READ_VARIANTS = ( + (0.00, 0.00, 1.00, 1.00, 0.80), + (0.00, 0.35, 1.00, 1.00, 0.88), + (0.00, 0.45, 1.00, 1.00, 0.96), + (0.12, 0.38, 0.88, 1.00, 1.00), +) + +DETECTOR_CLASSIFIER_SUPPORT_BONUS = 0.06 + +DETECTOR_CLASSIFIER_REGULATORY_BONUS = 0.05 + +DETECTOR_CLASSIFIER_NON_REGULATORY_PENALTY = 0.03 + +DETECTOR_CLASSIFIER_SMALL_BOX_AREA_RATIO = 0.004 + +DETECTOR_CLASSIFIER_TINY_LOW_CONF_AREA_RATIO = 0.002 + +DETECTOR_CLASSIFIER_TINY_LOW_CONF_MIN_CONFIDENCE = 0.16 + +DETECTOR_CLASSIFIER_MIN_ACCEPT_WIDTH = 28 + +DETECTOR_CLASSIFIER_MIN_ACCEPT_HEIGHT = 40 + +DETECTOR_CLASSIFIER_RESCUE_MIN_WIDTH = 14 + +DETECTOR_CLASSIFIER_RESCUE_MIN_HEIGHT = 18 + +DETECTOR_CLASSIFIER_RESCUE_MIN_X_RATIO = 0.52 + +DETECTOR_CLASSIFIER_RESCUE_MIN_SUPPORT = 2 + +DETECTOR_CLASSIFIER_RESCUE_MIN_CONFIDENCE = 0.90 + +DETECTOR_CLASSIFIER_RESCUE_MAX_SCORE = 0.64 + +DETECTOR_CLASSIFIER_STRONG_RESCUE_MIN_SUPPORT = 3 + +DETECTOR_CLASSIFIER_STRONG_RESCUE_MIN_PROPOSAL_CONFIDENCE = 0.60 + +DETECTOR_CLASSIFIER_STRONG_RESCUE_MIN_READ_CONFIDENCE = 0.995 + +DETECTOR_CLASSIFIER_STRONG_RESCUE_MAX_SCORE = 0.74 + +DETECTOR_CLASSIFIER_TRUSTED_MODEL_MAX_HEIGHT = 55 + +DETECTOR_CLASSIFIER_TRUSTED_MODEL_MAX_AREA_RATIO = 0.002 + +DETECTOR_CLASSIFIER_TRUSTED_MODEL_MIN_PROPOSAL_CONFIDENCE = 0.18 + +DETECTOR_CLASSIFIER_TRUSTED_MODEL_MIN_X_RATIO = 0.52 + +DETECTOR_CLASSIFIER_TRUSTED_MODEL_MIN_READ_CONFIDENCE = 0.65 + +DETECTOR_CLASSIFIER_TRUSTED_MODEL_MIN_SUPPORT = 2 + +DETECTOR_CLASSIFIER_STRONG_MODEL_MIN_PROPOSAL_CONFIDENCE = 0.60 + +DETECTOR_CLASSIFIER_STRONG_MODEL_MIN_READ_CONFIDENCE = 0.995 + +DETECTOR_CLASSIFIER_STRONG_MODEL_CONSENSUS_MIN_READ_CONFIDENCE = 0.95 + +DETECTOR_CLASSIFIER_STRONG_MODEL_CONSENSUS_ENABLED = True + +DETECTOR_CLASSIFIER_STRONG_MODEL_CONSENSUS_MIN_SUPPORT = 2 + +DETECTOR_CLASSIFIER_MODEL_ONLY_CONSENSUS_MIN_CONFIDENCE = 0.90 + +DETECTOR_CLASSIFIER_MODEL_ONLY_CONSENSUS_MIN_SUPPORT = 2 + +SCHOOL_ZONE_SPEED_PRIOR = 0.12 + +SCHOOL_ZONE_SUPPORT_BONUS = 0.08 + +SCHOOL_ZONE_MIN_SUPPORT = 2 + +SCHOOL_ZONE_MIN_CONFIDENCE = 0.70 + +SCHOOL_ZONE_SINGLE_READ_CONFIDENCE = 0.975 + +SCHOOL_ZONE_SHORT_CIRCUIT_CONFIDENCE = 0.78 + +SCHOOL_ZONE_FALLBACK_MIN_CONFIDENCE = 0.35 + +NON_SCHOOL_LOW_SPEED_COMPETING_MIN_CONFIDENCE = 0.95 + + +@dataclass(frozen=True) +class Detection: + speed_limit_mph: int + confidence: float + strong_consensus: bool = False + +@dataclass(frozen=True) +class DetectorProposal: + confidence: float + class_id: int + bbox: tuple[int, int, int, int] + speed_limit_mph: int = 0 + +@dataclass(frozen=True) +class HistoryEntry: + speed_limit_mph: int + confidence: float + created_at: float + strong_consensus: bool = False + +class VisionModelCore: + """The frozen active detector/classifier path, with no camera or Params ownership.""" + + ASSET_DIR = Path(__file__).resolve().parent / "assets" + DETECTOR_SHA256 = "82408b68c79c269296f0af942130c5383cace4ee06c78e2a4690e8488720116a" + CLASSIFIER_SHA256 = "07c6696e530eb940d2757d5849b4bc0f1d785cda704e5296e18c0a94959f30a5" + DETECTOR_SIZE = 256 + CLASSIFIER_SIZE = 128 + MAX_SUPPORT_AGE_SECONDS = 2.0 + + def __init__(self, *, is_metric: bool): + if cv2 is None: + raise RuntimeError("OpenCV DNN is unavailable") + self.is_metric = is_metric + self.net = self._load_net("speed_limit_us_detector.onnx", self.DETECTOR_SHA256) + self.classifier_net = self._load_net("speed_limit_us_value_classifier.onnx", self.CLASSIFIER_SHA256) + self._validate_model_abi() + self.detector_input_size = self.DETECTOR_SIZE + self.classifier_input_size = self.CLASSIFIER_SIZE + self.last_detector_forward_count = 0 + self.last_detector_forward_duration_s = 0.0 + self.last_classifier_forward_count = 0 + self.last_classifier_forward_duration_s = 0.0 + self.temporal_tracking_enabled = False + self.latest_detector_proposal = None + self.history: deque[HistoryEntry] = deque() + self.published_speed_limit_mph = 0 + self.published_confidence = 0.0 + self.previous_published_speed_limit_mph = 0 + self.last_publish_change_at = 0.0 + self.last_published_support_at = 0.0 + self.episode = 0 + self.published_support_count = 0 + + def reset(self) -> None: + """Discard evidence from a disconnected camera or a previous stream.""" + self.history.clear() + self.latest_detector_proposal = None + self.published_speed_limit_mph = 0 + self.published_confidence = 0.0 + self.previous_published_speed_limit_mph = 0 + self.last_publish_change_at = 0.0 + self.last_published_support_at = 0.0 + self.published_support_count = 0 + self.episode = 0 + + @classmethod + def _load_net(cls, name: str, expected_sha256: str): + path = cls.ASSET_DIR / name + with path.open("rb") as stream: + actual = hashlib.file_digest(stream, "sha256").hexdigest() + if actual != expected_sha256: + raise ValueError(f"Vision model identity mismatch: {name}") + net = cv2.dnn.readNetFromONNX(str(path)) + net.setPreferableBackend(cv2.dnn.DNN_BACKEND_OPENCV) + net.setPreferableTarget(cv2.dnn.DNN_TARGET_CPU) + return net + + def _validate_model_abi(self) -> None: + self.net.setInput(np.zeros((1, 3, self.DETECTOR_SIZE, self.DETECTOR_SIZE), dtype=np.float32)) + detector_output = self.net.forward() + self.classifier_net.setInput(np.zeros((1, 3, self.CLASSIFIER_SIZE, self.CLASSIFIER_SIZE), dtype=np.float32)) + classifier_output = self.classifier_net.forward() + if (detector_output.shape != (1, 5, 1344) or classifier_output.shape != (1, 19) or + not np.isfinite(detector_output).all() or not np.isfinite(classifier_output).all()): + raise ValueError("Unsupported Vision model output shape or nonfinite output") + + def infer(self, frame_bgr: np.ndarray) -> Detection | None: + if frame_bgr.ndim != 3 or frame_bgr.shape[2] != 3 or frame_bgr.size == 0: + raise ValueError("Expected a nonempty BGR frame") + return self._publishable_detection(self._detect_sign_from_detector_classifier(frame_bgr)) + + def observe(self, frame_bgr: np.ndarray, *, now: float) -> tuple[int, float, int, int] | None: + """Return a fresh confirmed mph value, confidence, support count and episode.""" + if not math.isfinite(now) or now <= 0: + raise ValueError("Invalid observation time") + detection = self.infer(frame_bgr) + if detection is not None and math.isfinite(detection.confidence) and 0 <= detection.confidence <= 1: + self.history.append(HistoryEntry(detection.speed_limit_mph, detection.confidence, now, detection.strong_consensus)) + self._prune_history(now) + confirmed = self._confirm_detection() + if confirmed is not None: + speed, confidence = confirmed + reverting_early = (speed != self.published_speed_limit_mph and + speed == self.previous_published_speed_limit_mph and + now - self.last_publish_change_at < PUBLISHED_CHANGE_COOLDOWN_SECONDS and + confidence < PUBLISHED_REVERT_CONFIDENCE) + if reverting_early: + return None if now - self.last_published_support_at > self.MAX_SUPPORT_AGE_SECONDS else ( + self.published_speed_limit_mph, self.published_confidence, self.published_support_count, self.episode) + support = sum(entry.speed_limit_mph == speed for entry in self.history) + if speed != self.published_speed_limit_mph: + self.previous_published_speed_limit_mph = self.published_speed_limit_mph + self.published_speed_limit_mph = speed + self.last_publish_change_at = now + self.episode += 1 + self.history.clear() + self.history.append(HistoryEntry(speed, confidence, now, detection.strong_consensus)) + self.published_confidence = confidence + self.published_support_count = support + if detection.speed_limit_mph == speed: + self.last_published_support_at = now + if self.published_speed_limit_mph and now - self.last_published_support_at <= self.MAX_SUPPORT_AGE_SECONDS: + return self.published_speed_limit_mph, self.published_confidence, self.published_support_count, self.episode + return None + + @staticmethod + def _letterbox(image, shape=(640, 640), color=(114, 114, 114)): + image_height, image_width = image.shape[:2] + ratio = min(shape[0] / image_height, shape[1] / image_width) + resized_width = int(round(image_width * ratio)) + resized_height = int(round(image_height * ratio)) + pad_width = (shape[1] - resized_width) / 2 + pad_height = (shape[0] - resized_height) / 2 + + if (image_width, image_height) != (resized_width, resized_height): + image = cv2.resize(image, (resized_width, resized_height), interpolation=cv2.INTER_LINEAR) + + top = int(round(pad_height - 0.1)) + bottom = int(round(pad_height + 0.1)) + left = int(round(pad_width - 0.1)) + right = int(round(pad_width + 0.1)) + image = cv2.copyMakeBorder(image, top, bottom, left, right, cv2.BORDER_CONSTANT, value=color) + return image, ratio, pad_width, pad_height + + + def _remember_detector_proposal(self, confidence, class_id, bbox, speed_limit_mph=0, preferred=False): + min_confidence = TRACK_MIN_PROPOSAL_CONFIDENCE if speed_limit_mph else TRACK_UNREADABLE_MIN_PROPOSAL_CONFIDENCE + if not getattr(self, "temporal_tracking_enabled", TEMPORAL_TRACKING_ENABLED) or class_id == 1 or confidence < min_confidence: + return + proposal = DetectorProposal(float(confidence), int(class_id), bbox, int(speed_limit_mph)) + latest_proposal = getattr(self, "latest_detector_proposal", None) + if preferred or latest_proposal is None or proposal.confidence > latest_proposal.confidence: + self.latest_detector_proposal = proposal + + + def _publishable_detection(self, detection): + if detection is None: + return None + if detection.speed_limit_mph < MIN_PUBLISHABLE_SPEED_LIMIT_MPH: + return None + if not getattr(self, "is_metric", False) and detection.speed_limit_mph > MAX_IMPERIAL_PUBLISHABLE_SPEED_LIMIT_MPH: + return None + return detection + + + def _is_regulatory_speed_sign(self, sign_crop): + if sign_crop.size == 0: + return False + + crop_height, crop_width = sign_crop.shape[:2] + crop_area = crop_height * crop_width + if crop_area <= 0: + return False + + hsv = cv2.cvtColor(sign_crop, cv2.COLOR_BGR2HSV) + hue = hsv[:, :, 0] + saturation = hsv[:, :, 1] + value = hsv[:, :, 2] + + white_mask = ((value >= REGULATORY_WHITE_VALUE_MIN) & (saturation <= REGULATORY_WHITE_SAT_MAX)).astype(np.uint8) + dark_mask = ((value <= REGULATORY_DARK_VALUE_MAX) & (saturation <= REGULATORY_DARK_SAT_MAX)).astype(np.uint8) + yellow_mask = ( + (hue >= REGULATORY_YELLOW_HUE_MIN) & + (hue <= REGULATORY_YELLOW_HUE_MAX) & + (saturation >= REGULATORY_YELLOW_SAT_MIN) & + (value >= REGULATORY_YELLOW_VALUE_MIN) + ).astype(np.uint8) + red_mask = ( + (((hue <= REGULATORY_RED_LOW_HUE_MAX) | (hue >= REGULATORY_RED_HIGH_HUE_MIN))) & + (saturation >= REGULATORY_RED_SAT_MIN) & + (value >= REGULATORY_RED_VALUE_MIN) + ).astype(np.uint8) + green_mask = ( + (hue >= REGULATORY_GREEN_HUE_MIN) & + (hue <= REGULATORY_GREEN_HUE_MAX) & + (saturation >= REGULATORY_COLORED_SAT_MIN) & + (value >= REGULATORY_COLORED_VALUE_MIN) + ).astype(np.uint8) + blue_mask = ( + (hue >= REGULATORY_BLUE_HUE_MIN) & + (hue <= REGULATORY_BLUE_HUE_MAX) & + (saturation >= REGULATORY_COLORED_SAT_MIN) & + (value >= REGULATORY_COLORED_VALUE_MIN) + ).astype(np.uint8) + + white_ratio = float(white_mask.mean()) + dark_ratio = float(dark_mask.mean()) + yellow_ratio = float(yellow_mask.mean()) + red_ratio = float(red_mask.mean()) + green_ratio = float(green_mask.mean()) + blue_ratio = float(blue_mask.mean()) + + if white_ratio < REGULATORY_MIN_WHITE_RATIO or dark_ratio < REGULATORY_MIN_DARK_RATIO: + return False + if yellow_ratio > REGULATORY_MAX_YELLOW_RATIO and yellow_ratio > white_ratio * 0.45: + return False + if red_ratio > REGULATORY_MAX_RED_RATIO and red_ratio > white_ratio * 0.35: + return False + if green_ratio > REGULATORY_MAX_GREEN_RATIO and green_ratio > white_ratio * 0.60: + return False + if blue_ratio > REGULATORY_MAX_BLUE_RATIO and blue_ratio > white_ratio * 0.60: + return False + + white_binary = (white_mask * 255).astype(np.uint8) + contours, _ = cv2.findContours(white_binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + min_component_area = crop_area * REGULATORY_MIN_WHITE_COMPONENT_RATIO + + for contour in contours: + area = cv2.contourArea(contour) + if area < min_component_area: + continue + + x, y, width, height = cv2.boundingRect(contour) + if height < crop_height * REGULATORY_MIN_COMPONENT_HEIGHT_RATIO: + continue + if width < crop_width * REGULATORY_MIN_COMPONENT_WIDTH_RATIO: + continue + + aspect_ratio = width / max(height, 1) + if aspect_ratio < REGULATORY_MIN_ASPECT_RATIO or aspect_ratio > REGULATORY_MAX_ASPECT_RATIO: + continue + + fill_ratio = area / max(width * height, 1) + if fill_ratio < REGULATORY_MIN_COMPONENT_FILL: + continue + + return True + + return False + + + @staticmethod + def _softmax(scores): + scores = scores.astype(np.float32) + scores = scores - np.max(scores) + exp_scores = np.exp(scores) + denominator = np.sum(exp_scores) + if denominator <= 0: + return exp_scores + return exp_scores / denominator + + + @staticmethod + def _normalize_classifier_output(scores): + scores = scores.astype(np.float32) + if scores.size == 0: + return scores + + # Ultralytics classifier ONNX exports may already emit normalized probabilities. + if np.all(scores >= 0.0) and np.all(scores <= 1.0): + total = float(np.sum(scores)) + if 0.99 <= total <= 1.01: + return scores + + return VisionModelCore._softmax(scores) + + + @staticmethod + def _square_resize(image, size=128, color=(114, 114, 114)): + image_height, image_width = image.shape[:2] + ratio = min(size / max(image_height, 1), size / max(image_width, 1)) + resized_width = max(int(round(image_width * ratio)), 1) + resized_height = max(int(round(image_height * ratio)), 1) + image = cv2.resize(image, (resized_width, resized_height), interpolation=cv2.INTER_LINEAR) + + canvas = np.full((size, size, image.shape[2]), color, dtype=image.dtype) + offset_x = (size - resized_width) // 2 + offset_y = (size - resized_height) // 2 + canvas[offset_y:offset_y + resized_height, offset_x:offset_x + resized_width] = image + return canvas + + + @staticmethod + def _crop_by_ratio(image, left_ratio, top_ratio, right_ratio, bottom_ratio): + image_height, image_width = image.shape[:2] + x1 = max(int(image_width * left_ratio), 0) + y1 = max(int(image_height * top_ratio), 0) + x2 = min(int(image_width * right_ratio), image_width) + y2 = min(int(image_height * bottom_ratio), image_height) + if x2 <= x1 or y2 <= y1: + return None + crop = image[y1:y2, x1:x2] + return crop if crop.size > 0 else None + + + def _iter_school_zone_read_crops(self, sign_crop): + yielded = set() + for left_ratio, top_ratio, right_ratio, bottom_ratio, weight in SCHOOL_ZONE_READ_VARIANTS: + crop = self._crop_by_ratio(sign_crop, left_ratio, top_ratio, right_ratio, bottom_ratio) + if crop is None: + continue + key = (crop.shape[1], crop.shape[0], left_ratio, top_ratio, right_ratio, bottom_ratio) + if key in yielded: + continue + yielded.add(key) + yield crop, weight + + + def _collect_detector_classifier_proposals_from_region(self, frame_bgr, origin_x, origin_y, full_frame_width, full_frame_height, min_confidence): + if self.net is None: + return [] + + region_height, region_width = frame_bgr.shape[:2] + detector_shape = (self.detector_input_size, self.detector_input_size) + letterboxed, ratio, pad_width, pad_height = self._letterbox(frame_bgr, shape=detector_shape) + blob = cv2.dnn.blobFromImage(letterboxed, scalefactor=1 / 255.0, size=detector_shape, swapRB=True, crop=False) + self.net.setInput(blob) + + forward_started_at = time.monotonic() + predictions = np.squeeze(self.net.forward()) + self.last_detector_forward_count += 1 + self.last_detector_forward_duration_s += time.monotonic() - forward_started_at + if predictions.ndim != 2: + return [] + if predictions.shape[0] < predictions.shape[1]: + predictions = predictions.T + + candidates = [] + for prediction in predictions: + class_scores = prediction[4:] + class_id = int(np.argmax(class_scores)) + confidence = float(class_scores[class_id]) + if class_id not in US_DETECTOR_CLASSES: + continue + if confidence < min_confidence: + continue + + center_x, center_y, width, height = prediction[:4] + x1 = max(int((center_x - width / 2 - pad_width) / ratio), 0) + y1 = max(int((center_y - height / 2 - pad_height) / ratio), 0) + x2 = min(int((center_x + width / 2 - pad_width) / ratio), region_width) + y2 = min(int((center_y + height / 2 - pad_height) / ratio), region_height) + if x2 <= x1 or y2 <= y1: + continue + + x1 += origin_x + y1 += origin_y + x2 += origin_x + y2 += origin_y + + box_width = x2 - x1 + box_height = y2 - y1 + if box_width < MODEL_PROPOSAL_MIN_WIDTH or box_height < MODEL_PROPOSAL_MIN_HEIGHT: + continue + if box_width * box_height > full_frame_width * full_frame_height * MODEL_PROPOSAL_MAX_AREA_RATIO: + continue + if (x1 + x2) / 2 < full_frame_width * MODEL_PROPOSAL_MIN_X_RATIO: + continue + if y1 > full_frame_height * MODEL_PROPOSAL_MAX_Y_RATIO: + continue + + candidates.append((confidence, class_id, (x1, y1, x2, y2))) + + return candidates + + + def _collect_detector_classifier_proposals(self, frame_bgr): + if self.net is None: + return [] + + frame_height, frame_width = frame_bgr.shape[:2] + candidates = [] + if DETECTOR_CLASSIFIER_REGION_MODE in ("full", "full_and_right_roi"): + candidates.extend(self._collect_detector_classifier_proposals_from_region( + frame_bgr, + 0, + 0, + frame_width, + frame_height, + US_DETECTOR_MIN_CONFIDENCE, + )) + + # A second pass on a focused right-side ROI materially improves small U.S. sign reads. + if DETECTOR_CLASSIFIER_REGION_MODE in ("right_roi", "full_and_right_roi"): + bounds = ROI_WINDOWS[-1]["bounds"] + minimum = ROI_WINDOWS[-1]["min_confidence"] + assert isinstance(bounds, tuple) and isinstance(minimum, (int, float)) + left_ratio, top_ratio, right_ratio, bottom_ratio = bounds + left = int(frame_width * left_ratio) + top = int(frame_height * top_ratio) + right = int(frame_width * right_ratio) + bottom = int(frame_height * bottom_ratio) + roi = frame_bgr[top:bottom, left:right] + if roi.size > 0: + candidates.extend(self._collect_detector_classifier_proposals_from_region( + roi, + left, + top, + frame_width, + frame_height, + max(float(minimum), US_DETECTOR_MIN_CONFIDENCE), + )) + + return sorted(candidates, reverse=True)[:MODEL_PROPOSAL_MAX_COUNT] + + + def _classify_speed_limit_from_model(self, sign_crop): + if self.classifier_net is None or sign_crop.size == 0: + return None + + speed_class_count = len(US_CLASSIFIER_SPEED_VALUES) + + input_size = self.classifier_input_size + padded_crop = self._square_resize(sign_crop, size=input_size) + blob = cv2.dnn.blobFromImage(padded_crop, scalefactor=1 / 255.0, size=(input_size, input_size), swapRB=True, crop=False) + self.classifier_net.setInput(blob) + + forward_started_at = time.monotonic() + scores = np.array(self.classifier_net.forward()).reshape(-1) + self.last_classifier_forward_count += 1 + self.last_classifier_forward_duration_s += time.monotonic() - forward_started_at + has_reject_class = scores.size == speed_class_count + 1 + if scores.size != speed_class_count and not has_reject_class: + return None + + probabilities = self._normalize_classifier_output(scores) + speed_probabilities = probabilities[:speed_class_count] + class_index = int(np.argmax(speed_probabilities)) + confidence = float(speed_probabilities[class_index]) + speed_limit = US_CLASSIFIER_SPEED_VALUES[class_index] + if has_reject_class and float(probabilities[speed_class_count]) >= max(confidence, US_CLASSIFIER_REJECT_MIN_CONFIDENCE): + return None + minimum_confidence = ( + EXTENDED_CLASSIFIER_MIN_CONFIDENCE if speed_limit in EXTENDED_CLASSIFIER_SPEED_VALUES else US_CLASSIFIER_MIN_CONFIDENCE + ) + if confidence < minimum_confidence: + return None + + return speed_limit, confidence + + + def _detect_sign_from_detector_classifier(self, frame_bgr): + frame_height, frame_width = frame_bgr.shape[:2] + best_detection = None + best_score = 0.0 + + for proposal_confidence, class_id, (x1, y1, x2, y2) in self._collect_detector_classifier_proposals(frame_bgr): + if class_id == 1: + continue + + box_width = x2 - x1 + box_height = y2 - y1 + if box_width <= 0 or box_height <= 0: + continue + is_small_box = box_width < DETECTOR_CLASSIFIER_MIN_ACCEPT_WIDTH or box_height < DETECTOR_CLASSIFIER_MIN_ACCEPT_HEIGHT + # Tiny far-away proposals are the main nighttime false-positive source. + # Keep a narrow rescue path for right-side high-confidence reads, + # then let temporal confirmation decide whether they are real. + if is_small_box and ( + box_width < DETECTOR_CLASSIFIER_RESCUE_MIN_WIDTH or + box_height < DETECTOR_CLASSIFIER_RESCUE_MIN_HEIGHT or + x1 < frame_width * DETECTOR_CLASSIFIER_RESCUE_MIN_X_RATIO + ): + continue + + proposal_area_ratio = (box_width * box_height) / max(frame_width * frame_height, 1) + is_tiny_low_conf_box = ( + class_id != 2 and + proposal_area_ratio < DETECTOR_CLASSIFIER_TINY_LOW_CONF_AREA_RATIO and + proposal_confidence < DETECTOR_CLASSIFIER_TINY_LOW_CONF_MIN_CONFIDENCE + ) + self._remember_detector_proposal(proposal_confidence, class_id, (x1, y1, x2, y2)) + + if class_id == 2: + school_scores: dict[int, float] = {} + competing_scores: dict[int, float] = {} + school_best_confidences: dict[int, float] = {} + school_support_counts: dict[int, int] = {} + for expand_left, expand_top, expand_right, expand_bottom in SCHOOL_ZONE_DIRECT_EXPANSIONS: + expanded_x1 = max(int(x1 - box_width * expand_left), 0) + expanded_y1 = max(int(y1 - box_height * expand_top), 0) + expanded_x2 = min(int(x2 + box_width * expand_right), frame_width) + expanded_y2 = min(int(y2 + box_height * expand_bottom), frame_height) + sign_crop = frame_bgr[expanded_y1:expanded_y2, expanded_x1:expanded_x2] + if sign_crop.size == 0: + continue + + for school_crop, crop_weight in self._iter_school_zone_read_crops(sign_crop): + read_result = self._classify_speed_limit_from_model(school_crop) + if read_result is None: + continue + + speed_limit_mph, read_confidence = read_result + if speed_limit_mph not in SCHOOL_ZONE_SPEED_VALUES: + competing_scores[speed_limit_mph] = competing_scores.get(speed_limit_mph, 0.0) + read_confidence * crop_weight + continue + + school_scores[speed_limit_mph] = school_scores.get(speed_limit_mph, 0.0) + read_confidence * crop_weight + school_best_confidences[speed_limit_mph] = max(school_best_confidences.get(speed_limit_mph, 0.0), read_confidence) + school_support_counts[speed_limit_mph] = school_support_counts.get(speed_limit_mph, 0) + 1 + + if school_scores: + speed_limit_mph = max( + school_scores, + key=lambda speed: ( + school_scores[speed] + max(school_support_counts[speed] - 1, 0) * SCHOOL_ZONE_SUPPORT_BONUS, + school_best_confidences[speed], + ), + ) + read_confidence = school_best_confidences[speed_limit_mph] + support_count = school_support_counts[speed_limit_mph] + if school_scores[speed_limit_mph] > max(competing_scores.values(), default=0.0): + if ( + (support_count >= SCHOOL_ZONE_MIN_SUPPORT and read_confidence >= SCHOOL_ZONE_MIN_CONFIDENCE) or + read_confidence >= SCHOOL_ZONE_SINGLE_READ_CONFIDENCE + ): + score = min( + read_confidence * 0.72 + + proposal_confidence * 0.22 + + max(support_count - 1, 0) * SCHOOL_ZONE_SUPPORT_BONUS + + 0.04, + 0.95, + ) + if score >= SCHOOL_ZONE_SHORT_CIRCUIT_CONFIDENCE: + self._remember_detector_proposal( + proposal_confidence, class_id, (x1, y1, x2, y2), speed_limit_mph, preferred=True, + ) + return Detection(speed_limit_mph, score) + + speed_scores: dict[int, float] = {} + speed_best_confidences: dict[int, float] = {} + speed_support_counts: dict[int, int] = {} + speed_regulatory_support: dict[int, int] = {} + speed_trusted_model_support: dict[int, int] = {} + speed_model_only_rescue_support: dict[int, int] = {} + speed_direct_model_support: dict[int, int] = {} + speed_strong_model_support: dict[int, int] = {} + + expansions = getattr(self, "detector_classifier_expansions", DETECTOR_CLASSIFIER_EXPANSIONS) + for expand_left, expand_top, expand_right, expand_bottom, expansion_weight in expansions: + expanded_x1 = max(int(x1 - box_width * expand_left), 0) + expanded_y1 = max(int(y1 - box_height * expand_top), 0) + expanded_x2 = min(int(x2 + box_width * expand_right), frame_width) + expanded_y2 = min(int(y2 + box_height * expand_bottom), frame_height) + sign_crop = frame_bgr[expanded_y1:expanded_y2, expanded_x1:expanded_x2] + if sign_crop.size == 0: + continue + + raw_is_regulatory = self._is_regulatory_speed_sign(sign_crop) + is_regulatory = raw_is_regulatory + if class_id == 2: + is_regulatory = True + + model_read = self._classify_speed_limit_from_model(sign_crop) + trusted_model_read = ( + class_id == 0 and + model_read is not None and + x1 >= frame_width * DETECTOR_CLASSIFIER_TRUSTED_MODEL_MIN_X_RATIO and + box_height <= DETECTOR_CLASSIFIER_TRUSTED_MODEL_MAX_HEIGHT and + proposal_area_ratio <= DETECTOR_CLASSIFIER_TRUSTED_MODEL_MAX_AREA_RATIO and + proposal_confidence >= DETECTOR_CLASSIFIER_TRUSTED_MODEL_MIN_PROPOSAL_CONFIDENCE and + model_read[1] >= DETECTOR_CLASSIFIER_TRUSTED_MODEL_MIN_READ_CONFIDENCE + ) + strong_model_read = ( + class_id == 0 and + model_read is not None and + not is_small_box and + proposal_confidence >= DETECTOR_CLASSIFIER_STRONG_MODEL_MIN_PROPOSAL_CONFIDENCE and + model_read[1] >= DETECTOR_CLASSIFIER_STRONG_MODEL_MIN_READ_CONFIDENCE + ) + strong_model_consensus_read = ( + class_id == 0 and + model_read is not None and + not is_small_box and + proposal_confidence >= DETECTOR_CLASSIFIER_STRONG_MODEL_MIN_PROPOSAL_CONFIDENCE and + model_read[1] >= DETECTOR_CLASSIFIER_STRONG_MODEL_CONSENSUS_MIN_READ_CONFIDENCE + ) + model_only_consensus_read = ( + class_id == 0 and + model_read is not None and + not is_small_box and + model_read[1] >= DETECTOR_CLASSIFIER_MODEL_ONLY_CONSENSUS_MIN_CONFIDENCE and + (is_regulatory or proposal_confidence >= DETECTOR_CLASSIFIER_STRONG_MODEL_MIN_PROPOSAL_CONFIDENCE) + ) + needs_ocr_confirmation = ( + class_id != 2 and + (not is_regulatory or is_tiny_low_conf_box) and + not trusted_model_read and + not strong_model_read + ) + read_result = model_read + if read_result is None: + continue + + if needs_ocr_confirmation and not trusted_model_read and not strong_model_read and not model_only_consensus_read: + continue + + speed_limit_mph, read_confidence = read_result + score_is_regulatory = is_regulatory or trusted_model_read or strong_model_read + if ( + class_id == 2 and + proposal_confidence < SCHOOL_ZONE_FALLBACK_MIN_CONFIDENCE and + not raw_is_regulatory + ): + continue + + score = read_confidence * expansion_weight + if score_is_regulatory: + score += DETECTOR_CLASSIFIER_REGULATORY_BONUS + elif proposal_area_ratio >= DETECTOR_CLASSIFIER_SMALL_BOX_AREA_RATIO: + score -= DETECTOR_CLASSIFIER_NON_REGULATORY_PENALTY + if class_id == 2: + score += SCHOOL_ZONE_SPEED_PRIOR if speed_limit_mph in SCHOOL_ZONE_SPEED_VALUES else -SCHOOL_ZONE_SPEED_PRIOR + + speed_scores[speed_limit_mph] = speed_scores.get(speed_limit_mph, 0.0) + score + speed_best_confidences[speed_limit_mph] = max(speed_best_confidences.get(speed_limit_mph, 0.0), read_confidence) + speed_support_counts[speed_limit_mph] = speed_support_counts.get(speed_limit_mph, 0) + 1 + if is_regulatory or class_id == 2 or strong_model_read: + speed_regulatory_support[speed_limit_mph] = speed_regulatory_support.get(speed_limit_mph, 0) + 1 + if trusted_model_read: + speed_trusted_model_support[speed_limit_mph] = speed_trusted_model_support.get(speed_limit_mph, 0) + 1 + if strong_model_consensus_read: + speed_strong_model_support[speed_limit_mph] = speed_strong_model_support.get(speed_limit_mph, 0) + 1 + if needs_ocr_confirmation and model_only_consensus_read: + speed_model_only_rescue_support[speed_limit_mph] = speed_model_only_rescue_support.get(speed_limit_mph, 0) + 1 + elif model_read is not None: + speed_direct_model_support[speed_limit_mph] = speed_direct_model_support.get(speed_limit_mph, 0) + 1 + + if not speed_scores: + continue + + speed_limit_mph = max( + speed_scores, + key=lambda speed: ( + speed_scores[speed] + max(speed_support_counts[speed] - 1, 0) * DETECTOR_CLASSIFIER_SUPPORT_BONUS, + speed_best_confidences[speed], + ), + ) + if class_id == 2 and speed_limit_mph not in SCHOOL_ZONE_SPEED_VALUES: + continue + model_only_rescue_support = speed_model_only_rescue_support.get(speed_limit_mph, 0) + if ( + speed_direct_model_support.get(speed_limit_mph, 0) < 1 and + 0 < model_only_rescue_support < DETECTOR_CLASSIFIER_MODEL_ONLY_CONSENSUS_MIN_SUPPORT + ): + continue + if ( + speed_regulatory_support.get(speed_limit_mph, 0) < 1 and + 0 < speed_trusted_model_support.get(speed_limit_mph, 0) < DETECTOR_CLASSIFIER_TRUSTED_MODEL_MIN_SUPPORT + ): + continue + if class_id != 2 and speed_limit_mph in SCHOOL_ZONE_SPEED_VALUES: + competing_speed_limit_mph = max( + (speed for speed in speed_scores if speed not in SCHOOL_ZONE_SPEED_VALUES), + key=lambda speed: (speed_best_confidences[speed], speed_scores[speed]), + default=None, + ) + if competing_speed_limit_mph is not None: + read_confidence = speed_best_confidences[speed_limit_mph] + competing_confidence = speed_best_confidences[competing_speed_limit_mph] + if ( + competing_confidence >= NON_SCHOOL_LOW_SPEED_COMPETING_MIN_CONFIDENCE and + competing_confidence >= read_confidence + ): + speed_limit_mph = competing_speed_limit_mph + read_confidence = speed_best_confidences[speed_limit_mph] + support_count = speed_support_counts[speed_limit_mph] + strong_rescue = ( + DETECTOR_CLASSIFIER_STRONG_MODEL_CONSENSUS_ENABLED and + speed_strong_model_support.get(speed_limit_mph, 0) >= DETECTOR_CLASSIFIER_STRONG_MODEL_CONSENSUS_MIN_SUPPORT + ) + score = min( + read_confidence * 0.72 + + proposal_confidence * 0.24 + + max(support_count - 1, 0) * DETECTOR_CLASSIFIER_SUPPORT_BONUS, + 0.95, + ) + selection_score = score + published_score = score + if class_id == 2: + if speed_limit_mph in SCHOOL_ZONE_SPEED_VALUES: + selection_score = min(score + 0.06, 0.95) + published_score = selection_score + else: + selection_score = max(score - 0.06, 0.0) + published_score = selection_score + elif is_small_box: + if ( + speed_regulatory_support.get(speed_limit_mph, 0) < 1 and + speed_trusted_model_support.get(speed_limit_mph, 0) < DETECTOR_CLASSIFIER_TRUSTED_MODEL_MIN_SUPPORT + ): + continue + if support_count < DETECTOR_CLASSIFIER_RESCUE_MIN_SUPPORT: + continue + if read_confidence < DETECTOR_CLASSIFIER_RESCUE_MIN_CONFIDENCE: + continue + strong_rescue = strong_rescue or ( + speed_trusted_model_support.get(speed_limit_mph, 0) >= DETECTOR_CLASSIFIER_STRONG_RESCUE_MIN_SUPPORT and + proposal_confidence >= DETECTOR_CLASSIFIER_STRONG_RESCUE_MIN_PROPOSAL_CONFIDENCE and + read_confidence >= DETECTOR_CLASSIFIER_STRONG_RESCUE_MIN_READ_CONFIDENCE + ) + rescue_max_score = ( + DETECTOR_CLASSIFIER_STRONG_RESCUE_MAX_SCORE if strong_rescue else DETECTOR_CLASSIFIER_RESCUE_MAX_SCORE + ) + published_score = min(score, rescue_max_score) + if speed_trusted_model_support.get(speed_limit_mph, 0) < DETECTOR_CLASSIFIER_TRUSTED_MODEL_MIN_SUPPORT: + selection_score = published_score + if selection_score > best_score: + best_score = selection_score + best_detection = Detection(speed_limit_mph, published_score, strong_rescue) + self._remember_detector_proposal( + proposal_confidence, class_id, (x1, y1, x2, y2), speed_limit_mph, preferred=True, + ) + if best_detection is not None and best_detection.confidence >= MODEL_DETECTION_SHORT_CIRCUIT_CONFIDENCE: + return best_detection + + return best_detection + + + def _prune_history(self, now): + while self.history and now - self.history[0].created_at > HISTORY_SECONDS: + self.history.popleft() + + + def _confirm_detection(self): + if not self.history: + return None + + counts = Counter(entry.speed_limit_mph for entry in self.history) + candidate_speed_limit, candidate_count = counts.most_common(1)[0] + matching_entries = [entry for entry in self.history if entry.speed_limit_mph == candidate_speed_limit] + matching_confidences = sorted((entry.confidence for entry in matching_entries), reverse=True) + best_confidence = max(entry.confidence for entry in matching_entries) + has_strong_consensus = any(entry.strong_consensus for entry in matching_entries) + current_speed_limit = self.published_speed_limit_mph + current_count = counts.get(current_speed_limit, 0) if current_speed_limit > 0 else 0 + + if current_speed_limit > 0 and candidate_speed_limit != current_speed_limit: + required_count = CHANGE_CONSISTENT_DETECTIONS + allow_single_frame_confirmation = ( + has_strong_consensus or best_confidence >= CHANGE_SINGLE_READ_MIN_CONFIDENCE + ) + if current_speed_limit >= 30 and candidate_speed_limit < 30: + required_count = LOW_SPEED_CHANGE_CONSISTENT_DETECTIONS + allow_single_frame_confirmation = ( + best_confidence >= CHANGE_SINGLE_READ_MIN_CONFIDENCE or + (has_strong_consensus and LOW_SPEED_CHANGE_ALLOW_STRONG_CONSENSUS) + ) + if best_confidence < LOW_SPEED_CHANGE_MIN_CONFIDENCE: + return None + if candidate_count < required_count and not allow_single_frame_confirmation: + return None + if ( + not allow_single_frame_confirmation and + matching_confidences[required_count - 1] < CHANGE_REPEAT_MIN_CONFIDENCE + ): + return None + if candidate_count <= current_count: + return None + return candidate_speed_limit, best_confidence + + if has_strong_consensus or best_confidence >= STRONG_DETECTION_CONFIDENCE or candidate_count >= CONSISTENT_DETECTIONS: + return candidate_speed_limit, best_confidence + return None diff --git a/openpilot/starpilot/speed_limits/vision/observation.py b/openpilot/starpilot/speed_limits/vision/observation.py new file mode 100644 index 0000000000..da8c671abc --- /dev/null +++ b/openpilot/starpilot/speed_limits/vision/observation.py @@ -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)) diff --git a/openpilot/starpilot/speed_limits/vision/producer.py b/openpilot/starpilot/speed_limits/vision/producer.py new file mode 100644 index 0000000000..3b3d7e03cd --- /dev/null +++ b/openpilot/starpilot/speed_limits/vision/producer.py @@ -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) diff --git a/openpilot/starpilot/speed_limits/vision/tests/test_producer.py b/openpilot/starpilot/speed_limits/vision/tests/test_producer.py new file mode 100644 index 0000000000..27c0f402f7 --- /dev/null +++ b/openpilot/starpilot/speed_limits/vision/tests/test_producer.py @@ -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() diff --git a/openpilot/starpilot/speed_limits/vision_gate.py b/openpilot/starpilot/speed_limits/vision_gate.py new file mode 100644 index 0000000000..8ca75bcdb6 --- /dev/null +++ b/openpilot/starpilot/speed_limits/vision_gate.py @@ -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))))