StarPilot/openpilot/starpilot/spot_monitor/observation.py

"""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