From 43eea2e977f001e30a83f154e9bbfd019de1686e Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 21 Oct 2026 12:30:55 -0500 Subject: [PATCH] Restore Vision Speed Limits --- .../speed_limits/vision/observation.py | 6 ++- .../starpilot/speed_limits/vision/producer.py | 16 +++++--- .../vision/tests/test_producer.py | 24 ++++++++---- .../speed_limits/vision/tests/test_vision.py | 39 ++++++++++++++++++- prebuilt.json | 2 +- 5 files changed, 70 insertions(+), 17 deletions(-) diff --git a/openpilot/starpilot/speed_limits/vision/observation.py b/openpilot/starpilot/speed_limits/vision/observation.py index da8c671abc..aa46dd8487 100644 --- a/openpilot/starpilot/speed_limits/vision/observation.py +++ b/openpilot/starpilot/speed_limits/vision/observation.py @@ -10,7 +10,9 @@ from openpilot.starpilot.speed_limits import selection as sel MAX_RECEIPT_AGE_NS = 1_000_000_000 MAX_FRAME_AGE_NS = 250_000_000 -MAX_OBSERVATION_AGE_NS = 1_000_000_000 +# Bound sign-processing latency independently of raw camera admission. +MAX_RESULT_AGE_NS = 2_000_000_000 +MAX_OBSERVATION_AGE_NS = MAX_RESULT_AGE_NS MAX_PAIR_SKEW_NS = 5_000_000 MODEL_ID = "82408b68c79c269296f0af942130c5383cace4ee06c78e2a4690e8488720116a:07c6696e530eb940d2757d5849b4bc0f1d785cda704e5296e18c0a94959f30a5" @@ -45,7 +47,7 @@ def vision_observation(event, *, receipt_mono_ns: int, now_mono_ns: int, now_boo eof_boot_ns = int(event.cameraFrameEofBootTime) observed_ns = int(event.observedMonoTime) expiry_ns = int(event.validUntilMonoTime) - if (eof_boot_ns <= 0 or not 0 <= now_boot_ns - eof_boot_ns <= MAX_FRAME_AGE_NS or + if (eof_boot_ns <= 0 or not 0 <= now_boot_ns - eof_boot_ns <= MAX_RESULT_AGE_NS or observed_ns <= 0 or not 0 <= now_mono_ns - observed_ns <= MAX_OBSERVATION_AGE_NS or expiry_ns <= observed_ns or now_mono_ns > expiry_ns): return stale diff --git a/openpilot/starpilot/speed_limits/vision/producer.py b/openpilot/starpilot/speed_limits/vision/producer.py index eb4bd84f64..ef755ffcd3 100644 --- a/openpilot/starpilot/speed_limits/vision/producer.py +++ b/openpilot/starpilot/speed_limits/vision/producer.py @@ -10,13 +10,14 @@ import uuid import numpy as np from openpilot.starpilot.speed_limits.vision.model import VisionModelCore, cv2 -from openpilot.starpilot.speed_limits.vision.observation import MODEL_ID, MAX_FRAME_AGE_NS, MAX_PAIR_SKEW_NS, clock_pair_ns +from openpilot.starpilot.speed_limits.vision.observation import MODEL_ID, MAX_FRAME_AGE_NS, MAX_RESULT_AGE_NS, MAX_PAIR_SKEW_NS, clock_pair_ns from openpilot.starpilot.spot_monitor.preferences import enabled as vasm_enabled from openpilot.starpilot.vision_cpu import LITTLE_CORES, VISION_WITH_VASM UNAVAILABLE_RETRY_SECONDS = 1.0 INFERENCE_INTERVAL_NS = 166_666_667 -BUSY_INFERENCE_INTERVAL_NS = 1_500_000_000 +# Retain a cooldown even when processing reaches the result-age budget. +BUSY_INFERENCE_INTERVAL_NS = MAX_RESULT_AGE_NS * 3 // 2 def _configure_process() -> None: @@ -106,7 +107,7 @@ def _send(pm, *, session: str, status: str, stream: str, observed_ns: int = 0, event.confidence = confidence event.supportCount = support event.episode = episode - event.validUntilMonoTime = observed_ns + MAX_FRAME_AGE_NS + event.validUntilMonoTime = observed_ns + MAX_RESULT_AGE_NS pm.send('slcVisionObservation', msg) @@ -235,7 +236,10 @@ def main() -> None: continue last_inference_ns = pair[0] try: - result = core.observe(frame, now=pair[0] / 1e9) + # The observation belongs to the original camera frame, not the end of + # CPU inference. Completion never restamps an older camera observation. + source_observed_ns = eof_boot_ns - offset_ns + result = core.observe(frame, now=source_observed_ns / 1e9) except Exception as error: cloudlog.error('vision SLC inference unavailable: %s', error) _send(pm, session=session, status='unavailable', stream=stream) @@ -253,7 +257,7 @@ def main() -> None: min(BUSY_INFERENCE_INTERVAL_NS, processing_ns * 5 // 2)) if (observed_pair is None or abs((observed_pair[1] - observed_pair[0]) - offset_ns) > MAX_PAIR_SKEW_NS or - not 0 <= observed_pair[1] - eof_boot_ns <= MAX_FRAME_AGE_NS): + not 0 <= observed_pair[1] - eof_boot_ns <= MAX_RESULT_AGE_NS): core.reset() session = uuid.uuid4().hex last_clock_offset_ns = None @@ -267,7 +271,7 @@ def main() -> None: _send(pm, session=session, status='unavailable', stream=stream) continue _send(pm, session=session, status='valid' if result is not None else 'unknown', stream=stream, - observed_ns=observed_pair[0], eof_boot_ns=eof_boot_ns, frame_id=frame_id, result=result) + observed_ns=source_observed_ns, eof_boot_ns=eof_boot_ns, frame_id=frame_id, result=result) except Exception as error: cloudlog.error('vision SLC camera unavailable: %s', error) _send(pm, session=session, status='unavailable', stream=stream) diff --git a/openpilot/starpilot/speed_limits/vision/tests/test_producer.py b/openpilot/starpilot/speed_limits/vision/tests/test_producer.py index c84262b343..70a68b5330 100644 --- a/openpilot/starpilot/speed_limits/vision/tests/test_producer.py +++ b/openpilot/starpilot/speed_limits/vision/tests/test_producer.py @@ -111,15 +111,25 @@ class TestVisionProducer(unittest.TestCase): self.assertEqual(str(messages[0].status), 'unavailable') self.assertEqual(str(messages[0].stream), 'unknown') - def test_slow_inference_remains_stale_after_camera_api_fix(self): + def test_inference_beyond_result_budget_remains_stale(self): _, core, messages = self.run_frame({VisionStreamType.VISION_STREAM_NARROW_ROAD}, - completion_age_ns=270_000_000) + completion_age_ns=2_000_000_001) core.observe.assert_called_once() self.assertEqual(len(messages), 1) self.assertEqual(str(messages[0].status), 'stale') self.assertEqual(messages[0].validUntilMonoTime, 0) self.assertEqual(messages[0].speedMps, 0) + def test_bounded_slow_inference_preserves_original_frame_time(self): + _, core, messages = self.run_frame({VisionStreamType.VISION_STREAM_NARROW_ROAD}, + completion_age_ns=500_000_000) + core.observe.assert_called_once() + self.assertEqual(str(messages[0].status), 'valid') + self.assertEqual(messages[0].observedMonoTime, 1_000_000_000) + self.assertEqual(messages[0].validUntilMonoTime, 3_000_000_000) + self.assertEqual(messages[0].cameraFrameEofBootTime, 5_000_000_000) + self.assertEqual(core.observe.call_args.kwargs['now'], 1.) + def test_unhealthy_driving_model_skips_inference_and_does_not_publish_speed(self): road = {VisionStreamType.VISION_STREAM_NARROW_ROAD} for state in ((False, True, 0, 0), (True, False, 0, 0), @@ -252,13 +262,13 @@ class TestVisionProducer(unittest.TestCase): camera = Mock(return_value=client) road = {VisionStreamType.VISION_STREAM_NARROW_ROAD} camera.available_streams.side_effect = [road, road, road, EndProducer()] - buffers = [(object(), 5_000_000_000, 12), (object(), 5_300_000_000, 13), - (object(), 5_700_000_000, 14)] + buffers = [(object(), 5_000_000_000, 12), (object(), 7_100_000_000, 13), + (object(), 8_600_000_000, 14)] frames = [(np.zeros((2, 2, 3)), item[1], item[2]) for item in (buffers[0], buffers[2])] clocks = [(1_000_000_000, 5_000_000_000), (1_000_000_000, 5_000_000_000), - (1_270_000_000, 5_270_000_000), (1_300_000_000, 5_300_000_000), - (1_700_000_000, 5_700_000_000), (1_700_000_000, 5_700_000_000), - (1_710_000_000, 5_710_000_000)] + (3_010_000_000, 7_010_000_000), (3_100_000_000, 7_100_000_000), + (4_600_000_000, 8_600_000_000), (4_600_000_000, 8_600_000_000), + (4_610_000_000, 8_610_000_000)] messages = [] def send(service, event): diff --git a/openpilot/starpilot/speed_limits/vision/tests/test_vision.py b/openpilot/starpilot/speed_limits/vision/tests/test_vision.py index 3b6e051025..1737079839 100644 --- a/openpilot/starpilot/speed_limits/vision/tests/test_vision.py +++ b/openpilot/starpilot/speed_limits/vision/tests/test_vision.py @@ -107,7 +107,7 @@ class TestVision(unittest.TestCase): self.assertIs(valid.kind, ObservationKind.VALID) self.assertAlmostEqual(valid.candidate.speed_mps, 24.5872, places=4) self.assertIs(vision_observation(decoded, receipt_mono_ns=1_020_000_000, now_mono_ns=1_030_000_000, - now_boot_ns=5_260_000_000, car_fingerprint='IONIQ6', + now_boot_ns=7_000_000_001, car_fingerprint='IONIQ6', drive_session='drive-a').kind, ObservationKind.STALE) self.assertIs(vision_observation(decoded, receipt_mono_ns=1_020_000_000, now_mono_ns=1_300_000_000, now_boot_ns=5_050_000_000, car_fingerprint='IONIQ6', @@ -151,6 +151,43 @@ class TestVision(unittest.TestCase): ObservationKind.UNKNOWN) self.assertIs(runtime.vision_diagnostic.kind, ObservationKind.STALE) + def test_slow_vision_result_uses_original_frame_deadline(self): + message = self._event() + event = message.slcVisionObservation.vision + event.validUntilMonoTime = 3_000_000_000 + decoded = messaging.log_from_bytes(message.to_bytes()).slcVisionObservation.vision + kw = {'receipt_mono_ns': 1_500_000_000, 'now_mono_ns': 1_500_000_000, + 'now_boot_ns': 5_500_000_000, 'car_fingerprint': 'IONIQ6', 'drive_session': 'drive-a'} + self.assertIs(vision_observation(decoded, **kw).kind, ObservationKind.VALID) + self.assertIs(vision_observation(decoded, **dict(kw, now_mono_ns=3_000_000_001, + now_boot_ns=7_000_000_001)).kind, ObservationKind.STALE) + + def test_runtime_slow_vision_supports_display_and_control(self): + message = self._event() + message.slcVisionObservation.vision.validUntilMonoTime = 3_000_000_000 + wire = messaging.log_from_bytes(message.to_bytes()).slcVisionObservation + + class SM: + valid = {'slcVisionObservation': True, 'carState': False} + alive = {'slcVisionObservation': True, 'carState': False} + logMonoTime = {'slcVisionObservation': 1_500_000_000, 'carState': 0} + + def __getitem__(self, _name): + return wire + + for control in (False, True): + with self.subTest(control=control): + settings = parse({'SpeedLimitController': control, 'ShowSpeedLimits': True, + 'SLCPriority1': 'Vision', 'SLCPriority2': 'Dashboard'}) + runtime = Runtime(settings, session_id='drive-a', vision_enabled=True, + vision_control_qualified=control, vision_display_qualified=True) + observations = runtime._observations(SM(), SimpleNamespace(carFingerprint='IONIQ6'), + 1_500_000_000, 5_500_000_000) + self.assertIs(observations[Source.VISION].kind, + ObservationKind.VALID) + selected = select_limit(observations, settings.selection) + self.assertEqual(selected.selected_source, Source.VISION) + def test_native_shared_source_choice_is_dev_and_parked_guarded(self): with tempfile.TemporaryDirectory() as root: params = Params(root) diff --git a/prebuilt.json b/prebuilt.json index c68de41d2f..da58fece10 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "4ab4cf8690be0c8942d43a2a34275df0870a9af30d4dcd0d12f70ac0637478b4", "schema_version": 1} +{"protected_inventory_sha256": "accbfb22312328a90cb22906acefde416d9eb24121e049a21d5283529531ed68", "schema_version": 1}