diff --git a/system/webrtc/device/video.py b/system/webrtc/device/video.py index b46d0222030794..8a07c5b9be9c92 100644 --- a/system/webrtc/device/video.py +++ b/system/webrtc/device/video.py @@ -1,4 +1,5 @@ import asyncio +import struct import time import av @@ -7,6 +8,13 @@ from cereal import messaging from openpilot.common.realtime import DT_MDL, DT_DMON +# arbitrary 16-byte UUID identifying openpilot frame-timing SEI messages +TIMING_SEI_UUID = bytes([ + 0xa5, 0xe0, 0xc4, 0xa4, 0x5b, 0x6e, 0x4e, 0x1e, + 0x9c, 0x7e, 0x12, 0x34, 0x56, 0x78, 0x9a, 0xbc, +]) +_SEI_PREFIX = b'\x00\x00\x00\x01\x06\x05\x30' + TIMING_SEI_UUID + class LiveStreamVideoStreamTrack(TiciVideoStreamTrack): camera_to_sock_mapping = { @@ -22,6 +30,7 @@ def __init__(self, camera_type: str): self._sock = self._make_sock(camera_type) self._pts = 0 self._t0_ns = time.monotonic_ns() + self.timing_sei_enabled = False def _make_sock(self, camera_type: str) -> messaging.SubSocket: return messaging.sub_sock(self.camera_to_sock_mapping[camera_type], conflate=True) @@ -29,6 +38,20 @@ def _make_sock(self, camera_type: str) -> messaging.SubSocket: def switch_camera(self, camera_type: str) -> None: self._sock = self._make_sock(camera_type) + def _build_frame_data(self, msg) -> bytes: + encode_data = getattr(msg, msg.which()) + if not self.timing_sei_enabled: + return encode_data.header + encode_data.data + + idx = encode_data.idx + sei_nal = _SEI_PREFIX + struct.pack('>4d', + (idx.timestampEof - idx.timestampSof) / 1e6, + (msg.logMonoTime - idx.timestampEof) / 1e6, + (time.monotonic_ns() - msg.logMonoTime) / 1e6, + time.time() * 1000, # noqa: TID251 + ) + b'\x80' + return encode_data.header + sei_nal + encode_data.data + async def recv(self): while True: msg = messaging.recv_one_or_none(self._sock) @@ -36,9 +59,7 @@ async def recv(self): break await asyncio.sleep(0.005) - evta = getattr(msg, msg.which()) - - packet = av.Packet(evta.header + evta.data) + packet = av.Packet(self._build_frame_data(msg)) packet.time_base = self._time_base self._pts = ((time.monotonic_ns() - self._t0_ns) * self._clock_rate) // 1_000_000_000 diff --git a/system/webrtc/webrtcd.py b/system/webrtc/webrtcd.py index 1239a1759cdea2..502e6716605680 100755 --- a/system/webrtc/webrtcd.py +++ b/system/webrtc/webrtcd.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 +import time import argparse import asyncio import contextlib @@ -173,12 +174,30 @@ async def get_answer(self): def message_handler(self, message: bytes): assert self.incoming_bridge is not None try: - msg_json = json.loads(message) - if msg_json.get("type") == "livestreamCameraSwitch" and hasattr(self.video_track, "switch_camera"): - self.video_track.switch_camera(msg_json["data"]["camera"]) - return - - if msg_json.get("type") not in self.incoming_bridge_services: + payload = json.loads(message) if isinstance(message, (bytes, str)) else None + if isinstance(payload, dict): + msg_type = payload.get("type") + + if msg_type == "livestreamCameraSwitch": + self.video_track.switch_camera(payload["data"]["camera"]) + return + + if msg_type == "clockSync": + data = payload.get("data", {}) + pong = json.dumps({"type": "clockSync", "data": { + "action": "pong", "browserSendTime": data.get("browserSendTime"), "deviceTime": time.time() * 1000, # noqa: TID251 + }}) + self.stream.get_messaging_channel().send(pong) + return + + if msg_type == "enableTimingSei": + enabled = bool(payload.get("data", {}).get("enabled")) + for track in self.video_tracks: + if hasattr(track, 'timing_sei_enabled'): + track.timing_sei_enabled = enabled + return + + if payload.get("type") not in self.incoming_bridge_services: return self.incoming_bridge.send(message) except Exception: