From 3d7a0d392bbf5a49f44c07cd71c40eed12b17890 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 10 Nov 2026 23:26:37 +0000 Subject: [PATCH] Preserve STOP confirmation history across expiry gaps --- .../longitudinal/force_stop_runtime.py | 6 +- .../tests/test_force_stop_runtime.py | 33 +++++ .../speed_limits/vision/stop_observation.py | 64 +++++++-- .../vision/tests/test_stop_observation.py | 130 +++++++++++++++++- openpilot/starpilot/ui/runtime_snapshot.py | 8 +- prebuilt.json | 2 +- 6 files changed, 222 insertions(+), 21 deletions(-) diff --git a/openpilot/starpilot/longitudinal/force_stop_runtime.py b/openpilot/starpilot/longitudinal/force_stop_runtime.py index bcbbad11a6..0a0e74fdbc 100644 --- a/openpilot/starpilot/longitudinal/force_stop_runtime.py +++ b/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: diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py b/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py index bf2f75db65..1eec1dd9a2 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py +++ b/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.} diff --git a/openpilot/starpilot/speed_limits/vision/stop_observation.py b/openpilot/starpilot/speed_limits/vision/stop_observation.py index 8d116492d9..dd83748c45 100644 --- a/openpilot/starpilot/speed_limits/vision/stop_observation.py +++ b/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 diff --git a/openpilot/starpilot/speed_limits/vision/tests/test_stop_observation.py b/openpilot/starpilot/speed_limits/vision/tests/test_stop_observation.py index 034708b0c7..a126212894 100644 --- a/openpilot/starpilot/speed_limits/vision/tests/test_stop_observation.py +++ b/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) diff --git a/openpilot/starpilot/ui/runtime_snapshot.py b/openpilot/starpilot/ui/runtime_snapshot.py index e441d3ac48..afadff368d 100644 --- a/openpilot/starpilot/ui/runtime_snapshot.py +++ b/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())): diff --git a/prebuilt.json b/prebuilt.json index 7f27be7f5b..832a54e4fa 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "a177f8228511e430e2cc418ebbd0dd315369ce0b43bee2db6ae5e37727b9f6f6", "schema_version": 1} +{"protected_inventory_sha256": "d052e2e61793a7031dd02bd306cf4ef80477ae4722f68431c8b800f41bdb697d", "schema_version": 1}