mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-10-28 00:53:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Restore Vision Speed Limits

Browse Source
This commit is contained in:
firestar5683
2026-10-21 12:30:55 -05:00
parent 3c687b55f7
commit 43eea2e977
5 changed files with 70 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/starpilot/speed_limits/vision/observation.py
+4 -2
View File
@@ -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
openpilot/starpilot/speed_limits/vision/producer.py
+10 -6
View File
@@ -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)
openpilot/starpilot/speed_limits/vision/tests/test_producer.py
+17 -7
View File
@@ -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):
openpilot/starpilot/speed_limits/vision/tests/test_vision.py
+38 -1
View File
@@ -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)
prebuilt.json
+1 -1
View File
@@ -1 +1 @@
{"protected_inventory_sha256": "4ab4cf8690be0c8942d43a2a34275df0870a9af30d4dcd0d12f70ac0637478b4", "schema_version": 1}
{"protected_inventory_sha256": "accbfb22312328a90cb22906acefde416d9eb24121e049a21d5283529531ed68", "schema_version": 1}