Preserve STOP confirmation history across expiry gaps

openpilot/starpilot/longitudinal/force_stop_runtime.py

@@ -15,7 +15,7 @@ from openpilot.starpilot.model_geometry import road_curvature, standstill_clear_
 from openpilot.starpilot.saved_source import read_saved
 from openpilot.starpilot.longitudinal.stop_resume import StopResume
 from openpilot.starpilot.longitudinal.ioniq6_start import eligible as ioniq6_long_eligible
-from openpilot.starpilot.speed_limits.vision.stop_observation import CONTROL_STOP_MAX_AGE_NS, StopConfirmation, read_stop_evidence
+from openpilot.starpilot.speed_limits.vision.stop_observation import CONTROL_STOP_MAX_AGE_NS, StopConfirmation
 
 
 KEY = 'ForceStops'
@@ -186,8 +186,8 @@ class ForceStopRuntime:
         self.policy.reset()
         self.plan = StopPlan()
         return self.plan
-      vision_sign = self.vision_stop.observe(
-        read_stop_evidence(sm, now_ns=now_ns, now_boot_ns=now_boot_ns, drive_id=drive_id), now_ns=now_ns)
+      vision_sign = self.vision_stop.observe_message(
+        sm, now_ns=now_ns, now_boot_ns=now_boot_ns, drive_id=drive_id)
       vision_fresh = bool(vision_sign and 0 <= now_ns - vision_sign <= CONTROL_STOP_MAX_AGE_NS)
       vision_distance = None
       if vision_fresh and vision_sign != self.vision_ranged_ns and self.vision_stop.distance_m is not None:

openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py

@@ -62,6 +62,39 @@ class ForceStopRuntimeTests(unittest.TestCase):
     self.assertTrue(output.forcing)
     return output
 
+  def test_vision_expiry_gap_preserves_confirmation_but_not_control_authority(self):
+    from unittest.mock import patch
+    from openpilot.starpilot.speed_limits.vision.tests.test_stop_observation import evidence, message
+
+    def vision_frame(tick, sample, *, published=None):
+      sm = Frame(tick)
+      event = message(sample, now=sm.stamp if published is None else published)
+      service = 'slcVisionObservation'
+      sm[service] = event.slcVisionObservation
+      sm.seen = {service: True}
+      sm.recv_frame = {service: tick}
+      sm.valid[service] = sm.alive[service] = True
+      sm.logMonoTime[service] = int(event.logMonoTime)
+      sm.recv_time[service] = sm.stamp / 1e9
+      return sm
+
+    self.sample(Frame(0))
+    first = evidence(BASE + 100_000_000, frame=3)
+    second = evidence(BASE + 1_200_000_000, frame=25)
+    with patch.object(self.owner.policy, 'step', wraps=self.owner.policy.step) as step:
+      self.sample(vision_frame(10, first))
+      self.assertFalse(step.call_args.args[0].sign)
+      self.sample(vision_frame(33, first, published=BASE + 1_200_000_000))
+      self.assertFalse(step.call_args.args[0].sign)
+      self.assertEqual(self.owner.vision_stop.previous.observed_ns, first.observed_ns)
+      self.assertIsNone(self.owner.vision_stop.distance_m)
+      self.sample(vision_frame(34, second))
+      self.assertTrue(step.call_args.args[0].sign)
+      self.assertEqual(self.owner.vision_stop.confirmed_ns, second.observed_ns)
+      self.sample(vision_frame(41, second))
+      self.assertEqual(self.owner.vision_stop.confirmed_ns, second.observed_ns)
+      self.assertFalse(step.call_args.args[0].sign)  # The unchanged 750 ms control age is stricter.
+
   def test_wheel_priority_requires_a_stop_cap_or_possible_commit_next_frame(self):
     tune = vehicle_tune(str(self.cp.carFingerprint))
     sign = {'light': False, 'sign': True, 'horizon_m': 40.}

openpilot/starpilot/speed_limits/vision/stop_observation.py

@@ -6,6 +6,8 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.starpilot.speed_limits.vision.observation import MODEL_ID, MAX_PAIR_SKEW_NS
 
 STOP_MAX_AGE_NS = 1_500_000_000
