mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-14 00:13:42 +08:00
Vision
Implement vision annotation observations, inference and preferences through a dedicated runtime owner.
This commit is contained in:
firestar5683
@@ -0,0 +1,67 @@
|
||||
# Visual adjacent-spot warnings
|
||||
|
||||
This package reconstructs V-ASM's image preparation, warning policy and optional
|
||||
onroad producer. Its observations can select the existing native side-camera
|
||||
PiP view. They never change OEM blindspot signals, lane-change eligibility or
|
||||
vehicle controls, and do not qualify an adjacent lane as clear.
|
||||
|
||||
Version-1 polygon annotations use bounded source-frame pixel coordinates. The
|
||||
decoder rejects malformed, empty, degenerate and self-intersecting contours.
|
||||
There is no automatic adoption of an older annotation. Source pixels are
|
||||
retained for a single float32 projection shared by the crop and mask; a lossy
|
||||
normalized-coordinate round trip can otherwise shift an NV12 crop by two pixels.
|
||||
The crop remains even-aligned and letterboxed to RGB NCHW `1×3×352×352`.
|
||||
|
||||
The CPU inference wrapper accepts an explicit external local model and verifies
|
||||
its exact size and SHA-256 before constructing OpenCV's network from those
|
||||
bytes. The loaded network keeps that identity until explicit reload; inference
|
||||
does not reread the model file. Missing or changed assets, unavailable OpenCV,
|
||||
invalid frames, and nonfinite or incorrectly shaped outputs return unavailable.
|
||||
|
||||
The reference artifact is 6,166,229 bytes with SHA-256
|
||||
`5d20cdbb457ba18db51a537ee2e305bbe442264b1613956068d473e35d15900d`.
|
||||
Its embedded metadata identifies Ultralytics and AGPL-3.0, with classes
|
||||
`0_nocar`, `1_car`, and `2_distant_or_rear`. Weights are not bundled here;
|
||||
artifact distribution and training-data provenance remain unresolved. Class 1
|
||||
drives the visual-warning score. Default smoothing is 0.2 seconds, with 0.94/0.79
|
||||
activation/release thresholds. Camera left maps to displayed right, preserving
|
||||
the reference's orientation. Invalid or reversed timestamps reset warning
|
||||
history, and each side expires after three seconds.
|
||||
|
||||
The manager requires device hardware, onroad state,
|
||||
`STARPILOT_VASM_DEVELOPMENT=1`, an absolute external
|
||||
`STARPILOT_VASM_MODEL_PATH`, and a valid enabled `VASMPreferences` document.
|
||||
That strict version-1 document contains an explicitly configured annotation,
|
||||
`enabled`, `confidence` (0.8–1.0) and `smoothSeconds` (0.01–0.5). Absent or
|
||||
invalid settings leave the producer disabled without replacing saved bytes.
|
||||
|
||||
The producer consumes the existing cabin VisionIPC stream without starting a
|
||||
camera. Fresh device state, valid CAN state and Drive gear are required. NV12
|
||||
planes are copied with their actual stride and UV offset. A frame is admitted
|
||||
only within 500 ms of its BOOTTIME end-of-frame timestamp; paired clocks detect
|
||||
resume. Each side retains its original source and three-second warning expiry.
|
||||
Settings and vehicle state are checked again after inference. Reconnect,
|
||||
disablement, stale authority and resume clear the policy. Model-load failures
|
||||
use a bounded retry interval. OpenCV runs one inference thread; the normal
|
||||
one-second interval and 0.3-second follow-up slow down under CPU load.
|
||||
|
||||
The custom `spotMonitorState` event uses an upstream reserved type ID and Event
|
||||
ordinal. Its historical `desiredCurvature @0` field keeps its original meaning
|
||||
and is never written by this producer. The appended, versioned observation
|
||||
contains model/settings identity, source times and per-side expiry. Old
|
||||
curvature-only logs have observation version zero and cannot become warnings.
|
||||
The service is sparse: consumers must check payload freshness, not rely on a
|
||||
nominal publication frequency.
|
||||
|
||||
The native PiP consumer additionally requires its existing saved enablement
|
||||
and `STARPILOT_PIP_DEV=1`. It uses the full event and validates source identity,
|
||||
sessions, paired clocks and the current saved-settings fingerprint. Missing,
|
||||
invalid or expired evidence clears the optional visual warning. Leaving onroad
|
||||
or losing camera/vehicle state closes the subscription. Neither layout nor OEM
|
||||
blindspot behavior changes.
|
||||
|
||||
Host checks cover geometry, input preparation, output handling, serialized
|
||||
observation lifecycle and PiP selection. Synthetic classifier execution does
|
||||
not establish real-scene accuracy, simultaneous device performance, calibration
|
||||
or full feature parity. Weights remain external and no device activation is
|
||||
implied by these checks.
|
||||
@@ -0,0 +1 @@
|
||||
"""Pure V-ASM annotation, crop and warning decisions."""
|
||||
@@ -0,0 +1,15 @@
|
||||
"""One exact-source, parked-authorized V-ASM preference replacement."""
|
||||
|
||||
from collections.abc import Callable
|
||||
|
||||
from openpilot.starpilot.saved_document import WriteResult, commit_exact
|
||||
from openpilot.starpilot.spot_monitor.preferences import KEY, MAX_BYTES, decode, encode
|
||||
|
||||
|
||||
def commit(params, raw: bytes, expected: bytes | None, authorized: Callable[[], bool]) -> WriteResult:
|
||||
"""Never rewrite a missing/invalid source on read; reset writes canonical Off."""
|
||||
preferences = decode(raw) if type(raw) is bytes else None
|
||||
if preferences is None or encode(preferences) != raw:
|
||||
return WriteResult(False, False)
|
||||
return commit_exact(params, key=KEY, max_bytes=MAX_BYTES, raw=raw, expected=expected,
|
||||
authorized=authorized, temp_prefix=".tmp_vasm_")
|
||||
@@ -0,0 +1,103 @@
|
||||
"""Exact-asset CPU V-ASM inference from an explicit local NV12 frame."""
|
||||
|
||||
import hashlib
|
||||
import os
|
||||
from pathlib import Path
|
||||
import stat
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.starpilot.spot_monitor.policy import (
|
||||
Annotation, MODEL_INPUT_SIZE, class_one_confidence, crop_for_side, polygon_pixels,
|
||||
)
|
||||
|
||||
|
||||
# Frozen tri-class artifact. Embedded metadata names class 1 as 1_car and
|
||||
# states AGPL-3.0/Ultralytics; weights remain external and are not bundled.
|
||||
MODEL_SIZE_BYTES = 6_166_229
|
||||
MODEL_SHA256 = "5d20cdbb457ba18db51a537ee2e305bbe442264b1613956068d473e35d15900d"
|
||||
|
||||
|
||||
def _verified_model_bytes(path: Path) -> bytes:
|
||||
fd = os.open(path, os.O_RDONLY | os.O_NOFOLLOW | os.O_NONBLOCK)
|
||||
with os.fdopen(fd, "rb") as source:
|
||||
info = os.fstat(fd)
|
||||
if not stat.S_ISREG(info.st_mode) or info.st_size != MODEL_SIZE_BYTES:
|
||||
raise ValueError("Unsupported V-ASM model size or file type")
|
||||
raw = source.read(MODEL_SIZE_BYTES + 1)
|
||||
if len(raw) != MODEL_SIZE_BYTES or hashlib.sha256(raw).hexdigest() != MODEL_SHA256:
|
||||
raise ValueError("V-ASM model hash changed")
|
||||
return raw
|
||||
|
||||
|
||||
def prepare_nv12(frame: np.ndarray, *, width: int, height: int, annotation: Annotation, side: str, cv2_module) -> np.ndarray:
|
||||
crop = crop_for_side(annotation, side, width, height)
|
||||
if crop is None or type(frame) is not np.ndarray or frame.dtype != np.uint8 or frame.ndim != 2 or \
|
||||
frame.shape[0] != height * 3 // 2 or frame.shape[1] < width:
|
||||
raise ValueError("Unsupported NV12 frame or unconfigured polygon")
|
||||
x, y, w, h = crop.x, crop.y, crop.width, crop.height
|
||||
nv12 = np.vstack((frame[y:y + h, x:x + w],
|
||||
frame[height + y // 2:height + (y + h) // 2, x:x + w]))
|
||||
rgb = cv2_module.cvtColor(nv12, cv2_module.COLOR_YUV2RGB_NV12)
|
||||
if rgb.shape != (h, w, 3) or rgb.dtype != np.uint8:
|
||||
raise ValueError("Invalid RGB crop")
|
||||
mask = np.zeros((h, w), dtype=np.uint8)
|
||||
points = np.array([(px - x, py - y) for px, py in polygon_pixels(annotation, side, width, height)], dtype=np.int32)
|
||||
cv2_module.fillPoly(mask, [points], 255)
|
||||
rgb = cv2_module.bitwise_and(rgb, rgb, mask=mask)
|
||||
resized = cv2_module.resize(rgb, (crop.model_width, crop.model_height), interpolation=cv2_module.INTER_LINEAR)
|
||||
if resized.shape != (crop.model_height, crop.model_width, 3) or resized.dtype != np.uint8:
|
||||
raise ValueError("Invalid resized crop")
|
||||
square = np.zeros((MODEL_INPUT_SIZE, MODEL_INPUT_SIZE, 3), dtype=np.uint8)
|
||||
square[crop.pad_top:crop.pad_top + crop.model_height, crop.pad_left:crop.pad_left + crop.model_width] = resized
|
||||
return np.expand_dims(np.transpose(square.astype(np.float32) / 255.0, (2, 0, 1)), 0)
|
||||
|
||||
|
||||
class VASMInference:
|
||||
def __init__(self, model_path: Path, *, cv2_module=None):
|
||||
self.model_path = model_path
|
||||
self.cv2 = cv2_module
|
||||
self.net = None
|
||||
self.loaded_sha256: str | None = None
|
||||
self.last_error = ""
|
||||
|
||||
def load(self) -> bool:
|
||||
self.net = None
|
||||
self.loaded_sha256 = None
|
||||
try:
|
||||
raw = _verified_model_bytes(self.model_path)
|
||||
if self.cv2 is None:
|
||||
import cv2 # optional runtime dependency; no model load on import
|
||||
self.cv2 = cv2
|
||||
self.cv2.setNumThreads(1)
|
||||
net = self.cv2.dnn.readNetFromONNX(raw)
|
||||
net.setPreferableBackend(self.cv2.dnn.DNN_BACKEND_OPENCV)
|
||||
net.setPreferableTarget(self.cv2.dnn.DNN_TARGET_CPU)
|
||||
self.net = net
|
||||
self.loaded_sha256 = MODEL_SHA256
|
||||
self.last_error = ""
|
||||
return True
|
||||
except Exception as error: # OpenCV raises its own extension exception type.
|
||||
self.last_error = f"V-ASM model unavailable: {error}"
|
||||
return False
|
||||
|
||||
def infer_nv12(self, frame: np.ndarray, *, width: int, height: int, annotation: Annotation, side: str) -> float | None:
|
||||
if self.net is None:
|
||||
return None
|
||||
try:
|
||||
# The network was constructed from verified bytes, not a pathname.
|
||||
# Later file changes cannot alter this loaded network; reloading
|
||||
# requires a new exact-hash verification in load().
|
||||
blob = prepare_nv12(frame, width=width, height=height, annotation=annotation, side=side, cv2_module=self.cv2)
|
||||
self.net.setInput(blob)
|
||||
output = np.asarray(self.net.forward())
|
||||
if output.shape != (1, 3) or not np.isfinite(output).all():
|
||||
raise ValueError("Unexpected tri-class V-ASM output")
|
||||
scores = output[0].tolist()
|
||||
if any(not 0.0 <= score <= 1.0 for score in scores):
|
||||
raise ValueError("Invalid V-ASM class scores")
|
||||
return class_one_confidence(scores)
|
||||
except Exception as error: # OpenCV raises its own extension exception type.
|
||||
self.net = None
|
||||
self.last_error = f"V-ASM inference unavailable: {error}"
|
||||
return None
|
||||
@@ -0,0 +1,200 @@
|
||||
"""Fail-closed, display-only reader for the versioned V-ASM camera observation."""
|
||||
|
||||
from dataclasses import dataclass
|
||||
import math
|
||||
import re
|
||||
|
||||
from openpilot.starpilot.spot_monitor.inference import MODEL_SHA256
|
||||
|
||||
|
||||
CAMERA_ADMISSION_NS = 500_000_000
|
||||
WARNING_HOLD_NS = 3_000_000_000
|
||||
CLOCK_OFFSET_TOLERANCE_NS = 5_000_000
|
||||
SESSION = re.compile(r"[0-9a-f]{32}\Z")
|
||||
FINGERPRINT = re.compile(r"[0-9a-f]{64}\Z")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraSide:
|
||||
status: str = "unknown"
|
||||
confidence: float = 0.0
|
||||
warning: bool = False
|
||||
frame_id: int = 0
|
||||
eof_boot_ns: int = 0
|
||||
observed_mono_ns: int = 0
|
||||
valid_until_boot_ns: int = 0
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class VisualWarning:
|
||||
display_left: CameraSide = CameraSide()
|
||||
display_right: CameraSide = CameraSide()
|
||||
session_id: str = ""
|
||||
sequence: int = 0
|
||||
observed_mono_ns: int = 0
|
||||
|
||||
|
||||
def _uint(value: object) -> bool:
|
||||
return type(value) is int and 0 <= value <= 0xffffffffffffffff
|
||||
|
||||
|
||||
def _side(value, *, event_frame_id: int, event_eof_boot_ns: int,
|
||||
event_observed_mono_ns: int, event_clock_offset_ns: int,
|
||||
now_boot_ns: int) -> CameraSide:
|
||||
status = str(value.status)
|
||||
if status == "unknown":
|
||||
if value.warning or value.confidence != 0:
|
||||
raise ValueError("Unknown camera side carries warning")
|
||||
return CameraSide()
|
||||
if status not in ("clear", "warning") or bool(value.warning) != (status == "warning"):
|
||||
raise ValueError("Contradictory camera side status")
|
||||
confidence = float(value.confidence)
|
||||
frame_id = int(value.sourceFrameId)
|
||||
eof = int(value.sourceFrameEofBootTime)
|
||||
observed = int(value.sourceObservedMonoTime)
|
||||
expires = int(value.validUntilBootTime)
|
||||
if not math.isfinite(confidence) or not 0 <= confidence <= 1 or \
|
||||
not (0 < eof <= event_eof_boot_ns and 0 < observed <= event_observed_mono_ns) or \
|
||||
frame_id > event_frame_id or expires != eof + WARNING_HOLD_NS:
|
||||
raise ValueError("Invalid camera side source or expiry")
|
||||
# The side stamp is the original camera EOF converted to MONOTONIC at
|
||||
# admission. Later opposite-side publications cannot mint a new side age.
|
||||
if abs((eof - observed) - event_clock_offset_ns) > CLOCK_OFFSET_TOLERANCE_NS:
|
||||
raise ValueError("Camera side clock pair changed")
|
||||
if now_boot_ns > expires:
|
||||
return CameraSide()
|
||||
return CameraSide(status, confidence, bool(value.warning), frame_id, eof, observed, expires)
|
||||
|
||||
|
||||
class ObservationReader:
|
||||
"""One high-water per process session; old camera samples never renew a hold."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self._session = ""
|
||||
self._sequence = 0
|
||||
self._observed_mono_ns = 0
|
||||
self._frame_id = -1
|
||||
self._frame_eof_boot_ns = 0
|
||||
self._sides = (CameraSide(), CameraSide())
|
||||
self._retired: list[str] = []
|
||||
self._clock_offset_ns: int | None = None
|
||||
self._post_resume_floor_ns = 0
|
||||
self._last_now_mono_ns = 0
|
||||
self._last_now_boot_ns = 0
|
||||
self._settings_fingerprint = ""
|
||||
self.current = VisualWarning()
|
||||
|
||||
def _clear(self) -> None:
|
||||
self.current = VisualWarning()
|
||||
self._sides = (CameraSide(), CameraSide())
|
||||
|
||||
def current_at(self, *, now_mono_ns: int, now_boot_ns: int,
|
||||
settings_fingerprint: str) -> VisualWarning:
|
||||
"""Recheck an already read warning without requiring another IPC packet."""
|
||||
if not _uint(now_mono_ns) or not _uint(now_boot_ns) or not \
|
||||
isinstance(settings_fingerprint, str) or FINGERPRINT.fullmatch(settings_fingerprint) is None:
|
||||
self._clear()
|
||||
return self.current
|
||||
if now_mono_ns < self._last_now_mono_ns or now_boot_ns < self._last_now_boot_ns:
|
||||
self._post_resume_floor_ns = max(self._post_resume_floor_ns, self._last_now_mono_ns)
|
||||
self._clear()
|
||||
return self.current
|
||||
self._last_now_mono_ns, self._last_now_boot_ns = now_mono_ns, now_boot_ns
|
||||
offset = now_boot_ns - now_mono_ns
|
||||
if self._clock_offset_ns is not None and abs(offset - self._clock_offset_ns) > CLOCK_OFFSET_TOLERANCE_NS:
|
||||
self._clock_offset_ns = offset
|
||||
self._post_resume_floor_ns = max(self._post_resume_floor_ns, now_mono_ns)
|
||||
self._clear()
|
||||
return self.current
|
||||
self._clock_offset_ns = offset
|
||||
if self.current.session_id and self._settings_fingerprint != settings_fingerprint:
|
||||
self._clear()
|
||||
return self.current
|
||||
if self.current.session_id:
|
||||
left, right = (side if now_boot_ns <= side.valid_until_boot_ns else CameraSide() for side in self._sides)
|
||||
self.current = VisualWarning(left, right, self._session, self._sequence, self._observed_mono_ns)
|
||||
return self.current
|
||||
|
||||
def read(self, event, *, now_mono_ns: int, now_boot_ns: int,
|
||||
settings_fingerprint: str) -> VisualWarning:
|
||||
self.current_at(now_mono_ns=now_mono_ns, now_boot_ns=now_boot_ns,
|
||||
settings_fingerprint=settings_fingerprint)
|
||||
if not _uint(now_mono_ns) or not _uint(now_boot_ns) or \
|
||||
type(settings_fingerprint) is not str or FINGERPRINT.fullmatch(settings_fingerprint) is None:
|
||||
return self.current
|
||||
offset = now_boot_ns - now_mono_ns
|
||||
try:
|
||||
if event.which() != "spotMonitorState":
|
||||
return self.current
|
||||
wire = event.spotMonitorState.observation
|
||||
if int(wire.version) != 1: # Old @136 curvature-only logs have version 0.
|
||||
return self.current
|
||||
session = str(wire.producerSessionId)
|
||||
sequence = int(wire.sequence)
|
||||
observed_mono = int(wire.observedMonoTime)
|
||||
if SESSION.fullmatch(session) is None or observed_mono <= self._post_resume_floor_ns:
|
||||
self._clear()
|
||||
return self.current
|
||||
# Producer reset envelopes use a fresh UUID, sequence zero and invalid
|
||||
# Event. Accept only a current, source-stamped reset, retiring the old
|
||||
# session so delayed old valid packets cannot re-arm a visual warning.
|
||||
if sequence == 0 and not event.valid:
|
||||
if event.logMonoTime != observed_mono or observed_mono > now_mono_ns or \
|
||||
now_mono_ns - observed_mono > CAMERA_ADMISSION_NS:
|
||||
self._clear()
|
||||
return self.current
|
||||
if session != self._session and session not in self._retired and observed_mono > self._observed_mono_ns:
|
||||
if self._session:
|
||||
self._retired.append(self._session)
|
||||
self._retired = self._retired[-16:]
|
||||
self._session, self._sequence, self._observed_mono_ns = session, 0, observed_mono
|
||||
self._frame_id, self._frame_eof_boot_ns = -1, 0
|
||||
self._clear()
|
||||
return self.current
|
||||
if sequence < 1:
|
||||
self._clear()
|
||||
return self.current
|
||||
if session in self._retired or \
|
||||
(session == self._session and sequence <= self._sequence) or \
|
||||
(session != self._session and observed_mono <= self._observed_mono_ns):
|
||||
return self.current # Older packets cannot clear or renew a newer state.
|
||||
if event.logMonoTime != observed_mono or not event.valid:
|
||||
self._clear()
|
||||
return self.current
|
||||
observed_boot = int(wire.observedBootTime)
|
||||
eof = int(wire.sourceFrameEofBootTime)
|
||||
frame_id = int(wire.sourceFrameId)
|
||||
expires = int(wire.validUntilBootTime)
|
||||
if str(wire.modelSha256) != MODEL_SHA256 or str(wire.settingsFingerprint) != settings_fingerprint or \
|
||||
not (0 < observed_mono <= now_mono_ns and now_mono_ns - observed_mono <= CAMERA_ADMISSION_NS) or \
|
||||
not (0 < eof <= observed_boot <= now_boot_ns and now_boot_ns - eof <= CAMERA_ADMISSION_NS) or \
|
||||
expires != eof + CAMERA_ADMISSION_NS or now_boot_ns > expires or \
|
||||
abs((observed_boot - observed_mono) - offset) > CLOCK_OFFSET_TOLERANCE_NS:
|
||||
self._clear()
|
||||
return self.current
|
||||
if session == self._session and (frame_id <= self._frame_id or eof <= self._frame_eof_boot_ns):
|
||||
return self.current
|
||||
left = _side(wire.left, event_frame_id=frame_id, event_eof_boot_ns=eof,
|
||||
event_observed_mono_ns=observed_mono,
|
||||
event_clock_offset_ns=observed_boot - observed_mono, now_boot_ns=now_boot_ns)
|
||||
right = _side(wire.right, event_frame_id=frame_id, event_eof_boot_ns=eof,
|
||||
event_observed_mono_ns=observed_mono,
|
||||
event_clock_offset_ns=observed_boot - observed_mono, now_boot_ns=now_boot_ns)
|
||||
if session == self._session:
|
||||
for before, after in zip(self._sides, (left, right), strict=True):
|
||||
if before.status != "unknown" and after.status != "unknown" and \
|
||||
(after.frame_id < before.frame_id or
|
||||
(after.frame_id == before.frame_id and after != before) or
|
||||
after.eof_boot_ns < before.eof_boot_ns):
|
||||
raise ValueError("Replayed or rewritten camera side")
|
||||
elif self._session:
|
||||
self._retired.append(self._session)
|
||||
self._retired = self._retired[-16:]
|
||||
self._session, self._sequence, self._observed_mono_ns = session, sequence, observed_mono
|
||||
self._frame_id, self._frame_eof_boot_ns = frame_id, eof
|
||||
self._sides = (left, right)
|
||||
self._settings_fingerprint = settings_fingerprint
|
||||
self.current = VisualWarning(left, right, session, sequence, observed_mono)
|
||||
except (AttributeError, OverflowError, TypeError, ValueError):
|
||||
self._clear()
|
||||
return self.current
|
||||
@@ -0,0 +1,259 @@
|
||||
"""Pure camera-side V-ASM geometry and warning policy; no model or camera access."""
|
||||
|
||||
from collections.abc import Sequence
|
||||
from dataclasses import dataclass
|
||||
import json
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
MODEL_INPUT_SIZE = 352
|
||||
STATE_TIMEOUT_SECONDS = 3.0
|
||||
MAX_ANNOTATION_BYTES = 8192
|
||||
MAX_POINTS = 32
|
||||
|
||||
|
||||
def _number(value: object) -> float:
|
||||
if (type(value) is not int and type(value) is not float) or not math.isfinite(value):
|
||||
raise ValueError("Annotation coordinates must be finite numbers")
|
||||
return float(value)
|
||||
|
||||
|
||||
def _unique(pairs: list[tuple[str, object]]) -> dict:
|
||||
result: dict = {}
|
||||
for key, value in pairs:
|
||||
if key in result:
|
||||
raise ValueError("Duplicate annotation field")
|
||||
result[key] = value
|
||||
return result
|
||||
|
||||
|
||||
def _reject_constant(value: str):
|
||||
raise ValueError(f"Nonfinite annotation value: {value}")
|
||||
|
||||
|
||||
def _orientation(a: tuple[float, float], b: tuple[float, float], c: tuple[float, float]) -> float:
|
||||
return (b[0] - a[0]) * (c[1] - a[1]) - (b[1] - a[1]) * (c[0] - a[0])
|
||||
|
||||
|
||||
def _segments_intersect(a: tuple[float, float], b: tuple[float, float],
|
||||
c: tuple[float, float], d: tuple[float, float]) -> bool:
|
||||
ab_c, ab_d = _orientation(a, b, c), _orientation(a, b, d)
|
||||
cd_a, cd_b = _orientation(c, d, a), _orientation(c, d, b)
|
||||
epsilon = 1e-12
|
||||
def on_segment(p: tuple[float, float], q: tuple[float, float], r: tuple[float, float]) -> bool:
|
||||
return min(p[0], r[0]) - epsilon <= q[0] <= max(p[0], r[0]) + epsilon and \
|
||||
min(p[1], r[1]) - epsilon <= q[1] <= max(p[1], r[1]) + epsilon
|
||||
if (abs(ab_c) <= epsilon and on_segment(a, c, b)) or (abs(ab_d) <= epsilon and on_segment(a, d, b)) or \
|
||||
(abs(cd_a) <= epsilon and on_segment(c, a, d)) or (abs(cd_b) <= epsilon and on_segment(c, b, d)):
|
||||
return True
|
||||
return (ab_c > epsilon) != (ab_d > epsilon) and (cd_a > epsilon) != (cd_b > epsilon)
|
||||
|
||||
|
||||
def _polygon(points: object, width: int, height: int) -> tuple[tuple[float, float], ...]:
|
||||
if type(points) is not list or len(points) < 3 or len(points) > MAX_POINTS:
|
||||
raise ValueError("Each configured side needs 3–32 polygon points")
|
||||
pixels: list[tuple[float, float]] = []
|
||||
for point in points:
|
||||
if type(point) is not list or len(point) != 2:
|
||||
raise ValueError("Each polygon point needs x and y")
|
||||
x, y = _number(point[0]), _number(point[1])
|
||||
if not 0 <= x <= width or not 0 <= y <= height:
|
||||
raise ValueError("Polygon point is outside its annotated frame")
|
||||
pixels.append((x, y))
|
||||
normalized = tuple((x / width, y / height) for x, y in pixels)
|
||||
if len(set(normalized)) != len(normalized):
|
||||
raise ValueError("Polygon repeats a vertex")
|
||||
area_twice = sum(a[0] * b[1] - b[0] * a[1]
|
||||
for a, b in zip(normalized, normalized[1:] + normalized[:1], strict=True))
|
||||
if abs(area_twice) <= 1e-9:
|
||||
raise ValueError("Polygon has no area")
|
||||
# Crossed annotations cannot define an unambiguous source mask. Adjacent
|
||||
# edges share a vertex and are intentionally omitted from this check.
|
||||
for i in range(len(normalized)):
|
||||
a, b = normalized[i], normalized[(i + 1) % len(normalized)]
|
||||
for j in range(i + 2, len(normalized)):
|
||||
if i == 0 and j == len(normalized) - 1:
|
||||
continue
|
||||
c, d = normalized[j], normalized[(j + 1) % len(normalized)]
|
||||
if _segments_intersect(a, b, c, d):
|
||||
raise ValueError("Polygon crosses or touches itself")
|
||||
return normalized
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Annotation:
|
||||
width: int
|
||||
height: int
|
||||
camera_left: tuple[tuple[float, float], ...] | None
|
||||
camera_right: tuple[tuple[float, float], ...] | None
|
||||
camera_left_source: tuple[tuple[float, float], ...] | None = None
|
||||
camera_right_source: tuple[tuple[float, float], ...] | None = None
|
||||
|
||||
@property
|
||||
def configured_sides(self) -> tuple[str, ...]:
|
||||
return tuple(side for side in ("left", "right") if getattr(self, f"camera_{side}") is not None)
|
||||
|
||||
|
||||
def decode_annotation(raw: bytes | str) -> Annotation:
|
||||
if type(raw) not in (bytes, str) or len(raw) > MAX_ANNOTATION_BYTES:
|
||||
raise ValueError("Annotation is unavailable or oversized")
|
||||
try:
|
||||
value = json.loads(raw, object_pairs_hook=_unique, parse_constant=_reject_constant)
|
||||
except (UnicodeDecodeError, json.JSONDecodeError, RecursionError) as error:
|
||||
raise ValueError("Invalid annotation JSON") from error
|
||||
if type(value) is not dict or set(value) != {"version", "width", "height", "poly_left", "poly_right"} or \
|
||||
type(value["version"]) is not int or value["version"] != 1:
|
||||
raise ValueError("Unexpected annotation fields")
|
||||
width, height = value["width"], value["height"]
|
||||
if type(width) is not int or type(height) is not int or not (32 <= width <= 8192 and 32 <= height <= 8192) or \
|
||||
width % 2 or height % 2:
|
||||
raise ValueError("Annotation dimensions must be bounded even camera dimensions")
|
||||
polygons = []
|
||||
source_polygons = []
|
||||
for side in ("left", "right"):
|
||||
points = value[f"poly_{side}"]
|
||||
polygons.append(None if points == [] else _polygon(points, width, height))
|
||||
source_polygons.append(None if points == [] else tuple((float(point[0]), float(point[1])) for point in points))
|
||||
if polygons == [None, None]:
|
||||
raise ValueError("At least one camera side must be annotated")
|
||||
return Annotation(width, height, *polygons, *source_polygons)
|
||||
|
||||
|
||||
def encode_annotation(annotation: Annotation) -> bytes:
|
||||
if not annotation.configured_sides:
|
||||
raise ValueError("No camera side configured")
|
||||
value = {"version": 1, "width": annotation.width, "height": annotation.height}
|
||||
for side in ("left", "right"):
|
||||
polygon = getattr(annotation, f"camera_{side}_source")
|
||||
value[f"poly_{side}"] = [] if polygon is None else [list(point) for point in polygon]
|
||||
raw = json.dumps(value, sort_keys=True, separators=(",", ":"), allow_nan=False).encode()
|
||||
# The same decoder validates dimensions, self-intersection, and point bounds.
|
||||
decode_annotation(raw)
|
||||
return raw
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Crop:
|
||||
x: int
|
||||
y: int
|
||||
width: int
|
||||
height: int
|
||||
model_width: int
|
||||
model_height: int
|
||||
pad_left: int
|
||||
pad_top: int
|
||||
|
||||
|
||||
def polygon_pixels(annotation: Annotation, side: str, width: int, height: int) -> tuple[tuple[int, int], ...]:
|
||||
source = getattr(annotation, f"camera_{side}_source")
|
||||
if source is None:
|
||||
return ()
|
||||
# Scale raw float32 points before truncation; normalized-coordinate round trips
|
||||
# can shift the NV12 crop.
|
||||
points = np.array(source, dtype=np.float32)
|
||||
points[:, 0] *= width / float(annotation.width)
|
||||
points[:, 1] *= height / float(annotation.height)
|
||||
return tuple((int(x), int(y)) for x, y in points.astype(np.int32))
|
||||
|
||||
|
||||
def crop_for_side(annotation: Annotation, side: str, frame_width: int, frame_height: int) -> Crop | None:
|
||||
if side not in ("left", "right") or type(frame_width) is not int or type(frame_height) is not int or \
|
||||
frame_width < 4 or frame_height < 4 or frame_width > 8192 or frame_height > 8192 or \
|
||||
frame_width % 2 or frame_height % 2:
|
||||
raise ValueError("Unsupported camera side or NV12 frame")
|
||||
polygon = getattr(annotation, f"camera_{side}")
|
||||
if polygon is None:
|
||||
return None
|
||||
# Bound integer-truncated points, then align the origin and extent to even NV12 pixels.
|
||||
pixels = polygon_pixels(annotation, side, frame_width, frame_height)
|
||||
min_x, min_y = min(x for x, _ in pixels), min(y for _, y in pixels)
|
||||
max_x, max_y = max(x for x, _ in pixels), max(y for _, y in pixels)
|
||||
raw_width, raw_height = max_x - min_x + 1, max_y - min_y + 1
|
||||
x = max(0, min((min_x // 2) * 2, frame_width - 2))
|
||||
y = max(0, min((min_y // 2) * 2, frame_height - 2))
|
||||
width = (max(2, min(((raw_width + 1) // 2) * 2, frame_width - x)) // 2) * 2
|
||||
height = (max(2, min(((raw_height + 1) // 2) * 2, frame_height - y)) // 2) * 2
|
||||
scale = MODEL_INPUT_SIZE / max(width, height)
|
||||
model_width, model_height = round(width * scale), round(height * scale)
|
||||
if model_width < 1 or model_height < 1:
|
||||
return None
|
||||
return Crop(x, y, width, height, model_width, model_height,
|
||||
(MODEL_INPUT_SIZE - model_width) // 2, (MODEL_INPUT_SIZE - model_height) // 2)
|
||||
|
||||
|
||||
def _valid_scores(scores: object) -> bool:
|
||||
return ((type(scores) is list or type(scores) is tuple) and len(scores) in (2, 3) and
|
||||
all((type(score) is int or type(score) is float) and math.isfinite(score) and
|
||||
0 <= score <= 1 for score in scores))
|
||||
|
||||
|
||||
def class_one_confidence(scores: Sequence[float] | None) -> float:
|
||||
# Class 1 is threat in both model variants; malformed output must not substitute class 0.
|
||||
if scores is None or not _valid_scores(scores):
|
||||
return 0.0
|
||||
return float(scores[1])
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class WarningState:
|
||||
display_left: bool
|
||||
display_right: bool
|
||||
display_left_confidence: float
|
||||
display_right_confidence: float
|
||||
|
||||
|
||||
class WarningPolicy:
|
||||
def __init__(self, *, threshold: float = 0.94, smooth_seconds: float = 0.2):
|
||||
if type(threshold) not in (int, float) or not math.isfinite(threshold) or not 0.8 <= threshold <= 1.0 or \
|
||||
type(smooth_seconds) not in (int, float) or not math.isfinite(smooth_seconds) or not 0.01 <= smooth_seconds <= 0.5:
|
||||
raise ValueError("Invalid V-ASM warning settings")
|
||||
self.threshold = float(threshold)
|
||||
self.smooth_seconds = float(smooth_seconds)
|
||||
self.reset()
|
||||
|
||||
def reset(self) -> None:
|
||||
self.score = {"left": 0.0, "right": 0.0}
|
||||
self.active = {"left": False, "right": False}
|
||||
self.confidence = {"left": 0.0, "right": 0.0}
|
||||
self.last_update: dict[str, float | None] = {"left": None, "right": None}
|
||||
self.last_now: float | None = None
|
||||
|
||||
def update(self, side: str, scores: Sequence[float] | None, *, now: float, dt: float) -> WarningState:
|
||||
if side not in ("left", "right"):
|
||||
raise ValueError("Unknown camera side")
|
||||
previous = self.last_update[side]
|
||||
if type(now) not in (int, float) or type(dt) not in (int, float) or not math.isfinite(now) or \
|
||||
not math.isfinite(dt) or now < 0 or dt <= 0 or (previous is not None and now <= previous) or \
|
||||
(self.last_now is not None and now < self.last_now) or not _valid_scores(scores):
|
||||
self.reset()
|
||||
return self.state(now)
|
||||
self.last_now = now
|
||||
if previous is not None and now - previous > STATE_TIMEOUT_SECONDS:
|
||||
self.score[side] = 0.0
|
||||
self.active[side] = False
|
||||
raw = class_one_confidence(scores)
|
||||
alpha = min(1.0, dt / max(self.smooth_seconds, 0.001))
|
||||
self.score[side] = (1.0 - alpha) * self.score[side] + alpha * raw
|
||||
self.confidence[side] = raw
|
||||
if not self.active[side]:
|
||||
self.active[side] = self.score[side] >= self.threshold
|
||||
elif self.score[side] < max(0.0, self.threshold - 0.15):
|
||||
self.active[side] = False
|
||||
self.last_update[side] = now
|
||||
return self.state(now)
|
||||
|
||||
def state(self, now: float) -> WarningState:
|
||||
if type(now) not in (int, float) or not math.isfinite(now) or now < 0 or \
|
||||
(self.last_now is not None and now < self.last_now):
|
||||
self.reset()
|
||||
return WarningState(False, False, 0.0, 0.0)
|
||||
self.last_now = now
|
||||
def fresh(side: str) -> bool:
|
||||
stamp = self.last_update[side]
|
||||
return stamp is not None and 0 <= now - stamp <= STATE_TIMEOUT_SECONDS
|
||||
# Camera-left maps to display-right and vice versa.
|
||||
return WarningState(self.active["right"] and fresh("right"), self.active["left"] and fresh("left"),
|
||||
self.confidence["right"] if fresh("right") else 0.0,
|
||||
self.confidence["left"] if fresh("left") else 0.0)
|
||||
@@ -0,0 +1,101 @@
|
||||
"""Strict default-off saved V-ASM observation settings; no legacy adoption."""
|
||||
|
||||
from dataclasses import dataclass
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
from typing import Any
|
||||
|
||||
from openpilot.starpilot.saved_source import read_saved
|
||||
from openpilot.starpilot.spot_monitor.policy import Annotation, decode_annotation, encode_annotation
|
||||
|
||||
|
||||
KEY = "VASMPreferences"
|
||||
MAX_BYTES = 8192
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Preferences:
|
||||
enabled: bool = False
|
||||
annotation: Annotation | None = None
|
||||
confidence: float = 0.94
|
||||
smooth_seconds: float = 0.2
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SavedPreferences:
|
||||
raw: bytes | None
|
||||
readable: bool
|
||||
valid: bool
|
||||
preferences: Preferences = Preferences()
|
||||
|
||||
@property
|
||||
def fingerprint(self) -> str:
|
||||
return hashlib.sha256(self.raw or b"").hexdigest() if self.valid else ""
|
||||
|
||||
|
||||
def _unique(pairs: list[tuple[str, object]]) -> dict[str, object]:
|
||||
result: dict[str, object] = {}
|
||||
for key, value in pairs:
|
||||
if key in result:
|
||||
raise ValueError("duplicate V-ASM preference")
|
||||
result[key] = value
|
||||
return result
|
||||
|
||||
|
||||
def decode(raw: bytes) -> Preferences | None:
|
||||
if type(raw) is not bytes or len(raw) > MAX_BYTES:
|
||||
return None
|
||||
try:
|
||||
data = json.loads(raw, object_pairs_hook=_unique,
|
||||
parse_constant=lambda _: (_ for _ in ()).throw(ValueError("nonfinite")))
|
||||
if type(data) is not dict or set(data) != {"version", "enabled", "annotation", "confidence", "smoothSeconds"}:
|
||||
return None
|
||||
if type(data["version"]) is not int or data["version"] != 1 or type(data["enabled"]) is not bool:
|
||||
return None
|
||||
confidence, smooth = data["confidence"], data["smoothSeconds"]
|
||||
if type(confidence) not in (int, float) or type(smooth) not in (int, float) or not math.isfinite(confidence) or \
|
||||
not math.isfinite(smooth) or not 0.8 <= confidence <= 1.0 or not 0.01 <= smooth <= 0.5:
|
||||
return None
|
||||
annotation = data["annotation"]
|
||||
if annotation is None:
|
||||
return Preferences(False, None, float(confidence), float(smooth)) if not data["enabled"] else None
|
||||
if type(annotation) is not dict:
|
||||
return None
|
||||
annotation_raw = json.dumps(annotation, separators=(",", ":"), allow_nan=False).encode()
|
||||
return Preferences(data["enabled"], decode_annotation(annotation_raw), float(confidence), float(smooth))
|
||||
except (UnicodeDecodeError, ValueError, TypeError, OverflowError, RecursionError):
|
||||
return None
|
||||
|
||||
|
||||
def encode(preferences: Preferences) -> bytes:
|
||||
if type(preferences) is not Preferences or type(preferences.enabled) is not bool or \
|
||||
(preferences.enabled and preferences.annotation is None) or type(preferences.confidence) not in (int, float) or \
|
||||
type(preferences.smooth_seconds) not in (int, float) or not math.isfinite(preferences.confidence) or \
|
||||
not math.isfinite(preferences.smooth_seconds) or not 0.8 <= preferences.confidence <= 1.0 or \
|
||||
not 0.01 <= preferences.smooth_seconds <= 0.5:
|
||||
raise ValueError("Invalid V-ASM preferences")
|
||||
annotation = json.loads(encode_annotation(preferences.annotation)) if preferences.annotation is not None else None
|
||||
raw = json.dumps({"version": 1, "enabled": preferences.enabled, "annotation": annotation,
|
||||
"confidence": preferences.confidence, "smoothSeconds": preferences.smooth_seconds},
|
||||
sort_keys=True, separators=(",", ":"), allow_nan=False).encode()
|
||||
if len(raw) > MAX_BYTES:
|
||||
raise ValueError("Oversized V-ASM preferences")
|
||||
return raw
|
||||
|
||||
|
||||
def read_preferences(params: Any) -> SavedPreferences:
|
||||
raw, readable = read_saved(params, KEY, MAX_BYTES)
|
||||
if not readable:
|
||||
return SavedPreferences(raw, False, False)
|
||||
if raw is None:
|
||||
return SavedPreferences(None, True, True)
|
||||
preferences = decode(raw)
|
||||
return SavedPreferences(raw, True, preferences is not None, preferences or Preferences())
|
||||
|
||||
|
||||
def enabled(params: Any, *, development: bool) -> bool:
|
||||
if not development:
|
||||
return False
|
||||
saved = read_preferences(params)
|
||||
return saved.readable and saved.valid and saved.preferences.enabled
|
||||
@@ -0,0 +1,391 @@
|
||||
"""Default-off onroad cabin observation; never starts cameras or controls a car."""
|
||||
|
||||
from collections.abc import Callable, Iterable
|
||||
from dataclasses import dataclass
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
import signal
|
||||
import threading
|
||||
import time
|
||||
from typing import Any
|
||||
import uuid
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.starpilot.speed_limits.vision.observation import clock_pair_ns
|
||||
from openpilot.starpilot.spot_monitor.inference import VASMInference
|
||||
from openpilot.starpilot.spot_monitor.policy import WarningPolicy, WarningState
|
||||
from openpilot.starpilot.spot_monitor.preferences import SavedPreferences, read_preferences
|
||||
|
||||
|
||||
TICK_SECONDS = 0.1
|
||||
SETTINGS_REFRESH_NS = 2_000_000_000
|
||||
SOURCE_TTL_NS = 500_000_000
|
||||
DEVICE_TTL_NS = 1_500_000_000 # hardwared publishes at 2 Hz; allow one missed sample.
|
||||
RESUME_SKEW_NS = 5_000_000
|
||||
BASE_INTERVAL_NS = 1_000_000_000
|
||||
FOLLOWUP_INTERVAL_NS = 300_000_000
|
||||
FOLLOWUP_WINDOW_NS = 1_000_000_000
|
||||
MODEL_RETRY_NS = 5_000_000_000
|
||||
MAX_BUFFER_BYTES = 32 * 1024 * 1024
|
||||
|
||||
|
||||
class CpuBudget:
|
||||
"""Frozen V-ASM cadence scaling over the fresh deviceState reported cores."""
|
||||
|
||||
def __init__(self):
|
||||
self.factor = 1.0
|
||||
self.last_mono_ns: int | None = None
|
||||
|
||||
def update(self, usage: Iterable[int | float] | None, *, now_mono_ns: int) -> float:
|
||||
try:
|
||||
values = list(usage) if usage is not None and not isinstance(usage, (str, bytes)) else []
|
||||
except TypeError:
|
||||
values = []
|
||||
if (type(now_mono_ns) is not int or now_mono_ns < 0 or
|
||||
(self.last_mono_ns is not None and now_mono_ns < self.last_mono_ns) or
|
||||
not values or len(values) > 32 or
|
||||
any(type(value) not in (int, float) or not math.isfinite(value) or not 0 <= value <= 100 for value in values)):
|
||||
self.factor = 4.0
|
||||
self.last_mono_ns = now_mono_ns if type(now_mono_ns) is int and now_mono_ns >= 0 else None
|
||||
return self.factor
|
||||
average = sum(values) / len(values)
|
||||
hot = sum(value >= 89.0 for value in values)
|
||||
average_target = 1.0 if average < 74.0 else 1.0 + (average - 74.0) / 8.0
|
||||
hot_target = 1.0 + max(0, hot - 4 + 1) * 0.5
|
||||
target = min(max(average_target, hot_target), 4.0)
|
||||
dt = (now_mono_ns - self.last_mono_ns) / 1e9 if self.last_mono_ns is not None else 0.0
|
||||
alpha = min(1.0 - math.exp(-0.8 * dt), 1.0)
|
||||
self.factor = min(max(target * alpha + self.factor * (1.0 - alpha), 1.0), 4.0)
|
||||
self.last_mono_ns = now_mono_ns
|
||||
return self.factor
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Observation:
|
||||
session: str
|
||||
sequence: int
|
||||
status: str
|
||||
settings_fingerprint: str
|
||||
model_sha256: str
|
||||
observed_mono_ns: int
|
||||
observed_boot_ns: int
|
||||
camera_eof_boot_ns: int
|
||||
frame_id: int
|
||||
side: str
|
||||
display_left: bool
|
||||
display_right: bool
|
||||
display_left_confidence: float
|
||||
display_right_confidence: float
|
||||
left_frame_id: int
|
||||
right_frame_id: int
|
||||
left_eof_boot_ns: int
|
||||
right_eof_boot_ns: int
|
||||
left_source_mono_ns: int
|
||||
right_source_mono_ns: int
|
||||
|
||||
|
||||
def copy_cabin_nv12(client: Any) -> tuple[np.ndarray, int, int] | None:
|
||||
"""Copy the complete visible NV12 planes before releasing the VisionBuf."""
|
||||
buffer = client.recv(0)
|
||||
if buffer is None:
|
||||
return None
|
||||
try:
|
||||
width, height = int(client.width), int(client.height)
|
||||
stride, uv_offset = int(client.stride), int(client.uv_offset)
|
||||
eof_boot_ns, frame_id = int(client.timestamp_eof), int(client.frame_id)
|
||||
if (width < 32 or height < 32 or width > 8192 or height > 8192 or width % 2 or height % 2 or
|
||||
stride < width or stride > 16384 or stride % 2 or uv_offset < stride * height or uv_offset % stride or
|
||||
uv_offset + stride * (height // 2) > MAX_BUFFER_BYTES or eof_boot_ns <= 0 or frame_id < 0):
|
||||
return None
|
||||
raw = np.frombuffer(buffer.data, dtype=np.uint8)
|
||||
required = uv_offset + stride * (height // 2)
|
||||
if raw.size < required or raw.size > MAX_BUFFER_BYTES:
|
||||
return None
|
||||
y = raw[:stride * height].reshape((height, stride))[:, :width]
|
||||
uv = raw[uv_offset:required].reshape((height // 2, stride))[:, :width]
|
||||
return np.concatenate((y, uv), axis=0), eof_boot_ns, frame_id
|
||||
except (AttributeError, TypeError, ValueError, OverflowError):
|
||||
return None
|
||||
finally:
|
||||
del buffer
|
||||
|
||||
|
||||
def publish_observation(pm: Any, observation: Observation) -> None:
|
||||
"""Send the historical slot-10 compatible observation; never curvature."""
|
||||
from openpilot.cereal import messaging
|
||||
|
||||
valid = observation.status == "valid" and observation.sequence > 0
|
||||
event = messaging.new_message("spotMonitorState", valid=valid)
|
||||
event.logMonoTime = observation.observed_mono_ns
|
||||
wire = event.spotMonitorState.init("observation")
|
||||
wire.version = 1
|
||||
wire.producerSessionId = observation.session
|
||||
wire.sequence = observation.sequence
|
||||
wire.modelSha256 = observation.model_sha256
|
||||
wire.settingsFingerprint = observation.settings_fingerprint
|
||||
wire.observedMonoTime = observation.observed_mono_ns
|
||||
wire.observedBootTime = observation.observed_boot_ns
|
||||
wire.sourceFrameId = observation.frame_id
|
||||
wire.sourceFrameEofBootTime = observation.camera_eof_boot_ns
|
||||
wire.validUntilBootTime = observation.camera_eof_boot_ns + SOURCE_TTL_NS if valid else 0
|
||||
# Camera-left appears on display-right; opposite-side frames must not renew its expiry.
|
||||
for display, camera in (("left", "right"), ("right", "left")):
|
||||
side = getattr(wire, display)
|
||||
frame_id = getattr(observation, f"{camera}_frame_id")
|
||||
eof_ns = getattr(observation, f"{camera}_eof_boot_ns")
|
||||
source_mono_ns = getattr(observation, f"{camera}_source_mono_ns")
|
||||
warning = bool(getattr(observation, f"display_{display}")) if valid else False
|
||||
side.status = "warning" if warning else "clear" if valid and eof_ns > 0 and \
|
||||
observation.observed_boot_ns - eof_ns <= 3_000_000_000 else "unknown"
|
||||
side.warning = warning
|
||||
side.confidence = float(getattr(observation, f"display_{display}_confidence")) if side.status != "unknown" else 0.0
|
||||
side.sourceFrameId = frame_id
|
||||
side.sourceFrameEofBootTime = eof_ns
|
||||
side.sourceObservedMonoTime = source_mono_ns
|
||||
side.validUntilBootTime = eof_ns + 3_000_000_000 if eof_ns > 0 else 0
|
||||
pm.send("spotMonitorState", event)
|
||||
|
||||
|
||||
def cabin_client_factory(client_type: Any, stream_type: Any) -> Any:
|
||||
"""Attach only to camerad's existing cabin stream; no camera startup."""
|
||||
return client_type("camerad", stream_type.VISION_STREAM_CABIN, True)
|
||||
|
||||
|
||||
class VASMRuntime:
|
||||
def __init__(self, params: Params, messages: Any, client_factory: Callable[[], Any], emit: Callable[[Observation], None],
|
||||
*, model_path: Path | None = None, mono_clock: Callable[[], int] = time.monotonic_ns,
|
||||
pair_clock: Callable[[], tuple[int, int] | None] = clock_pair_ns,
|
||||
model_factory: Callable[[Path], VASMInference] = VASMInference):
|
||||
self.params, self.messages = params, messages
|
||||
self.client_factory, self.emit = client_factory, emit
|
||||
self.model_path = model_path
|
||||
self.mono_clock, self.pair_clock, self.model_factory = mono_clock, pair_clock, model_factory
|
||||
self.session = uuid.uuid4().hex
|
||||
self.sequence = 0
|
||||
self.saved = SavedPreferences(None, True, True)
|
||||
self.settings_read_ns = -SETTINGS_REFRESH_NS
|
||||
self.model: VASMInference | None = None
|
||||
self.last_model_attempt_ns = -MODEL_RETRY_NS
|
||||
self.policy = WarningPolicy()
|
||||
self.cpu_budget = CpuBudget()
|
||||
self.client = None
|
||||
self.last_pair_offset_ns: int | None = None
|
||||
self.last_frame_id = -1
|
||||
self.last_eof_boot_ns = 0
|
||||
self.camera_after_boot_ns = 0
|
||||
self.last_processed_ns = 0
|
||||
self.last_side_mono_ns = {"left": 0, "right": 0}
|
||||
self.side_frame_id = {"left": 0, "right": 0}
|
||||
self.side_eof_boot_ns = {"left": 0, "right": 0}
|
||||
self.next_side = "left"
|
||||
self.followup_until_ns = 0
|
||||
self.status = "disabled"
|
||||
self.last_error_log_ns = -10_000_000_000
|
||||
|
||||
def _reset(self, status: str, now_ns: int) -> None:
|
||||
changed = status != self.status or self.last_frame_id >= 0
|
||||
if changed and (self.last_frame_id >= 0 or self.client is not None):
|
||||
pair = self.pair_clock()
|
||||
if pair is not None:
|
||||
self.camera_after_boot_ns = max(self.camera_after_boot_ns, pair[1])
|
||||
self.policy.reset()
|
||||
self.client = None
|
||||
self.session = uuid.uuid4().hex if changed else self.session
|
||||
if changed:
|
||||
self.sequence = 0
|
||||
self.last_pair_offset_ns = None
|
||||
self.last_frame_id = -1
|
||||
self.last_eof_boot_ns = 0
|
||||
self.last_processed_ns = 0
|
||||
self.last_side_mono_ns = {"left": 0, "right": 0}
|
||||
self.side_frame_id = {"left": 0, "right": 0}
|
||||
self.side_eof_boot_ns = {"left": 0, "right": 0}
|
||||
self.followup_until_ns = 0
|
||||
self.status = status
|
||||
if changed:
|
||||
self.emit(self._observation(status, now_ns, 0, 0, 0, ""))
|
||||
|
||||
def _observation(self, status: str, now_ns: int, boot_ns: int, eof_boot_ns: int, frame_id: int, side: str,
|
||||
state: WarningState | None = None) -> Observation:
|
||||
return Observation(self.session, self.sequence, status, self.saved.fingerprint,
|
||||
self.model.loaded_sha256 if self.model is not None and
|
||||
self.model.loaded_sha256 is not None else "", now_ns, boot_ns, eof_boot_ns, frame_id, side,
|
||||
bool(state.display_left) if state else False, bool(state.display_right) if state else False,
|
||||
float(state.display_left_confidence) if state else 0.0,
|
||||
float(state.display_right_confidence) if state else 0.0,
|
||||
self.side_frame_id["left"], self.side_frame_id["right"],
|
||||
self.side_eof_boot_ns["left"], self.side_eof_boot_ns["right"],
|
||||
self.last_side_mono_ns["left"], self.last_side_mono_ns["right"])
|
||||
|
||||
def _fresh_authority(self, now_ns: int) -> bool:
|
||||
from opendbc.car.structs import car
|
||||
|
||||
sm = self.messages
|
||||
for name in ("deviceState", "carState"):
|
||||
ttl_ns = DEVICE_TTL_NS if name == "deviceState" else SOURCE_TTL_NS
|
||||
stamp = int(sm.logMonoTime[name])
|
||||
receipt = int(float(sm.recv_time[name]) * 1e9)
|
||||
if (not sm.seen[name] or not sm.alive[name] or not sm.valid[name] or
|
||||
not 0 < stamp <= now_ns or not 0 < receipt <= now_ns or
|
||||
now_ns - stamp > ttl_ns or now_ns - receipt > ttl_ns):
|
||||
return False
|
||||
return (bool(sm["deviceState"].started) and bool(sm["carState"].canValid) and
|
||||
sm["carState"].gearShifter == car.CarState.GearShifter.drive)
|
||||
|
||||
def _settings_current(self) -> bool:
|
||||
current = read_preferences(self.params)
|
||||
return (current.readable and current.valid and current.raw is not None and
|
||||
current.raw == self.saved.raw and current.preferences.enabled)
|
||||
|
||||
def _refresh_settings(self, now_ns: int) -> None:
|
||||
if now_ns - self.settings_read_ns < SETTINGS_REFRESH_NS:
|
||||
return
|
||||
self.settings_read_ns = now_ns
|
||||
saved = read_preferences(self.params)
|
||||
if saved != self.saved:
|
||||
self.saved = saved
|
||||
self.model = None
|
||||
self.last_model_attempt_ns = -MODEL_RETRY_NS
|
||||
self._reset("disabled", now_ns)
|
||||
|
||||
def tick(self) -> Observation | None:
|
||||
self.messages.update(0)
|
||||
now_ns = self.mono_clock()
|
||||
self._refresh_settings(now_ns)
|
||||
active = (os.getenv("STARPILOT_VASM_DEVELOPMENT") == "1" and self.saved.readable and self.saved.valid and
|
||||
self.saved.preferences.enabled and self.saved.preferences.annotation is not None and self.model_path is not None)
|
||||
if not active:
|
||||
self._reset("disabled", now_ns)
|
||||
return None
|
||||
try:
|
||||
if not self._fresh_authority(now_ns):
|
||||
self._reset("unavailable", now_ns)
|
||||
return None
|
||||
if self.model is None:
|
||||
if now_ns - self.last_model_attempt_ns < MODEL_RETRY_NS:
|
||||
return None
|
||||
self.last_model_attempt_ns = now_ns
|
||||
self.model = self.model_factory(self.model_path)
|
||||
if not self.model.load():
|
||||
self.model = None
|
||||
self._reset("unavailable", now_ns)
|
||||
return None
|
||||
self.messages.update(0)
|
||||
if not self._fresh_authority(self.mono_clock()) or not self._settings_current():
|
||||
self._reset("unavailable", self.mono_clock())
|
||||
return None
|
||||
if self.client is None:
|
||||
self.client = self.client_factory()
|
||||
if not self.client.is_connected():
|
||||
self._reset("unavailable", now_ns)
|
||||
self.client = self.client_factory()
|
||||
self.client.connect(False)
|
||||
return None
|
||||
base_interval_ns = FOLLOWUP_INTERVAL_NS if now_ns < self.followup_until_ns else BASE_INTERVAL_NS
|
||||
cpu_usage = getattr(self.messages["deviceState"], "cpuUsagePercent", None)
|
||||
interval_ns = int(base_interval_ns * self.cpu_budget.update(cpu_usage, now_mono_ns=now_ns))
|
||||
if self.last_processed_ns and now_ns - self.last_processed_ns < interval_ns:
|
||||
return None
|
||||
frame = copy_cabin_nv12(self.client)
|
||||
if frame is None:
|
||||
return None
|
||||
image, eof_boot_ns, frame_id = frame
|
||||
pair = self.pair_clock()
|
||||
if pair is None:
|
||||
self._reset("stale", now_ns)
|
||||
return None
|
||||
offset_ns = pair[1] - pair[0]
|
||||
if (self.last_pair_offset_ns is not None and abs(offset_ns - self.last_pair_offset_ns) > RESUME_SKEW_NS or
|
||||
eof_boot_ns <= max(self.last_eof_boot_ns, self.camera_after_boot_ns) or frame_id <= self.last_frame_id or
|
||||
not 0 <= pair[1] - eof_boot_ns <= SOURCE_TTL_NS):
|
||||
self._reset("stale", now_ns)
|
||||
return None
|
||||
self.last_pair_offset_ns = offset_ns
|
||||
self.last_eof_boot_ns, self.last_frame_id = eof_boot_ns, frame_id
|
||||
sides = self.saved.preferences.annotation.configured_sides
|
||||
side = self.next_side if self.next_side in sides else sides[0]
|
||||
source_mono_ns = eof_boot_ns - offset_ns
|
||||
previous_side_ns = self.last_side_mono_ns[side]
|
||||
dt = (source_mono_ns - previous_side_ns) / 1e9 if previous_side_ns else interval_ns / 1e9
|
||||
score = self.model.infer_nv12(image, width=image.shape[1], height=image.shape[0] * 2 // 3,
|
||||
annotation=self.saved.preferences.annotation, side=side)
|
||||
if score is None:
|
||||
self.model = None
|
||||
self.last_model_attempt_ns = self.mono_clock()
|
||||
self._reset("stale", now_ns)
|
||||
return None
|
||||
self.messages.update(0)
|
||||
saved_current = self._settings_current()
|
||||
after = self.pair_clock()
|
||||
if (after is None or abs((after[1] - after[0]) - offset_ns) > RESUME_SKEW_NS or
|
||||
not 0 <= after[1] - eof_boot_ns <= SOURCE_TTL_NS):
|
||||
self._reset("stale", now_ns)
|
||||
return None
|
||||
if not saved_current or not self._fresh_authority(after[0]):
|
||||
self._reset("unavailable", after[0])
|
||||
return None
|
||||
if self.policy.threshold != self.saved.preferences.confidence or \
|
||||
self.policy.smooth_seconds != self.saved.preferences.smooth_seconds:
|
||||
self.policy = WarningPolicy(threshold=self.saved.preferences.confidence,
|
||||
smooth_seconds=self.saved.preferences.smooth_seconds)
|
||||
self.policy.update(side, [1.0 - score, score, 0.0], now=source_mono_ns / 1e9, dt=dt)
|
||||
self.last_processed_ns = after[0]
|
||||
self.last_side_mono_ns[side] = source_mono_ns
|
||||
self.side_frame_id[side], self.side_eof_boot_ns[side] = frame_id, eof_boot_ns
|
||||
self.next_side = sides[(sides.index(side) + 1) % len(sides)]
|
||||
state = self.policy.state(source_mono_ns / 1e9)
|
||||
if state.display_left or state.display_right:
|
||||
self.followup_until_ns = after[0] + FOLLOWUP_WINDOW_NS
|
||||
self.status = "valid"
|
||||
self.sequence += 1
|
||||
observation = self._observation("valid", after[0], after[1], eof_boot_ns, frame_id, side, state)
|
||||
self.emit(observation)
|
||||
return observation
|
||||
except Exception as error:
|
||||
if now_ns - self.last_error_log_ns >= 10_000_000_000:
|
||||
cloudlog.error("V-ASM observation unavailable: %s", error)
|
||||
self.last_error_log_ns = now_ns
|
||||
self._reset("unavailable", now_ns)
|
||||
return None
|
||||
|
||||
def close(self) -> None:
|
||||
self._reset("disabled", self.mono_clock())
|
||||
for socket in getattr(self.messages, "sock", {}).values():
|
||||
close = getattr(socket, "close", None)
|
||||
if close is not None:
|
||||
close()
|
||||
|
||||
|
||||
def main() -> None:
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.cereal.visionipc import VisionStreamType
|
||||
from msgq.visionipc import VisionIpcClient
|
||||
|
||||
stop = threading.Event()
|
||||
previous = {number: signal.getsignal(number) for number in (signal.SIGINT, signal.SIGTERM)}
|
||||
for number in previous:
|
||||
signal.signal(number, lambda _signal, _frame: stop.set())
|
||||
path = os.getenv("STARPILOT_VASM_MODEL_PATH", "")
|
||||
model_path = Path(path) if path and Path(path).is_absolute() else None
|
||||
messages = messaging.SubMaster(["deviceState", "carState"])
|
||||
pm = messaging.PubMaster(["spotMonitorState"])
|
||||
runtime = VASMRuntime(Params(), messages,
|
||||
lambda: cabin_client_factory(VisionIpcClient, VisionStreamType),
|
||||
lambda observation: publish_observation(pm, observation), model_path=model_path)
|
||||
try:
|
||||
while not stop.is_set():
|
||||
started = time.monotonic()
|
||||
runtime.tick()
|
||||
stop.wait(max(0.0, TICK_SECONDS - (time.monotonic() - started)))
|
||||
finally:
|
||||
runtime.close()
|
||||
for number, handler in previous.items():
|
||||
signal.signal(number, handler)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,169 @@
|
||||
"""Exact-asset CPU wrapper with deterministic NV12/mask and fake-network tests."""
|
||||
|
||||
import hashlib
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from openpilot.starpilot.spot_monitor import inference
|
||||
from openpilot.starpilot.spot_monitor.policy import decode_annotation
|
||||
|
||||
|
||||
class FakeNet:
|
||||
def __init__(self):
|
||||
self.blob = None
|
||||
self.output = np.array([[0.05, 0.95, 0.0]], dtype=np.float32)
|
||||
self.backend = None
|
||||
self.target = None
|
||||
|
||||
def setPreferableBackend(self, backend):
|
||||
self.backend = backend
|
||||
|
||||
def setPreferableTarget(self, target):
|
||||
self.target = target
|
||||
|
||||
def setInput(self, blob):
|
||||
self.blob = blob
|
||||
|
||||
def forward(self):
|
||||
return self.output
|
||||
|
||||
|
||||
class FakeDnn:
|
||||
DNN_BACKEND_OPENCV = 7
|
||||
DNN_TARGET_CPU = 8
|
||||
|
||||
def __init__(self, net):
|
||||
self.net = net
|
||||
self.loaded = None
|
||||
|
||||
def readNetFromONNX(self, raw):
|
||||
self.loaded = raw
|
||||
return self.net
|
||||
|
||||
|
||||
class FakeCV2:
|
||||
COLOR_YUV2RGB_NV12 = 1
|
||||
INTER_LINEAR = 2
|
||||
|
||||
def __init__(self, net):
|
||||
self.dnn = FakeDnn(net)
|
||||
self.mask: np.ndarray | None = None
|
||||
self.threads = None
|
||||
|
||||
def setNumThreads(self, count):
|
||||
self.threads = count
|
||||
|
||||
def cvtColor(self, nv12, code):
|
||||
assert code == self.COLOR_YUV2RGB_NV12
|
||||
height = nv12.shape[0] * 2 // 3
|
||||
return np.repeat(nv12[:height, :, None], 3, axis=2)
|
||||
|
||||
def fillPoly(self, mask, polygons, value):
|
||||
points = polygons[0]
|
||||
for y in range(mask.shape[0]):
|
||||
for x in range(mask.shape[1]):
|
||||
# A strict interior raster is enough to expose the wrapper's mask
|
||||
# application without requiring OpenCV in the host test environment.
|
||||
inside = False
|
||||
for a, b in zip(points, np.roll(points, -1, axis=0), strict=True):
|
||||
if (a[1] > y + 0.5) != (b[1] > y + 0.5):
|
||||
boundary = a[0] + (y + 0.5 - a[1]) * (b[0] - a[0]) / (b[1] - a[1])
|
||||
if x + 0.5 < boundary:
|
||||
inside = not inside
|
||||
if inside:
|
||||
mask[y, x] = value
|
||||
self.mask = mask.copy()
|
||||
|
||||
def bitwise_and(self, left, right, *, mask):
|
||||
return np.where(mask[:, :, None] != 0, left, 0).astype(np.uint8)
|
||||
|
||||
def resize(self, image, size, *, interpolation):
|
||||
assert interpolation == self.INTER_LINEAR
|
||||
width, height = size
|
||||
ys = (np.arange(height) * image.shape[0] // height).astype(int)
|
||||
xs = (np.arange(width) * image.shape[1] // width).astype(int)
|
||||
return image[np.ix_(ys, xs)]
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def tiny_model(tmp_path: Path, monkeypatch):
|
||||
raw = b"small reviewed ONNX fixture"
|
||||
monkeypatch.setattr(inference, "MODEL_SIZE_BYTES", len(raw))
|
||||
monkeypatch.setattr(inference, "MODEL_SHA256", hashlib.sha256(raw).hexdigest())
|
||||
path = tmp_path / "model.onnx"
|
||||
path.write_bytes(raw)
|
||||
return path
|
||||
|
||||
|
||||
def test_exact_asset_cpu_mask_crop_class_one_and_loaded_bytes_pinned(tiny_model, monkeypatch):
|
||||
net = FakeNet()
|
||||
cv2 = FakeCV2(net)
|
||||
model = inference.VASMInference(tiny_model, cv2_module=cv2)
|
||||
assert model.load()
|
||||
assert cv2.threads == 1
|
||||
assert model.loaded_sha256 == inference.MODEL_SHA256
|
||||
assert cv2.dnn.loaded == tiny_model.read_bytes()
|
||||
assert (net.backend, net.target) == (cv2.dnn.DNN_BACKEND_OPENCV, cv2.dnn.DNN_TARGET_CPU)
|
||||
annotation = decode_annotation(b'{"version":1,"width":32,"height":32,"poly_left":[[4,4],[20,4],[4,20]],"poly_right":[]}')
|
||||
frame = np.full((48, 32), 128, dtype=np.uint8)
|
||||
frame[:32, :] = 255
|
||||
assert model.infer_nv12(frame, width=32, height=32, annotation=annotation, side="left") == pytest.approx(0.95)
|
||||
assert cv2.mask is not None
|
||||
assert cv2.mask[2, 2] == 255
|
||||
assert cv2.mask[-2, -2] == 0
|
||||
assert net.blob.shape == (1, 3, 352, 352)
|
||||
assert net.blob.dtype == np.float32
|
||||
assert net.blob.max() == 1.0 and net.blob.min() == 0.0
|
||||
tiny_model.write_bytes(b"small tampered ONNX fixture")
|
||||
def fail_reopen(_):
|
||||
raise AssertionError("infer reopened model source")
|
||||
|
||||
monkeypatch.setattr(inference, "_verified_model_bytes", fail_reopen)
|
||||
assert model.infer_nv12(frame, width=32, height=32, annotation=annotation, side="left") == pytest.approx(0.95)
|
||||
assert model.loaded_sha256 == inference.MODEL_SHA256
|
||||
|
||||
|
||||
def test_changed_source_rejected_on_explicit_reload(tiny_model):
|
||||
model = inference.VASMInference(tiny_model, cv2_module=FakeCV2(FakeNet()))
|
||||
assert model.load()
|
||||
tiny_model.write_bytes(b"small tampered ONNX fixture")
|
||||
assert not model.load()
|
||||
assert model.net is None and model.loaded_sha256 is None
|
||||
|
||||
|
||||
def test_asset_missing_wrong_size_symlink_and_bad_output_fail_unavailable(tiny_model, tmp_path):
|
||||
cv2 = FakeCV2(FakeNet())
|
||||
missing = inference.VASMInference(tmp_path / "missing.onnx", cv2_module=cv2)
|
||||
assert not missing.load()
|
||||
alias = tmp_path / "alias.onnx"
|
||||
alias.symlink_to(tiny_model)
|
||||
assert not inference.VASMInference(alias, cv2_module=cv2).load()
|
||||
tiny_model.write_bytes(b"wrong")
|
||||
assert not inference.VASMInference(tiny_model, cv2_module=cv2).load()
|
||||
tiny_model.write_bytes(b"small reviewed ONNX fixture")
|
||||
model = inference.VASMInference(tiny_model, cv2_module=cv2)
|
||||
assert model.load()
|
||||
frame = np.zeros((48, 32), dtype=np.uint8)
|
||||
annotation = decode_annotation(b'{"version":1,"width":32,"height":32,"poly_left":[[4,4],[20,4],[4,20]],"poly_right":[]}')
|
||||
cv2.dnn.net.output = np.array([[float("nan"), 0.95, 0.0]], dtype=np.float32)
|
||||
assert model.infer_nv12(frame, width=32, height=32, annotation=annotation, side="left") is None
|
||||
assert model.net is None
|
||||
|
||||
|
||||
def test_fifo_asset_refused_without_waiting_for_writer(tmp_path):
|
||||
fifo = tmp_path / "model.fifo"
|
||||
os.mkfifo(fifo)
|
||||
assert not inference.VASMInference(fifo, cv2_module=FakeCV2(FakeNet())).load()
|
||||
|
||||
|
||||
def test_wrong_frame_and_unconfigured_side_fail_without_model_output(tiny_model):
|
||||
net = FakeNet()
|
||||
model = inference.VASMInference(tiny_model, cv2_module=FakeCV2(net))
|
||||
assert model.load()
|
||||
annotation = decode_annotation(b'{"version":1,"width":32,"height":32,"poly_left":[[4,4],[20,4],[4,20]],"poly_right":[]}')
|
||||
assert model.infer_nv12(np.zeros((32, 32), dtype=np.uint8), width=32, height=32,
|
||||
annotation=annotation, side="left") is None
|
||||
assert net.blob is None
|
||||
@@ -0,0 +1,160 @@
|
||||
"""Serialized V-ASM wire and historical @136 compatibility tests."""
|
||||
|
||||
import math
|
||||
import unittest
|
||||
|
||||
from openpilot.cereal import log
|
||||
from openpilot.starpilot.spot_monitor.inference import MODEL_SHA256
|
||||
from openpilot.starpilot.spot_monitor.observation import ObservationReader
|
||||
|
||||
|
||||
SESSION = "a" * 32
|
||||
OTHER_SESSION = "c" * 32
|
||||
FINGERPRINT = "b" * 64
|
||||
M = 1_000_000
|
||||
# Actual frozen StarPilotLateralManeuverPlanDEPRECATED Event bytes, emitted by
|
||||
# the frozen parser in a separate process (Cap'n Proto IDs cannot be loaded
|
||||
# twice into one process). The historical field remains curvature in 1/m.
|
||||
FROZEN_CURVATURE_EVENT = bytes.fromhex(
|
||||
"00000000050000000000000002000100000000000000000086000000000000000000000001000000cdcc4c3c00000000")
|
||||
|
||||
|
||||
def packet(*, session=SESSION, sequence=1, frame_id=0, observed=1000*M,
|
||||
eof=900*M, side_eof=None, side_frame_id=0, valid=True):
|
||||
msg = log.Event.new_message()
|
||||
msg.init("spotMonitorState")
|
||||
msg.valid = valid
|
||||
msg.logMonoTime = observed
|
||||
obs = msg.spotMonitorState.init("observation")
|
||||
obs.version = 1
|
||||
obs.producerSessionId = session
|
||||
obs.sequence = sequence
|
||||
obs.modelSha256 = MODEL_SHA256
|
||||
obs.settingsFingerprint = FINGERPRINT
|
||||
obs.observedMonoTime = observed
|
||||
obs.observedBootTime = observed
|
||||
obs.sourceFrameId = frame_id
|
||||
obs.sourceFrameEofBootTime = eof
|
||||
obs.validUntilBootTime = eof + 500*M
|
||||
left = obs.init("left")
|
||||
left.status = "warning"
|
||||
left.confidence = 0.96
|
||||
left.warning = True
|
||||
left.sourceFrameId = side_frame_id
|
||||
left.sourceFrameEofBootTime = eof if side_eof is None else side_eof
|
||||
left.sourceObservedMonoTime = left.sourceFrameEofBootTime
|
||||
left.validUntilBootTime = left.sourceFrameEofBootTime + 3000*M
|
||||
return msg
|
||||
|
||||
|
||||
def read(reader, msg, *, now=1100*M, fingerprint=FINGERPRINT, boot_offset=0):
|
||||
msg.clear_write_flag() # The cached-event regression serializes one builder twice.
|
||||
with log.Event.from_bytes(msg.to_bytes()) as event:
|
||||
return reader.read(event, now_mono_ns=now, now_boot_ns=now + boot_offset,
|
||||
settings_fingerprint=fingerprint)
|
||||
|
||||
|
||||
class ObservationWireTest(unittest.TestCase):
|
||||
def test_historical_curvature_has_no_vasm_version(self):
|
||||
with log.Event.from_bytes(FROZEN_CURVATURE_EVENT) as event:
|
||||
self.assertEqual(event.which(), "spotMonitorState")
|
||||
self.assertAlmostEqual(event.spotMonitorState.desiredCurvature, 0.0125)
|
||||
self.assertEqual(event.spotMonitorState.observation.version, 0)
|
||||
self.assertFalse(ObservationReader().read(event, now_mono_ns=1100*M,
|
||||
now_boot_ns=1100*M,
|
||||
settings_fingerprint=FINGERPRINT).display_left.warning)
|
||||
|
||||
def test_serialized_warning_expires_even_when_cached_event_is_reused(self):
|
||||
reader = ObservationReader()
|
||||
first = packet()
|
||||
self.assertTrue(read(reader, first).display_left.warning)
|
||||
self.assertTrue(reader.current_at(now_mono_ns=1200*M, now_boot_ns=1200*M,
|
||||
settings_fingerprint=FINGERPRINT).display_left.warning)
|
||||
# The camera may publish at 1 Hz; validated warnings retain the original
|
||||
# 3-second hold even though a new frame must be admitted within 500 ms.
|
||||
self.assertTrue(read(reader, first, now=1401*M).display_left.warning)
|
||||
self.assertFalse(reader.current_at(now_mono_ns=3901*M, now_boot_ns=3901*M,
|
||||
settings_fingerprint=FINGERPRINT).display_left.warning)
|
||||
|
||||
def test_new_frame_does_not_renew_other_side_hold(self):
|
||||
reader = ObservationReader()
|
||||
self.assertTrue(read(reader, packet()).display_left.warning)
|
||||
later = packet(sequence=2, frame_id=1, observed=1500*M, eof=1450*M,
|
||||
side_eof=900*M)
|
||||
self.assertTrue(read(reader, later, now=1550*M).display_left.warning)
|
||||
very_late = packet(sequence=3, frame_id=2, observed=4000*M, eof=3950*M,
|
||||
side_eof=900*M)
|
||||
self.assertFalse(read(reader, very_late, now=4050*M).display_left.warning)
|
||||
|
||||
def test_replay_invalid_loss_and_session_retirement(self):
|
||||
reader = ObservationReader()
|
||||
self.assertTrue(read(reader, packet()).display_left.warning)
|
||||
self.assertTrue(read(reader, packet(sequence=1, valid=False)).display_left.warning)
|
||||
self.assertFalse(read(reader, packet(sequence=2, frame_id=1, observed=1200*M,
|
||||
eof=1150*M, valid=False), now=1250*M).display_left.warning)
|
||||
self.assertTrue(read(reader, packet(session=OTHER_SESSION, observed=1400*M,
|
||||
eof=1350*M), now=1450*M).display_left.warning)
|
||||
self.assertTrue(read(reader, packet(session=SESSION, observed=1500*M,
|
||||
eof=1450*M, valid=False), now=1550*M).display_left.warning)
|
||||
|
||||
def test_invalid_reset_retires_old_session_before_delayed_valid_packet(self):
|
||||
reader = ObservationReader()
|
||||
self.assertTrue(read(reader, packet()).display_left.warning)
|
||||
self.assertFalse(read(reader, packet(session=OTHER_SESSION, sequence=0,
|
||||
observed=1200*M, eof=0, valid=False),
|
||||
now=1250*M).display_left.warning)
|
||||
# The old producer's later sequence remains in-flight after its reset.
|
||||
self.assertFalse(read(reader, packet(session=SESSION, sequence=2, frame_id=1,
|
||||
observed=1300*M, eof=1250*M), now=1350*M).display_left.warning)
|
||||
self.assertTrue(read(reader, packet(session=OTHER_SESSION, observed=1400*M,
|
||||
eof=1350*M), now=1450*M).display_left.warning)
|
||||
|
||||
def test_backward_clock_cannot_resurrect_expired_warning(self):
|
||||
reader = ObservationReader()
|
||||
self.assertTrue(read(reader, packet()).display_left.warning)
|
||||
self.assertFalse(reader.current_at(now_mono_ns=3901*M, now_boot_ns=3901*M,
|
||||
settings_fingerprint=FINGERPRINT).display_left.warning)
|
||||
self.assertFalse(reader.current_at(now_mono_ns=1100*M, now_boot_ns=1100*M,
|
||||
settings_fingerprint=FINGERPRINT).display_left.warning)
|
||||
self.assertFalse(read(reader, packet(sequence=2, frame_id=1,
|
||||
observed=1200*M, eof=1150*M), now=1250*M).display_left.warning)
|
||||
|
||||
def test_bad_source_settings_hash_and_resume_fail_closed(self):
|
||||
for mutate in (lambda m: setattr(m.spotMonitorState.observation, "modelSha256", "0" * 64),
|
||||
lambda m: setattr(m.spotMonitorState.observation, "settingsFingerprint", "0" * 64),
|
||||
lambda m: setattr(m.spotMonitorState.observation, "validUntilBootTime", 9000*M),
|
||||
lambda m: setattr(m.spotMonitorState.observation.left, "sourceObservedMonoTime", 500*M),
|
||||
lambda m: setattr(m.spotMonitorState.observation.left, "confidence", math.nan)):
|
||||
with self.subTest(mutate=mutate):
|
||||
msg = packet()
|
||||
mutate(msg)
|
||||
self.assertFalse(read(ObservationReader(), msg).display_left.warning)
|
||||
reader = ObservationReader()
|
||||
self.assertTrue(read(reader, packet()).display_left.warning)
|
||||
self.assertFalse(reader.current_at(now_mono_ns=1200*M, now_boot_ns=10200*M,
|
||||
settings_fingerprint=FINGERPRINT).display_left.warning)
|
||||
# A queued pre-resume packet cannot claim the new BOOT/MONO epoch.
|
||||
self.assertFalse(read(reader, packet(sequence=2, frame_id=1, observed=1150*M,
|
||||
eof=1100*M), now=1200*M, boot_offset=9000*M).display_left.warning)
|
||||
|
||||
def test_nonzero_boot_offset_and_future_frame_bounds(self):
|
||||
def shifted():
|
||||
msg = packet()
|
||||
wire = msg.spotMonitorState.observation
|
||||
wire.observedBootTime += 9000*M
|
||||
wire.sourceFrameEofBootTime += 9000*M
|
||||
wire.validUntilBootTime += 9000*M
|
||||
wire.left.sourceFrameEofBootTime += 9000*M
|
||||
wire.left.validUntilBootTime += 9000*M
|
||||
return msg
|
||||
|
||||
self.assertTrue(read(ObservationReader(), shifted(), now=1100*M,
|
||||
boot_offset=9000*M).display_left.warning)
|
||||
future = shifted()
|
||||
future.spotMonitorState.observation.sourceFrameEofBootTime = 10150*M
|
||||
self.assertFalse(read(ObservationReader(), future, now=1100*M,
|
||||
boot_offset=9000*M).display_left.warning)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -0,0 +1,126 @@
|
||||
"""Frozen V-ASM crop/side mapping with malformed and stale input boundaries."""
|
||||
|
||||
import json
|
||||
|
||||
import pytest
|
||||
|
||||
from openpilot.starpilot.spot_monitor.policy import (
|
||||
WarningPolicy, class_one_confidence, crop_for_side, decode_annotation, encode_annotation, polygon_pixels,
|
||||
)
|
||||
|
||||
|
||||
ONE_SIDE = {"version": 1, "width": 200, "height": 100,
|
||||
"poly_left": [[10, 10], [80, 10], [80, 80], [10, 80]], "poly_right": []}
|
||||
|
||||
|
||||
def test_frozen_pixel_polygons_normalize_and_even_nv12_crop():
|
||||
annotation = decode_annotation(json.dumps(ONE_SIDE).encode())
|
||||
assert annotation.configured_sides == ("left",)
|
||||
assert annotation.camera_left is not None
|
||||
assert annotation.camera_left[0] == (0.05, 0.1)
|
||||
assert crop_for_side(annotation, "right", 200, 100) is None
|
||||
crop = crop_for_side(annotation, "left", 200, 100)
|
||||
assert (crop.x, crop.y, crop.width, crop.height) == (10, 10, 72, 72)
|
||||
assert (crop.model_width, crop.model_height, crop.pad_left, crop.pad_top) == (352, 352, 0, 0)
|
||||
doubled = crop_for_side(annotation, "left", 400, 200)
|
||||
assert (doubled.x, doubled.y, doubled.width, doubled.height) == (20, 20, 142, 142)
|
||||
assert decode_annotation(encode_annotation(annotation)) == annotation
|
||||
|
||||
|
||||
def test_rectangular_crop_letterboxes_to_frozen_352_square():
|
||||
config = {"version": 1, "width": 200, "height": 100,
|
||||
"poly_left": [[10, 10], [110, 10], [110, 50], [10, 50]], "poly_right": []}
|
||||
crop = crop_for_side(decode_annotation(json.dumps(config)), "left", 200, 100)
|
||||
assert (crop.x, crop.y, crop.width, crop.height) == (10, 10, 102, 42)
|
||||
assert (crop.model_width, crop.model_height, crop.pad_left, crop.pad_top) == (352, 145, 0, 103)
|
||||
|
||||
|
||||
def test_frozen_float32_pixel_projection_same_and_cross_resolution():
|
||||
config = {"version": 1, "width": 1928, "height": 1208,
|
||||
"poly_left": [[1609, 480], [1861, 480], [1861, 552], [1609, 552]], "poly_right": []}
|
||||
annotation = decode_annotation(json.dumps(config))
|
||||
assert polygon_pixels(annotation, "left", 1928, 1208)[0] == (1609, 480)
|
||||
same = crop_for_side(annotation, "left", 1928, 1208)
|
||||
assert (same.x, same.y, same.width, same.height) == (1608, 480, 254, 74)
|
||||
assert polygon_pixels(annotation, "left", 1344, 760)[0] == (1121, 301)
|
||||
scaled = crop_for_side(annotation, "left", 1344, 760)
|
||||
assert (scaled.x, scaled.y, scaled.width, scaled.height) == (1120, 300, 178, 48)
|
||||
|
||||
config = {"version": 1, "width": 1344, "height": 760,
|
||||
"poly_left": [[448, 253], [635, 253], [635, 346], [448, 346]], "poly_right": []}
|
||||
annotation = decode_annotation(json.dumps(config))
|
||||
crop = crop_for_side(annotation, "left", 1344, 760)
|
||||
assert (crop.x, crop.y, crop.width, crop.height) == (448, 252, 188, 94)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("changed", [
|
||||
{"width": 0}, {"width": True}, {"width": 201}, {"poly_left": [[1, 1], [2, 2]]},
|
||||
{"poly_left": [[1, 1], [2, 2], [3, 3]]}, {"poly_left": [[1, 1], [2, 1], [2, float("nan")]]},
|
||||
{"poly_left": [[1, 1], [201, 1], [2, 2]]},
|
||||
{"poly_left": [[10, 10], [80, 80], [10, 80], [80, 10]]},
|
||||
{"poly_left": [[10, 10], [80, 10], [80, 80], [40, 10], [10, 80]]},
|
||||
{"poly_left": [[10, 10], [80, 10], [80, 80], [10, 80], [10, 10]]},
|
||||
{"version": True}, {"version": 2},
|
||||
{"poly_left": [], "poly_right": []},
|
||||
])
|
||||
def test_malformed_annotations_rejected(changed):
|
||||
with pytest.raises(ValueError):
|
||||
decode_annotation(json.dumps({**ONE_SIDE, **changed}))
|
||||
|
||||
|
||||
def test_duplicate_unknown_oversized_and_nonfinite_json_rejected():
|
||||
for raw in (b'{"width":200,"width":200,"height":100,"poly_left":[],"poly_right":[]}',
|
||||
json.dumps({key: value for key, value in ONE_SIDE.items() if key != "version"}).encode(),
|
||||
json.dumps({**ONE_SIDE, "unknown": 1}).encode(),
|
||||
b" " * 8193,
|
||||
json.dumps(ONE_SIDE).replace("200", "NaN", 1).encode()):
|
||||
with pytest.raises(ValueError):
|
||||
decode_annotation(raw)
|
||||
|
||||
|
||||
def test_class_one_and_camera_to_display_swap_with_hysteresis():
|
||||
assert class_one_confidence([0.05, 0.95, 0.0]) == 0.95
|
||||
assert class_one_confidence([0.2, 0.8]) == 0.8
|
||||
for malformed in ([0.9], [0.1, float("nan")], [0.1, 1.1], [], None):
|
||||
assert class_one_confidence(malformed) == 0.0
|
||||
policy = WarningPolicy(threshold=0.94, smooth_seconds=0.2)
|
||||
state = policy.update("left", [0.05, 0.95, 0.0], now=1.0, dt=0.5)
|
||||
assert (state.display_left, state.display_right) == (False, True)
|
||||
assert (state.display_left_confidence, state.display_right_confidence) == (0.0, 0.95)
|
||||
assert policy.update("left", [0.2, 0.8, 0.0], now=1.5, dt=0.5).display_right
|
||||
assert not policy.update("left", [0.22, 0.78, 0.0], now=2.0, dt=0.5).display_right
|
||||
assert policy.update("right", [0.04, 0.96, 0.0], now=2.5, dt=0.5).display_left
|
||||
assert not policy.update("left", [0.1, float("nan")], now=2.6, dt=0.5).display_left
|
||||
|
||||
|
||||
def test_frozen_ema_and_per_side_freshness_fail_closed():
|
||||
policy = WarningPolicy(threshold=0.8, smooth_seconds=0.5)
|
||||
for index in range(1, 8):
|
||||
state = policy.update("left", [0.0, 1.0], now=index / 10, dt=0.1)
|
||||
assert not state.display_right
|
||||
state = policy.update("left", [0.0, 1.0], now=0.8, dt=0.1)
|
||||
assert state.display_right
|
||||
assert policy.score["left"] == pytest.approx(1 - 0.8**8)
|
||||
assert not policy.state(3.81).display_right
|
||||
# Fresh raw confidence after a gap must build the EMA again.
|
||||
assert not policy.update("left", [0.0, 1.0], now=4.0, dt=0.1).display_right
|
||||
assert not policy.update("left", [0.0, 1.0], now=4.0, dt=0.1).display_right
|
||||
assert not policy.update("left", [0.0, 1.0], now=4.1, dt=float("nan")).display_right
|
||||
|
||||
|
||||
def test_disabled_or_clock_invalid_reset_never_retains_warning():
|
||||
policy = WarningPolicy()
|
||||
assert policy.update("right", [0.02, 0.98], now=1.0, dt=1.0).display_left
|
||||
policy.reset()
|
||||
assert not policy.state(1.1).display_left
|
||||
assert policy.update("right", [0.02, 0.98], now=2.0, dt=1.0).display_left
|
||||
assert not policy.update("right", [0.02, 0.98], now=1.9, dt=1.0).display_left
|
||||
with pytest.raises(ValueError):
|
||||
policy.update("middle", [0.02, 0.98], now=2.1, dt=1.0)
|
||||
|
||||
|
||||
def test_reversed_status_clock_clears_history_without_resurrection():
|
||||
policy = WarningPolicy()
|
||||
assert policy.update("left", [0.02, 0.98], now=5.0, dt=1.0).display_right
|
||||
assert not policy.state(4.0).display_right
|
||||
assert not policy.state(5.1).display_right
|
||||
@@ -0,0 +1,323 @@
|
||||
"""Synthetic serialized cabin planes and fail-closed observation lifecycle."""
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.starpilot.spot_monitor.inference import VASMInference
|
||||
from openpilot.starpilot.spot_monitor.policy import decode_annotation
|
||||
from openpilot.starpilot.spot_monitor.preferences import Preferences, encode
|
||||
from openpilot.starpilot.spot_monitor.runtime import CpuBudget, VASMRuntime, cabin_client_factory, copy_cabin_nv12, publish_observation
|
||||
|
||||
|
||||
class State:
|
||||
def __init__(self, **kwargs):
|
||||
self.__dict__.update(kwargs)
|
||||
|
||||
|
||||
class Messages:
|
||||
def __init__(self, clock):
|
||||
from opendbc.car.structs import car
|
||||
self.clock = clock
|
||||
self.seen = dict.fromkeys(("deviceState", "carState"), True)
|
||||
self.alive = self.seen.copy()
|
||||
self.valid = self.seen.copy()
|
||||
self.logMonoTime = dict.fromkeys(self.seen, clock[0])
|
||||
self.recv_time = dict.fromkeys(self.seen, clock[0] / 1e9)
|
||||
self.values = {"deviceState": State(started=True, cpuUsagePercent=[10.0] * 8),
|
||||
"carState": State(canValid=True, gearShifter=car.CarState.GearShifter.drive)}
|
||||
|
||||
def __getitem__(self, name):
|
||||
return self.values[name]
|
||||
|
||||
def update(self, timeout):
|
||||
assert timeout == 0
|
||||
|
||||
def refresh(self):
|
||||
for name in self.seen:
|
||||
self.logMonoTime[name] = self.clock[0]
|
||||
self.recv_time[name] = self.clock[0] / 1e9
|
||||
|
||||
|
||||
class Client:
|
||||
width, height, stride, uv_offset = 32, 32, 40, 1320
|
||||
|
||||
def __init__(self):
|
||||
self.frames = []
|
||||
self.connected = True
|
||||
|
||||
def is_connected(self):
|
||||
return self.connected
|
||||
|
||||
def connect(self, blocking):
|
||||
assert blocking is False
|
||||
self.connected = True
|
||||
|
||||
def recv(self, timeout):
|
||||
assert timeout == 0
|
||||
if not self.frames:
|
||||
return None
|
||||
self.timestamp_eof, self.frame_id, raw = self.frames.pop(0)
|
||||
return State(data=raw)
|
||||
|
||||
def add(self, eof, frame_id):
|
||||
raw = bytearray(self.uv_offset + self.stride * self.height // 2)
|
||||
for y in range(self.height):
|
||||
raw[y * self.stride:y * self.stride + self.width] = bytes([y]) * self.width
|
||||
raw[self.uv_offset:] = bytes([128]) * (len(raw) - self.uv_offset)
|
||||
self.frames.append((eof, frame_id, bytes(raw)))
|
||||
|
||||
|
||||
class Model(VASMInference):
|
||||
def __init__(self):
|
||||
super().__init__(Path("/external/model.onnx"))
|
||||
self.loaded_sha256 = "5d20cdbb457ba18db51a537ee2e305bbe442264b1613956068d473e35d15900d"
|
||||
|
||||
def load(self):
|
||||
return True
|
||||
|
||||
def infer_nv12(self, frame, **kwargs):
|
||||
assert frame.shape == (48, 32) and frame[31, 0] == 31 and frame[32, 0] == 128
|
||||
return 0.96
|
||||
|
||||
|
||||
def test_stride_uv_offset_copy_and_short_buffer_refused():
|
||||
client = Client()
|
||||
client.add(2_000_000_000, 0)
|
||||
copied = copy_cabin_nv12(client)
|
||||
assert copied is not None
|
||||
image, eof, frame_id = copied
|
||||
assert image.shape == (48, 32) and eof == 2_000_000_000 and frame_id == 0
|
||||
assert image[31, 0] == 31 and image[32, 0] == 128
|
||||
client.frames.append((2_000_000_100, 1, b"short"))
|
||||
assert copy_cabin_nv12(client) is None
|
||||
|
||||
|
||||
def test_real_vision_stream_enum_selects_only_existing_camerad_cabin():
|
||||
from openpilot.cereal.visionipc import VisionStreamType
|
||||
|
||||
calls = []
|
||||
def constructor(*args):
|
||||
calls.append(args)
|
||||
return object()
|
||||
cabin_client_factory(constructor, VisionStreamType)
|
||||
assert calls == [("camerad", VisionStreamType.VISION_STREAM_CABIN, True)]
|
||||
|
||||
|
||||
def test_cpu_budget_frozen_target_smoothing_and_invalid_conservative():
|
||||
budget = CpuBudget()
|
||||
assert budget.update([10.0] * 8, now_mono_ns=1_000_000_000) == 1.0
|
||||
busy = budget.update([90.0] * 8, now_mono_ns=6_000_000_000)
|
||||
assert 3.3 < busy < 3.6 # frozen hot-core target 3.5, smoothed
|
||||
recovered = budget.update([0.0] * 8, now_mono_ns=11_000_000_000)
|
||||
assert 1.0 < recovered < 1.1
|
||||
assert budget.update([], now_mono_ns=12_000_000_000) == 4.0
|
||||
assert budget.update([float("nan")], now_mono_ns=13_000_000_000) == 4.0
|
||||
assert budget.update([True], now_mono_ns=14_000_000_000) == 4.0
|
||||
|
||||
|
||||
def test_default_off_fresh_drive_new_frames_and_stale_reset(tmp_path, monkeypatch):
|
||||
from opendbc.car.structs import car
|
||||
|
||||
annotation = decode_annotation(b'{"version":1,"width":32,"height":32,"poly_left":[[0,0],[32,0],[32,32],[0,32]],"poly_right":[]}')
|
||||
params = Params(str(tmp_path / "params"))
|
||||
path = Path(params.get_param_path("VASMPreferences"))
|
||||
path.write_bytes(encode(Preferences(True, annotation)))
|
||||
clock = [1_000_000_000]
|
||||
messages = Messages(clock)
|
||||
client = Client()
|
||||
client.add(2_000_000_000, 0)
|
||||
events = []
|
||||
runtime = VASMRuntime(params, messages, lambda: client, events.append, model_path=Path("/external/model.onnx"),
|
||||
mono_clock=lambda: clock[0], pair_clock=lambda: (clock[0], clock[0] + 1_000_000_000),
|
||||
model_factory=lambda _: Model())
|
||||
assert runtime.tick() is None
|
||||
assert not any(event.status == "valid" for event in events)
|
||||
monkeypatch.setenv("STARPILOT_VASM_DEVELOPMENT", "1")
|
||||
client.add(2_000_000_001, 0)
|
||||
event = runtime.tick()
|
||||
assert event is not None and event.status == "valid" and event.frame_id == 0
|
||||
assert event.display_right and not event.display_left
|
||||
assert event.left_frame_id == 0 and event.left_eof_boot_ns == 2_000_000_000
|
||||
assert event.observed_mono_ns == clock[0] and event.observed_boot_ns == clock[0] + 1_000_000_000
|
||||
assert event.sequence == 1
|
||||
assert len(event.settings_fingerprint) == 64 and len(event.session) == 32
|
||||
sent = []
|
||||
publish_observation(State(send=lambda service, message: sent.append((service, message))), event)
|
||||
service, envelope = sent[0]
|
||||
assert service == "spotMonitorState" and envelope.valid and envelope.logMonoTime == event.observed_mono_ns
|
||||
assert envelope.spotMonitorState.desiredCurvature == 0
|
||||
wire = envelope.spotMonitorState.observation
|
||||
assert wire.version == 1 and wire.sequence == 1 and wire.sourceFrameId == 0
|
||||
assert wire.right.status == "warning" and wire.right.sourceFrameEofBootTime == event.left_eof_boot_ns
|
||||
assert wire.left.status == "unknown" and wire.left.sourceFrameEofBootTime == 0
|
||||
clock[0] += 1_100_000_000
|
||||
messages.refresh()
|
||||
client.add(clock[0] + 1_000_000_000, 0)
|
||||
assert runtime.tick() is None # duplicate frame cannot renew held warning
|
||||
assert events[-1].status == "stale" and not events[-1].display_right
|
||||
clock[0] += 600_000_000
|
||||
messages.refresh()
|
||||
messages.values["carState"].gearShifter = car.CarState.GearShifter.park
|
||||
assert runtime.tick() is None
|
||||
assert events[-1].status == "unavailable"
|
||||
assert not events[-1].display_right
|
||||
|
||||
|
||||
def test_alternating_sides_do_not_renew_other_source_and_resume_resets(tmp_path, monkeypatch):
|
||||
annotation = decode_annotation(b'{"version":1,"width":32,"height":32,"poly_left":[[0,0],[32,0],[32,32],[0,32]],"poly_right":[[0,0],[32,0],[32,32],[0,32]]}')
|
||||
params = Params(str(tmp_path / "params"))
|
||||
path = Path(params.get_param_path("VASMPreferences"))
|
||||
path.write_bytes(encode(Preferences(True, annotation)))
|
||||
clock = [1_000_000_000]
|
||||
offset = [1_000_000_000]
|
||||
messages = Messages(clock)
|
||||
client = Client()
|
||||
client.add(2_000_000_000, 0)
|
||||
events = []
|
||||
monkeypatch.setenv("STARPILOT_VASM_DEVELOPMENT", "1")
|
||||
runtime = VASMRuntime(params, messages, lambda: client, events.append, model_path=Path("/external/model.onnx"),
|
||||
mono_clock=lambda: clock[0], pair_clock=lambda: (clock[0], clock[0] + offset[0]),
|
||||
model_factory=lambda _: Model())
|
||||
first = runtime.tick()
|
||||
assert first is not None and first.display_right and not first.display_left
|
||||
clock[0] += 350_000_000
|
||||
messages.refresh()
|
||||
client.add(clock[0] + offset[0], 1)
|
||||
second = runtime.tick()
|
||||
assert second is not None and second.display_right and second.display_left
|
||||
assert second.left_eof_boot_ns == first.left_eof_boot_ns
|
||||
assert second.right_eof_boot_ns > second.left_eof_boot_ns
|
||||
sent = []
|
||||
publish_observation(State(send=lambda service, message: sent.append(message)), second)
|
||||
wire = sent[0].spotMonitorState.observation
|
||||
assert wire.right.sourceFrameId == 0 and wire.left.sourceFrameId == 1
|
||||
assert wire.right.validUntilBootTime == first.left_eof_boot_ns + 3_000_000_000
|
||||
|
||||
clock[0] += 1_100_000_000
|
||||
messages.refresh()
|
||||
offset[0] += 9_000_000_000 # suspend/resume clock-pair discontinuity
|
||||
client.add(clock[0] + offset[0], 2)
|
||||
assert runtime.tick() is None
|
||||
assert events[-1].status == "stale" and not events[-1].display_left and not events[-1].display_right
|
||||
|
||||
|
||||
def test_authority_or_saved_doc_change_during_inference_denies_output(tmp_path, monkeypatch):
|
||||
from opendbc.car.structs import car
|
||||
|
||||
annotation = decode_annotation(b'{"version":1,"width":32,"height":32,"poly_left":[[0,0],[32,0],[32,32],[0,32]],"poly_right":[]}')
|
||||
params = Params(str(tmp_path / "params"))
|
||||
path = Path(params.get_param_path("VASMPreferences"))
|
||||
path.write_bytes(encode(Preferences(True, annotation)))
|
||||
clock = [1_000_000_000]
|
||||
messages = Messages(clock)
|
||||
client = Client()
|
||||
client.add(2_000_000_000, 0)
|
||||
events = []
|
||||
monkeypatch.setenv("STARPILOT_VASM_DEVELOPMENT", "1")
|
||||
|
||||
class ChangingModel(Model):
|
||||
def infer_nv12(self, frame, **kwargs):
|
||||
messages.values["carState"].gearShifter = car.CarState.GearShifter.park
|
||||
return super().infer_nv12(frame, **kwargs)
|
||||
|
||||
runtime = VASMRuntime(params, messages, lambda: client, events.append, model_path=Path("/external/model.onnx"),
|
||||
mono_clock=lambda: clock[0], pair_clock=lambda: (clock[0], clock[0] + 1_000_000_000),
|
||||
model_factory=lambda _: ChangingModel())
|
||||
assert runtime.tick() is None
|
||||
assert all(event.status != "valid" for event in events)
|
||||
|
||||
messages.values["carState"].gearShifter = car.CarState.GearShifter.drive
|
||||
client.add(2_000_000_100, 1)
|
||||
|
||||
class SavingModel(Model):
|
||||
def infer_nv12(self, frame, **kwargs):
|
||||
path.write_bytes(encode(Preferences(False, annotation)))
|
||||
return super().infer_nv12(frame, **kwargs)
|
||||
|
||||
runtime.model = SavingModel()
|
||||
assert runtime.tick() is None
|
||||
assert all(event.status != "valid" for event in events)
|
||||
|
||||
|
||||
def test_reconnect_rejects_queued_preconnection_frame(tmp_path, monkeypatch):
|
||||
annotation = decode_annotation(b'{"version":1,"width":32,"height":32,"poly_left":[[0,0],[32,0],[32,32],[0,32]],"poly_right":[]}')
|
||||
params = Params(str(tmp_path / "params"))
|
||||
path = Path(params.get_param_path("VASMPreferences"))
|
||||
path.write_bytes(encode(Preferences(True, annotation)))
|
||||
clock = [1_000_000_000]
|
||||
messages = Messages(clock)
|
||||
client = Client()
|
||||
client.add(2_000_000_000, 0)
|
||||
events = []
|
||||
monkeypatch.setenv("STARPILOT_VASM_DEVELOPMENT", "1")
|
||||
runtime = VASMRuntime(params, messages, lambda: client, events.append, model_path=Path("/external/model.onnx"),
|
||||
mono_clock=lambda: clock[0], pair_clock=lambda: (clock[0], clock[0] + 1_000_000_000),
|
||||
model_factory=lambda _: Model())
|
||||
assert runtime.tick() is not None
|
||||
client.connected = False
|
||||
clock[0] += 100_000_000
|
||||
messages.refresh()
|
||||
client.add(2_050_000_000, 1) # queued from before the new connection
|
||||
assert runtime.tick() is None
|
||||
clock[0] += 100_000_000
|
||||
messages.refresh()
|
||||
assert runtime.tick() is None and events[-1].status == "stale"
|
||||
assert all(event.sequence <= 1 for event in events)
|
||||
|
||||
|
||||
def test_model_load_failure_retries_only_after_bounded_backoff(tmp_path, monkeypatch):
|
||||
annotation = decode_annotation(b'{"version":1,"width":32,"height":32,"poly_left":[[0,0],[32,0],[32,32],[0,32]],"poly_right":[]}')
|
||||
params = Params(str(tmp_path / "params"))
|
||||
path = Path(params.get_param_path("VASMPreferences"))
|
||||
path.write_bytes(encode(Preferences(True, annotation)))
|
||||
clock = [1_000_000_000]
|
||||
messages = Messages(clock)
|
||||
attempts = []
|
||||
monkeypatch.setenv("STARPILOT_VASM_DEVELOPMENT", "1")
|
||||
|
||||
class BadModel(Model):
|
||||
def load(self):
|
||||
attempts.append(clock[0])
|
||||
return False
|
||||
|
||||
runtime = VASMRuntime(params, messages, Client, lambda event: None,
|
||||
model_path=Path("/external/model.onnx"), mono_clock=lambda: clock[0],
|
||||
pair_clock=lambda: (clock[0], clock[0] + 1_000_000_000), model_factory=lambda _: BadModel())
|
||||
assert runtime.tick() is None
|
||||
clock[0] += 4_000_000_000
|
||||
messages.refresh()
|
||||
assert runtime.tick() is None
|
||||
assert attempts == [1_000_000_000]
|
||||
clock[0] += 1_000_000_000
|
||||
messages.refresh()
|
||||
assert runtime.tick() is None
|
||||
assert attempts == [1_000_000_000, 6_000_000_000]
|
||||
|
||||
|
||||
def test_source_timestamp_advancing_during_post_inference_update_is_checked_with_final_clock(tmp_path, monkeypatch):
|
||||
annotation = decode_annotation(b'{"version":1,"width":32,"height":32,"poly_left":[[0,0],[32,0],[32,32],[0,32]],"poly_right":[]}')
|
||||
params = Params(str(tmp_path / "params"))
|
||||
path = Path(params.get_param_path("VASMPreferences"))
|
||||
path.write_bytes(encode(Preferences(True, annotation)))
|
||||
clock = [1_000_000_000]
|
||||
|
||||
class UpdatingMessages(Messages):
|
||||
calls = 0
|
||||
|
||||
def update(self, timeout):
|
||||
super().update(timeout)
|
||||
self.calls += 1
|
||||
if self.calls == 3: # final update after inference receives a newer state
|
||||
clock[0] += 100_000_000
|
||||
self.refresh()
|
||||
|
||||
messages = UpdatingMessages(clock)
|
||||
client = Client()
|
||||
client.add(2_000_000_000, 0)
|
||||
monkeypatch.setenv("STARPILOT_VASM_DEVELOPMENT", "1")
|
||||
runtime = VASMRuntime(params, messages, lambda: client, lambda event: None,
|
||||
model_path=Path("/external/model.onnx"), mono_clock=lambda: clock[0],
|
||||
pair_clock=lambda: (clock[0], clock[0] + 1_000_000_000), model_factory=lambda _: Model())
|
||||
event = runtime.tick()
|
||||
assert event is not None and event.status == "valid"
|
||||
assert event.observed_mono_ns == 1_100_000_000 and event.observed_boot_ns == 2_100_000_000
|
||||
Reference in New Issue
Block a user