diff --git a/openpilot/starpilot/spot_monitor/README.md b/openpilot/starpilot/spot_monitor/README.md new file mode 100644 index 0000000000..6b6003523b --- /dev/null +++ b/openpilot/starpilot/spot_monitor/README.md @@ -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. diff --git a/openpilot/starpilot/spot_monitor/__init__.py b/openpilot/starpilot/spot_monitor/__init__.py new file mode 100644 index 0000000000..5e89d27bcf --- /dev/null +++ b/openpilot/starpilot/spot_monitor/__init__.py @@ -0,0 +1 @@ +"""Pure V-ASM annotation, crop and warning decisions.""" diff --git a/openpilot/starpilot/spot_monitor/actions.py b/openpilot/starpilot/spot_monitor/actions.py new file mode 100644 index 0000000000..ca1d100f91 --- /dev/null +++ b/openpilot/starpilot/spot_monitor/actions.py @@ -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_") diff --git a/openpilot/starpilot/spot_monitor/inference.py b/openpilot/starpilot/spot_monitor/inference.py new file mode 100644 index 0000000000..37555fd3d6 --- /dev/null +++ b/openpilot/starpilot/spot_monitor/inference.py @@ -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 diff --git a/openpilot/starpilot/spot_monitor/observation.py b/openpilot/starpilot/spot_monitor/observation.py new file mode 100644 index 0000000000..4da9c7eac1 --- /dev/null +++ b/openpilot/starpilot/spot_monitor/observation.py @@ -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 diff --git a/openpilot/starpilot/spot_monitor/policy.py b/openpilot/starpilot/spot_monitor/policy.py new file mode 100644 index 0000000000..5b1f00e102 --- /dev/null +++ b/openpilot/starpilot/spot_monitor/policy.py @@ -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) diff --git a/openpilot/starpilot/spot_monitor/preferences.py b/openpilot/starpilot/spot_monitor/preferences.py new file mode 100644 index 0000000000..5c7fb1f4e1 --- /dev/null +++ b/openpilot/starpilot/spot_monitor/preferences.py @@ -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 diff --git a/openpilot/starpilot/spot_monitor/runtime.py b/openpilot/starpilot/spot_monitor/runtime.py new file mode 100644 index 0000000000..0b73f78848 --- /dev/null +++ b/openpilot/starpilot/spot_monitor/runtime.py @@ -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() diff --git a/openpilot/starpilot/spot_monitor/tests/__init__.py b/openpilot/starpilot/spot_monitor/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/starpilot/spot_monitor/tests/test_inference.py b/openpilot/starpilot/spot_monitor/tests/test_inference.py new file mode 100644 index 0000000000..88baf53b9b --- /dev/null +++ b/openpilot/starpilot/spot_monitor/tests/test_inference.py @@ -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 diff --git a/openpilot/starpilot/spot_monitor/tests/test_observation.py b/openpilot/starpilot/spot_monitor/tests/test_observation.py new file mode 100644 index 0000000000..bd15bf4be1 --- /dev/null +++ b/openpilot/starpilot/spot_monitor/tests/test_observation.py @@ -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() diff --git a/openpilot/starpilot/spot_monitor/tests/test_policy.py b/openpilot/starpilot/spot_monitor/tests/test_policy.py new file mode 100644 index 0000000000..a2f61c9e4e --- /dev/null +++ b/openpilot/starpilot/spot_monitor/tests/test_policy.py @@ -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 diff --git a/openpilot/starpilot/spot_monitor/tests/test_runtime.py b/openpilot/starpilot/spot_monitor/tests/test_runtime.py new file mode 100644 index 0000000000..fcb56147bf --- /dev/null +++ b/openpilot/starpilot/spot_monitor/tests/test_runtime.py @@ -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