+# One maximum source gap plus the next sample's fresh arrival window.
+STOP_HISTORY_MAX_AGE_NS = 2 * STOP_MAX_AGE_NS
 CONTROL_STOP_MAX_AGE_NS = 750_000_000
 RECEIPT_MAX_AGE_NS = 1_000_000_000
 STOP_CONFIRM_CONFIDENCE = .7
@@ -33,42 +35,52 @@ class StopEvidence:
   boxes: tuple[StopBox, ...]
 
 
-def read_stop_evidence(sm, *, now_ns: int, now_boot_ns: int, drive_id: int,
-                       after_frame: int = -1) -> StopEvidence | None:
+def _read_stop_evidence(sm, *, now_ns: int, now_boot_ns: int, drive_id: int,
+                        after_frame: int = -1) -> tuple[StopEvidence | None, bool, int | None]:
   try:
     service = 'slcVisionObservation'
     if (drive_id <= 0 or not sm.seen[service] or not sm.valid[service] or not sm.alive[service] or
         int(sm.recv_frame[service]) <= after_frame):
-      return None
+      return None, False, None
     published = int(sm.logMonoTime[service])
     receipt = int(sm.recv_time[service] * 1e9)
     if not (drive_id < published <= now_ns and now_ns - published <= RECEIPT_MAX_AGE_NS and
             drive_id < receipt <= now_ns and now_ns - receipt <= RECEIPT_MAX_AGE_NS):
-      return None
+      return None, False, None
     event = sm[service].vision
     sample = event.stopSigns
     session, stream = str(event.producerSessionId), str(event.stream)
     observed, eof = int(sample.observedMonoTime), int(sample.cameraFrameEofBootTime)
     expiry = int(sample.validUntilMonoTime)
     width, height, frame = int(sample.imageWidth), int(sample.imageHeight), int(sample.frameId)
-    if (str(event.modelId) != MODEL_ID or not session or len(session) > 64 or stream not in ('road', 'wideRoad') or
+    if (str(event.modelId) != MODEL_ID or str(event.status) not in ('valid', 'unknown') or
+        not session or len(session) > 64 or stream not in ('road', 'wideRoad') or
         str(sample.status) != 'valid' or not drive_id < observed <= published or
-        not 0 <= now_ns - observed < STOP_MAX_AGE_NS or not observed < expiry <= observed + STOP_MAX_AGE_NS or
-        now_ns >= expiry or eof <= 0 or not 0 <= now_boot_ns - eof < STOP_MAX_AGE_NS or
+        not 0 <= now_ns - observed < STOP_HISTORY_MAX_AGE_NS or not observed < expiry <= observed + STOP_MAX_AGE_NS or
+        eof <= 0 or not 0 <= now_boot_ns - eof < STOP_HISTORY_MAX_AGE_NS or
         abs((now_boot_ns - eof) - (now_ns - observed)) > MAX_PAIR_SKEW_NS or
         not 1 <= width <= 8192 or not 1 <= height <= 8192 or not 0 <= frame <= 0xffffffff or
         len(sample.detections) > 4):
-      return None
+      return None, False, None
     boxes = []
     for item in sample.detections:
       confidence = float(item.confidence)
       box = (int(item.x1Pixels), int(item.y1Pixels), int(item.x2Pixels), int(item.y2Pixels))
       if not math.isfinite(confidence) or not .6 <= confidence <= 1 or not (0 <= box[0] < box[2] <= width and 0 <= box[1] < box[3] <= height):
-        return None
+        return None, False, None
       boxes.append(StopBox(confidence, box))
-    return StopEvidence(session, stream, observed, eof, frame, width, height, tuple(boxes))
+    expired = (now_ns >= expiry or now_ns - observed >= STOP_MAX_AGE_NS or
+               now_boot_ns - eof >= STOP_MAX_AGE_NS)
+    return StopEvidence(session, stream, observed, eof, frame, width, height, tuple(boxes)), expired, expiry
   except (AttributeError, KeyError, TypeError, ValueError, OverflowError, RuntimeError):
-    return None
+    return None, False, None
+
+
+def read_stop_evidence(sm, *, now_ns: int, now_boot_ns: int, drive_id: int,
+                       after_frame: int = -1) -> StopEvidence | None:
+  sample, expired, _ = _read_stop_evidence(sm, now_ns=now_ns, now_boot_ns=now_boot_ns,
+                                         drive_id=drive_id, after_frame=after_frame)
+  return None if expired else sample
 
 
 def stop_distance(sample: StopEvidence, box: StopBox) -> float | None:
@@ -99,8 +111,36 @@ class StopConfirmation:
     self.confirmed_ns = 0
     self.first_seen_ns = 0
     self.distance_m: float | None = None
+    self._drive_id: int | None = None
+    self._previous_expiry_ns: int | None = None
 
-  def observe(self, sample: StopEvidence | None, *, now_ns: int) -> int:
+  def observe_message(self, sm, *, now_ns: int, now_boot_ns: int, drive_id: int,
+                      after_frame: int = -1) -> int:
+    if self._drive_id != drive_id:
+      self.reset()
+    sample, expired, expiry = _read_stop_evidence(
+      sm, now_ns=now_ns, now_boot_ns=now_boot_ns, drive_id=drive_id, after_frame=after_frame)
+    previous = self.previous
+    if (sample is not None and previous is not None and
+        (sample.session, sample.stream, sample.observed_ns, sample.frame_id) ==
+        (previous.session, previous.stream, previous.observed_ns, previous.frame_id) and
+        self._previous_expiry_ns is not None and expiry != self._previous_expiry_ns):
+      self.reset()
+      return 0
+    confirmed = self.observe(sample, now_ns=now_ns, expiry_only=expired)
+    if self.previous is not None:
+      self._drive_id = drive_id
+      self._previous_expiry_ns = expiry
+    return confirmed
+
+  def observe(self, sample: StopEvidence | None, *, now_ns: int, expiry_only: bool = False) -> int:
+    if expiry_only:
+      if sample is not None and sample == self.previous and 0 <= now_ns - sample.observed_ns < STOP_HISTORY_MAX_AGE_NS:
+        self.confirmed_ns = 0
+        self.distance_m = None
+        return 0
+      self.reset()
+      return 0
     if sample is None or not 0 <= now_ns - sample.observed_ns < STOP_MAX_AGE_NS:
       self.reset()
       return 0

openpilot/starpilot/speed_limits/vision/tests/test_stop_observation.py

@@ -6,7 +6,7 @@ from openpilot.cereal import messaging
 from openpilot.starpilot.speed_limits.vision.observation import MODEL_ID
 from openpilot.starpilot.speed_limits.vision.producer import _send
 from openpilot.starpilot.speed_limits.vision.stop_observation import (
-  StopBox, StopConfirmation, StopEvidence, STOP_MAX_AGE_NS, read_stop_evidence,
+  StopBox, StopConfirmation, StopEvidence, STOP_HISTORY_MAX_AGE_NS, STOP_MAX_AGE_NS, read_stop_evidence,
 )
 
 BASE = 100_000_000_000
@@ -147,3 +147,131 @@ class TestStopObservation(unittest.TestCase):
       sample = evidence(BASE + 1_000_000_000 + i * 200_000_000, frame=21 + i * 4)
       output = owner.observe(sample, now_ns=sample.observed_ns)
     self.assertEqual(output, sample.observed_ns)
+
+  def observe_message(self, owner, sm, now, *, drive=DRIVE, after_frame=-1):
+    return owner.observe_message(sm, now_ns=now, now_boot_ns=now + OFFSET,
+                                 drive_id=drive, after_frame=after_frame)
+
+  def test_expiry_gap_keeps_history_but_never_publishes_expired_evidence(self):
+    for gap in (20_000_000, 80_000_000, 150_000_000, 300_000_000):
+      with self.subTest(gap=gap):
+        owner = StopConfirmation()
+        first = evidence()
+        self.assertEqual(self.observe_message(owner, Input(message(first)), BASE + 400_000_000), 0)
+        held = Input(message(first, now=BASE + 1_100_000_000))
+        now = BASE + STOP_MAX_AGE_NS + gap
+        self.assertIsNone(self.read(held, now))
+        self.assertEqual(self.observe_message(owner, held, now), 0)
+        self.assertEqual(owner.previous, first)
+        self.assertIsNone(owner.distance_m)
+        second = evidence(BASE + 1_100_000_000, frame=23)
+        self.assertEqual(self.observe_message(owner, Input(message(second, now=now)), now), second.observed_ns)
+
+  def test_expired_confirmed_output_and_distance_clear_until_a_new_source(self):
+    owner = StopConfirmation()
+    first = evidence(width=1928, height=1208)
+    second = evidence(BASE + 600_000_000, frame=13, width=1928, height=1208)
+    self.observe_message(owner, Input(message(first)), BASE + 400_000_000)
+    self.assertEqual(self.observe_message(owner, Input(message(second)), BASE + 1_000_000_000), second.observed_ns)
+    self.assertIsNotNone(owner.distance_m)
+    held = Input(message(second, now=BASE + 1_700_000_000))
+    for now in (BASE + 2_150_000_000, BASE + 2_200_000_000):
+      self.assertEqual(self.observe_message(owner, held, now), 0)
+      self.assertEqual(owner.confirmed_ns, 0)
+      self.assertIsNone(owner.distance_m)
+    third = evidence(BASE + 1_700_000_000, frame=35, width=1928, height=1208)
+    self.assertEqual(self.observe_message(owner, Input(message(third, now=BASE + 2_250_000_000)), BASE + 2_250_000_000), third.observed_ns)
+
+  def test_expired_sources_cannot_seed_or_extend_unbounded_history(self):
+    owner = StopConfirmation()
+    first = evidence()
+    now = BASE + STOP_MAX_AGE_NS + 50_000_000
+    expired = Input(message(first, now=now))
+    self.assertEqual(self.observe_message(owner, expired, now), 0)
+    self.assertIsNone(owner.previous)
+    self.observe_message(owner, Input(message(first)), BASE + 400_000_000)
+    now = BASE + STOP_HISTORY_MAX_AGE_NS
+    self.assertEqual(self.observe_message(owner, Input(message(first, now=now)), now), 0)
+    self.assertIsNone(owner.previous)
+    next_source = evidence(BASE + STOP_HISTORY_MAX_AGE_NS, frame=61)
+    self.assertEqual(self.observe_message(owner, Input(message(next_source)), now + 400_000_000), 0)
+
+  def test_invalid_expired_payloads_still_clear_all_history(self):
+    defects = ('unseen', 'invalid', 'dead', 'receipt', 'publication', 'model', 'outer_status',
+               'status', 'session', 'stream', 'dimensions', 'eof', 'future_eof', 'payload', 'expiry', 'drive', 'old_frame')
+    for defect in defects:
+      with self.subTest(defect=defect):
+        owner = StopConfirmation()
+        first = evidence()
+        self.observe_message(owner, Input(message(first)), BASE + 400_000_000)
+        held = message(first, now=BASE + 1_100_000_000).as_builder()
+        event = held.slcVisionObservation.vision
+        if defect == 'model':
+          event.modelId = 'wrong'
+        elif defect == 'outer_status':
+          event.status = 'unavailable'
+        elif defect == 'status':
+          event.stopSigns.status = 'unavailable'
+        elif defect == 'session':
+          event.producerSessionId = 'new'
+        elif defect == 'stream':
+          event.stream = 'wideRoad'
+        elif defect == 'dimensions':
+          event.stopSigns.imageWidth = 1400
+        elif defect == 'eof':
+          event.stopSigns.cameraFrameEofBootTime += 10_000_000
+        elif defect == 'future_eof':
+          event.stopSigns.cameraFrameEofBootTime = BASE + OFFSET + 9_000_000_000
+        elif defect == 'payload':
+          event.stopSigns.detections[0].confidence = .9
+        elif defect == 'expiry':
+          event.stopSigns.validUntilMonoTime -= 1
+        sm = Input(held)
+        now = BASE + STOP_MAX_AGE_NS + 50_000_000
+        if defect == 'unseen':
+          sm.seen['slcVisionObservation'] = False
+        elif defect == 'invalid':
+          sm.valid['slcVisionObservation'] = False
+        elif defect == 'dead':
+          sm.alive['slcVisionObservation'] = False
+        elif defect == 'receipt':
+          sm.recv_time['slcVisionObservation'] = (now - 1_000_000_001) / 1e9
+        elif defect == 'publication':
+          sm.logMonoTime['slcVisionObservation'] = now + 1
+        self.assertEqual(self.observe_message(owner, sm, now, drive=DRIVE + (defect == 'drive'),
+                                             after_frame=2 if defect == 'old_frame' else -1), 0)
+        self.assertIsNone(owner.previous)
+        next_source = evidence(BASE + 1_100_000_000, frame=23)
+        self.assertEqual(self.observe_message(owner, Input(message(next_source, now=now)), now), 0)
+
+  def test_fresh_negative_and_context_changes_do_not_bridge_expiry_history(self):
+    for change in ('negative', 'session', 'stream', 'dimensions', 'reorder'):
+      with self.subTest(change=change):
+        owner = StopConfirmation()
+        first = evidence()
+        self.observe_message(owner, Input(message(first)), BASE + 400_000_000)
+        held = Input(message(first, now=BASE + 1_100_000_000))
+        self.observe_message(owner, held, BASE + 1_550_000_000)
+        second = evidence(BASE + 1_100_000_000, frame=23)
+        if change == 'negative':
+          second = replace(second, boxes=())
+        elif change == 'session':
+          second = replace(second, session='new')
+        elif change == 'stream':
+          second = replace(second, stream='wideRoad')
+        elif change == 'dimensions':
+          second = replace(second, width=1400)
+        else:
+          second = replace(second, frame_id=0)
+        self.assertEqual(self.observe_message(owner, Input(message(second, now=BASE + 1_600_000_000)), BASE + 1_600_000_000), 0)
+
+  def test_heartbeat_low_confidence_and_short_source_gap_never_create_confirmation(self):
+    for confidence, source_gap in ((.69, 1_100_000_000), (.875, 400_000_000)):
+      with self.subTest(confidence=confidence, source_gap=source_gap):
+        owner = StopConfirmation()
+        first = evidence(boxes=(replace(BOX, confidence=confidence),))
+        self.observe_message(owner, Input(message(first)), BASE + 400_000_000)
+        self.assertEqual(self.observe_message(owner, Input(message(first, now=BASE + 800_000_000)),
+                                              BASE + 800_000_000), 0)
+        second = evidence(BASE + source_gap, frame=23, boxes=(replace(BOX, confidence=confidence),))
+        self.assertEqual(self.observe_message(owner, Input(message(second)), second.observed_ns + 400_000_000), 0)

openpilot/starpilot/ui/runtime_snapshot.py

@@ -47,7 +47,7 @@ from openpilot.starpilot.ui.onroad_state import (AlertSize, BorderSignals, Obser
 from openpilot.starpilot.ui.unified_speed_presentation import (displayed_adjusted_limit_mps, displayed_limit_mps,
                                                               displayed_limit_source, positive, retaining_limit)
 from openpilot.starpilot.ui.onroad_stopped_timer import StoppedTimer
-from openpilot.starpilot.speed_limits.vision.stop_observation import StopConfirmation, read_stop_evidence
+from openpilot.starpilot.speed_limits.vision.stop_observation import StopConfirmation
 from openpilot.starpilot.ui.onroad_lane_alerts import lateral_lane_alert
 from openpilot.starpilot.ui.onroad_camera import ReverseDriverCamera
 from openpilot.starpilot.ui.navigation_state import navigation_display, official_navigation_display
@@ -702,9 +702,9 @@ class RuntimeSnapshotAdapter:
       torque=current_message(sm, 'lateralTorqueParameters', now_ns, after_frame=after) if started else None,
       parameters=display_message(sm, 'vehicleParameters', now_ns, after_frame=after) if started else None,
       metric=bool(ui.is_metric), cp=cp)
-    stop_evidence = (read_stop_evidence(sm, now_ns=now_ns, now_boot_ns=now_ns + pair[2],
-                                      drive_id=drive_id, after_frame=after) if started and pair is not None else None)
-    stop_observed_ns = self._vision_stop.observe(stop_evidence, now_ns=now_ns)
+    stop_observed_ns = (self._vision_stop.observe_message(
+      sm, now_ns=now_ns, now_boot_ns=now_ns + pair[2], drive_id=drive_id, after_frame=after)
+      if started and pair is not None else self._vision_stop.observe(None, now_ns=now_ns))
     weather_location = None
     if (started and getattr(ui, 'replay_clock', None) is None and
         any(layout.get('weather', {}).get('enabled', False) for layout in self._customization_value['layouts'].values())):

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "a177f8228511e430e2cc418ebbd0dd315369ce0b43bee2db6ae5e37727b9f6f6", "schema_version": 1}
+{"protected_inventory_sha256": "d052e2e61793a7031dd02bd306cf4ef80477ae4722f68431c8b800f41bdb697d", "schema_version": 1}