IQ.Pilot Prebuilt Release @ 27f668a
This commit is contained in:
IQ.Lvbs CI [bot]
135
iqpilot/system/webrtc/device/native_audio.py
Normal file
135
iqpilot/system/webrtc/device/native_audio.py
Normal file
@@ -0,0 +1,135 @@
|
||||
import asyncio
|
||||
import contextlib
|
||||
from collections import deque
|
||||
from fractions import Fraction
|
||||
|
||||
import av
|
||||
|
||||
from iqpilot.cereal import messaging
|
||||
from iqpilot.selfdrive.ui.soundd import SAMPLE_RATE as SOUND_SAMPLE_RATE
|
||||
|
||||
|
||||
WEBRTC_AUDIO_SERVICE = "webrtcAudioData"
|
||||
WEBRTC_AUDIO_PTIME = 0.020
|
||||
|
||||
|
||||
class AudioInputOpusProducer:
|
||||
def __init__(self) -> None:
|
||||
self._sock = messaging.sub_sock("rawAudioData", conflate=False)
|
||||
self._pcm = bytearray()
|
||||
self._source_rate = 16_000
|
||||
self._next_pts = 0
|
||||
self._packet_pts = 0
|
||||
self._pending: deque[tuple[bytes, int]] = deque()
|
||||
self._enabled = True
|
||||
self._resampler = av.AudioResampler(format="fltp", layout="mono", rate=48_000)
|
||||
self._encoder = av.CodecContext.create("libopus", "w")
|
||||
self._encoder.sample_rate = 48_000
|
||||
self._encoder.layout = "mono"
|
||||
self._encoder.format = "fltp"
|
||||
self._encoder.open()
|
||||
|
||||
def enable(self, enabled: bool) -> None:
|
||||
self._enabled = enabled
|
||||
|
||||
async def _read_pcm_frame(self) -> av.AudioFrame:
|
||||
while True:
|
||||
samples = max(1, int(WEBRTC_AUDIO_PTIME * self._source_rate))
|
||||
target_bytes = samples * 2
|
||||
while len(self._pcm) < target_bytes:
|
||||
msg = messaging.recv_one_or_none(self._sock)
|
||||
if msg is None:
|
||||
await asyncio.sleep(0.002)
|
||||
continue
|
||||
audio = msg.rawAudioData
|
||||
rate = int(audio.sampleRate) or self._source_rate
|
||||
if rate != self._source_rate:
|
||||
self._source_rate = rate
|
||||
self._pcm.clear()
|
||||
continue
|
||||
self._pcm.extend(bytes(audio.data))
|
||||
|
||||
data = bytes(self._pcm[:target_bytes])
|
||||
del self._pcm[:target_bytes]
|
||||
frame = av.AudioFrame(format="s16", layout="mono", samples=samples)
|
||||
frame.planes[0].update(data)
|
||||
frame.sample_rate = self._source_rate
|
||||
return frame
|
||||
|
||||
async def recv(self) -> tuple[bytes, int] | None:
|
||||
if not self._enabled:
|
||||
await asyncio.sleep(WEBRTC_AUDIO_PTIME)
|
||||
return None
|
||||
while not self._pending:
|
||||
source_frame = await self._read_pcm_frame()
|
||||
for frame in self._resampler.resample(source_frame):
|
||||
frame.pts = self._next_pts
|
||||
frame.time_base = Fraction(1, 48_000)
|
||||
self._next_pts += frame.samples
|
||||
for packet in self._encoder.encode(frame):
|
||||
self._pending.append((bytes(packet), self._packet_pts))
|
||||
self._packet_pts += int(packet.duration or frame.samples)
|
||||
return self._pending.popleft()
|
||||
|
||||
|
||||
class DebugAudioOpusProducer(AudioInputOpusProducer):
|
||||
async def _read_pcm_frame(self) -> av.AudioFrame:
|
||||
samples = int(WEBRTC_AUDIO_PTIME * self._source_rate)
|
||||
await asyncio.sleep(WEBRTC_AUDIO_PTIME)
|
||||
frame = av.AudioFrame(format="s16", layout="mono", samples=samples)
|
||||
frame.planes[0].update(bytes(samples * 2))
|
||||
frame.sample_rate = self._source_rate
|
||||
return frame
|
||||
|
||||
|
||||
class IncomingOpusCerealProxy:
|
||||
def __init__(self, track) -> None:
|
||||
self._loop = asyncio.get_running_loop()
|
||||
self._queue: asyncio.Queue[bytes] = asyncio.Queue(maxsize=32)
|
||||
self._pm = messaging.PubMaster([WEBRTC_AUDIO_SERVICE])
|
||||
self._decoder = av.CodecContext.create("opus", "r")
|
||||
self._resampler = av.AudioResampler(format="s16", layout="mono", rate=SOUND_SAMPLE_RATE)
|
||||
self._task: asyncio.Task | None = None
|
||||
track.on_frame(self._on_frame)
|
||||
|
||||
def _on_frame(self, payload: bytes, _info) -> None:
|
||||
def enqueue() -> None:
|
||||
if self._queue.full():
|
||||
with contextlib.suppress(asyncio.QueueEmpty):
|
||||
self._queue.get_nowait()
|
||||
self._queue.put_nowait(bytes(payload))
|
||||
self._loop.call_soon_threadsafe(enqueue)
|
||||
|
||||
def start(self) -> None:
|
||||
if self._task is None:
|
||||
self._task = asyncio.create_task(self.run())
|
||||
|
||||
async def stop(self) -> None:
|
||||
if self._task is None:
|
||||
return
|
||||
self._task.cancel()
|
||||
try:
|
||||
await self._task
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
self._task = None
|
||||
|
||||
def _publish(self, frame: av.AudioFrame) -> None:
|
||||
data = frame.to_ndarray().tobytes()
|
||||
if not data:
|
||||
return
|
||||
msg = messaging.new_message(WEBRTC_AUDIO_SERVICE, valid=True)
|
||||
msg.webrtcAudioData.data = data
|
||||
msg.webrtcAudioData.sampleRate = frame.sample_rate
|
||||
self._pm.send(WEBRTC_AUDIO_SERVICE, msg)
|
||||
|
||||
async def run(self) -> None:
|
||||
while True:
|
||||
payload = await self._queue.get()
|
||||
try:
|
||||
for decoded in self._decoder.decode(av.Packet(payload)):
|
||||
for frame in self._resampler.resample(decoded):
|
||||
self._publish(frame)
|
||||
except Exception:
|
||||
# A malformed or stale packet must not end the video/control session.
|
||||
continue
|
||||
311
iqpilot/system/webrtc/device/native_video.py
Normal file
311
iqpilot/system/webrtc/device/native_video.py
Normal file
@@ -0,0 +1,311 @@
|
||||
import asyncio
|
||||
import logging
|
||||
import struct
|
||||
import time
|
||||
|
||||
import av
|
||||
from iqpilot.system.webrtc.rtc.tracks import TiciVideoStreamTrack
|
||||
|
||||
from iqpilot.cereal import messaging
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.realtime import DT_MDL, DT_DMON
|
||||
|
||||
# Arbitrary 16-byte UUID identifying konn3kt frame-timing SEI messages. When timing
|
||||
# telemetry is enabled, each frame carries a user_data_unregistered SEI NAL with four
|
||||
# big-endian doubles (ms): encode duration, IPC/queue delay, host transit, and the
|
||||
# device wall clock. The client decodes these to compute true glass-to-glass latency.
|
||||
TIMING_SEI_UUID = bytes([
|
||||
0xa5, 0xe0, 0xc4, 0xa4, 0x5b, 0x6e, 0x4e, 0x1e,
|
||||
0x9c, 0x7e, 0x12, 0x34, 0x56, 0x78, 0x9a, 0xbc,
|
||||
])
|
||||
# Annex-B start code + SEI NAL (type 6) + user_data_unregistered (type 5) + payload size
|
||||
# (0x30 = 48 bytes = 16 UUID + 32 data). Trailing 0x80 is the RBSP stop bit.
|
||||
_SEI_PREFIX = b'\x00\x00\x00\x01\x06\x05\x30' + TIMING_SEI_UUID
|
||||
|
||||
|
||||
class LiveStreamVideoStreamTrack(TiciVideoStreamTrack):
|
||||
livestream_camera_to_sock_mapping = {
|
||||
"driver": "livestreamDriverEncodeData",
|
||||
"wideRoad": "livestreamWideRoadEncodeData",
|
||||
"road": "livestreamRoadEncodeData",
|
||||
}
|
||||
main_camera_to_sock_mapping = {
|
||||
"driver": "driverEncodeData",
|
||||
"wideRoad": "wideRoadEncodeData",
|
||||
"road": "roadEncodeData",
|
||||
}
|
||||
|
||||
# Number of live tracks still waiting for their first keyframe. The on-demand
|
||||
# keyframe request (LivestreamRequestKeyframe) is a single global param honored by
|
||||
# every encoder, so with multiple concurrent tracks (dual-camera PiP) we must not
|
||||
# clear it until *all* tracks have received an IDR — otherwise the first track to
|
||||
# get its keyframe clears the request and starves the others (black feed).
|
||||
_kf_pending_count = 0
|
||||
|
||||
def __init__(self, camera_type: str):
|
||||
dt = DT_DMON if camera_type == "driver" else DT_MDL
|
||||
super().__init__(camera_type, dt)
|
||||
|
||||
self._params = Params()
|
||||
self._camera_type = camera_type
|
||||
self._candidate_topics = [
|
||||
self.main_camera_to_sock_mapping[camera_type],
|
||||
self.livestream_camera_to_sock_mapping[camera_type],
|
||||
]
|
||||
self._socks = {topic: messaging.sub_sock(topic, conflate=True) for topic in self._candidate_topics}
|
||||
self._active_topic = self._preferred_topics()[0]
|
||||
self._pts = 0
|
||||
self._t0_ns = time.monotonic_ns()
|
||||
self._cached_header: bytes = b""
|
||||
self._sent_keyframe = False
|
||||
self._kf_requested = False # whether this track counts toward _kf_pending_count
|
||||
self._frame_count = 0
|
||||
self._last_frame_time = 0.0
|
||||
self._last_preference_refresh = 0.0
|
||||
# Tracks how long the H264 livestream feed has been silent, to gate the last-resort main-feed
|
||||
# fallback (see recv) without flapping between sources frame-by-frame.
|
||||
self._live_silent_since: float | None = None
|
||||
# Opt-in glass-to-glass latency telemetry (toggled by the client over the data channel).
|
||||
self.timing_sei_enabled = False
|
||||
self._logger = logging.getLogger("LiveStreamVideoStreamTrack")
|
||||
|
||||
# Ask the encoder for an immediate IDR so the stream starts fast instead of waiting up to a full
|
||||
# GOP for the next periodic keyframe (encoderd honors LivestreamRequestKeyframe per-frame).
|
||||
self._mark_keyframe_needed()
|
||||
|
||||
def _request_keyframe(self, enabled: bool) -> None:
|
||||
try:
|
||||
self._params.put_bool("LivestreamRequestKeyframe", enabled)
|
||||
except Exception:
|
||||
self._logger.exception("failed to set LivestreamRequestKeyframe")
|
||||
|
||||
def _mark_keyframe_needed(self) -> None:
|
||||
"""This track needs (another) keyframe: keep the global request asserted."""
|
||||
if not self._kf_requested:
|
||||
LiveStreamVideoStreamTrack._kf_pending_count += 1
|
||||
self._kf_requested = True
|
||||
self._request_keyframe(True)
|
||||
|
||||
def request_keyframe(self) -> None:
|
||||
"""RTCP PLI hook used by libdatachannel's native H.264 packetizer."""
|
||||
self._mark_keyframe_needed()
|
||||
|
||||
def _mark_keyframe_received(self) -> None:
|
||||
"""This track got its keyframe; only clear the global request once no track needs one."""
|
||||
if self._kf_requested:
|
||||
self._kf_requested = False
|
||||
LiveStreamVideoStreamTrack._kf_pending_count = max(0, LiveStreamVideoStreamTrack._kf_pending_count - 1)
|
||||
if LiveStreamVideoStreamTrack._kf_pending_count == 0:
|
||||
self._request_keyframe(False)
|
||||
|
||||
def stop(self):
|
||||
# Release our pending-keyframe hold so a torn-down track that never received an
|
||||
# IDR doesn't pin LivestreamRequestKeyframe True forever (continuous keyframes).
|
||||
if getattr(self, "_kf_requested", False):
|
||||
self._kf_requested = False
|
||||
LiveStreamVideoStreamTrack._kf_pending_count = max(0, LiveStreamVideoStreamTrack._kf_pending_count - 1)
|
||||
try:
|
||||
super().stop()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def switch_camera(self, camera_type: str) -> None:
|
||||
"""Repoint this track at a different camera without renegotiating the peer connection.
|
||||
|
||||
Lets a single video track back the whole Live View — the client flips cameras over the
|
||||
data channel and we swap the source here, instead of uplinking every camera at once."""
|
||||
if camera_type not in self.livestream_camera_to_sock_mapping:
|
||||
self._logger.warning("[%s] ignoring switch to unknown camera %s", self._id, camera_type)
|
||||
return
|
||||
if camera_type == self._camera_type:
|
||||
return
|
||||
self._logger.info("[%s] switching camera %s -> %s", self._id, self._camera_type, camera_type)
|
||||
self._camera_type = camera_type
|
||||
self._candidate_topics = [
|
||||
self.main_camera_to_sock_mapping[camera_type],
|
||||
self.livestream_camera_to_sock_mapping[camera_type],
|
||||
]
|
||||
self._socks = {topic: messaging.sub_sock(topic, conflate=True) for topic in self._candidate_topics}
|
||||
self._active_topic = self._preferred_topics()[0]
|
||||
# Force a fresh keyframe/header before emitting frames from the new source, and ask the encoder
|
||||
# for an immediate IDR so the camera switch isn't stalled waiting for the next periodic keyframe.
|
||||
self._cached_header = b""
|
||||
self._sent_keyframe = False
|
||||
self._last_preference_refresh = 0.0
|
||||
self._live_silent_since = None
|
||||
self._mark_keyframe_needed()
|
||||
|
||||
def _preferred_topics(self) -> list[str]:
|
||||
# WebRTC currently forces H.264. The dedicated livestream topics are the H.264 feeds,
|
||||
# while the main encode topics are the full-resolution HEVC recordings. Prefer the
|
||||
# livestream feeds both onroad and offroad, and keep the main topics only as fallback.
|
||||
return [
|
||||
self.livestream_camera_to_sock_mapping[self._camera_type],
|
||||
self.main_camera_to_sock_mapping[self._camera_type],
|
||||
]
|
||||
|
||||
def _reset_decoder_state(self, topic: str) -> None:
|
||||
if topic == self._active_topic:
|
||||
return
|
||||
self._logger.info("[%s] switching video source from %s to %s", self._id, self._active_topic, topic)
|
||||
self._active_topic = topic
|
||||
self._cached_header = b""
|
||||
self._sent_keyframe = False
|
||||
|
||||
def _timing_sei(self, evta, log_mono_time: int) -> bytes:
|
||||
"""Build a timing SEI NAL from encode metadata, or empty bytes when disabled."""
|
||||
if not self.timing_sei_enabled:
|
||||
return b""
|
||||
idx = evta.idx
|
||||
return _SEI_PREFIX + struct.pack(
|
||||
'>4d',
|
||||
(idx.timestampEof - idx.timestampSof) / 1e6, # encode duration (ms)
|
||||
(log_mono_time - idx.timestampEof) / 1e6, # IPC/queue delay (ms)
|
||||
(time.monotonic_ns() - log_mono_time) / 1e6, # host transit so far (ms)
|
||||
time.time() * 1000, # device wall clock (ms) # noqa: TID251
|
||||
) + b'\x80'
|
||||
|
||||
def _is_keyframe(self, data: bytes) -> bool:
|
||||
"""Check if H.264 NAL unit contains an IDR keyframe (NAL type 5)."""
|
||||
i = 0
|
||||
while i < len(data) - 4:
|
||||
# Look for Annex B start codes: 0x000001 or 0x00000001
|
||||
if data[i:i+3] == b'\x00\x00\x01':
|
||||
nal_type = data[i+3] & 0x1f
|
||||
if nal_type == 5: # IDR slice
|
||||
return True
|
||||
i += 3
|
||||
elif data[i:i+4] == b'\x00\x00\x00\x01':
|
||||
nal_type = data[i+4] & 0x1f
|
||||
if nal_type == 5: # IDR slice
|
||||
return True
|
||||
i += 4
|
||||
else:
|
||||
i += 1
|
||||
return False
|
||||
|
||||
async def recv(self):
|
||||
while True:
|
||||
now = time.monotonic()
|
||||
# Resolve topics each iteration: a camera switch (different async task) can rebuild self._socks
|
||||
# across the await below, so a value cached before the loop would index a stale key (KeyError).
|
||||
live_topic = self.livestream_camera_to_sock_mapping[self._camera_type]
|
||||
main_topic = self.main_camera_to_sock_mapping[self._camera_type]
|
||||
# Lock onto the dedicated H264 livestream feed. Onroad the HEVC main feed also publishes at
|
||||
# 20fps; eagerly preferring whichever socket had a frame ready raced frame-by-frame, reset the
|
||||
# decoder every frame, and (the track is negotiated H264) shoved HEVC garbage into the stream —
|
||||
# the onroad choppiness. Only fall back to the main feed as a last resort after a long
|
||||
# livestream silence (e.g. stream_encoderd still spinning up), and snap back when it returns.
|
||||
msg = messaging.recv_one_or_none(self._socks[live_topic])
|
||||
if msg is not None:
|
||||
self._reset_decoder_state(live_topic)
|
||||
self._last_frame_time = now
|
||||
self._live_silent_since = None
|
||||
break
|
||||
|
||||
if self._live_silent_since is None:
|
||||
self._live_silent_since = now
|
||||
elif now - self._live_silent_since > 3.0:
|
||||
maybe_msg = messaging.recv_one_or_none(self._socks[main_topic])
|
||||
if maybe_msg is not None:
|
||||
self._reset_decoder_state(main_topic)
|
||||
self._last_frame_time = now
|
||||
msg = maybe_msg
|
||||
break
|
||||
|
||||
await asyncio.sleep(0.005)
|
||||
|
||||
evta = getattr(msg, msg.which())
|
||||
|
||||
header = bytes(evta.header)
|
||||
data = bytes(evta.data)
|
||||
self._frame_count += 1
|
||||
|
||||
# Cache SPS/PPS header when it arrives
|
||||
if header:
|
||||
self._cached_header = header
|
||||
self._logger.debug(f"[{self._id}] cached SPS/PPS header ({len(header)} bytes)")
|
||||
|
||||
# CRITICAL: Cannot decode without SPS/PPS. Wait for it.
|
||||
if not self._cached_header:
|
||||
self._logger.debug(f"[{self._id}] frame {self._frame_count}: no SPS/PPS yet, skipping")
|
||||
return await self.recv()
|
||||
|
||||
is_keyframe = self._is_keyframe(data)
|
||||
|
||||
# Wait for first keyframe before sending any frames
|
||||
# Browser decoder needs IDR to initialize properly
|
||||
if not self._sent_keyframe:
|
||||
if not is_keyframe:
|
||||
self._logger.debug(f"[{self._id}] frame {self._frame_count}: waiting for keyframe")
|
||||
return await self.recv()
|
||||
self._sent_keyframe = True
|
||||
# Got the IDR we asked for — stop nagging the encoder, but only once every
|
||||
# concurrent track has its keyframe (multi-track PiP shares the global param).
|
||||
self._mark_keyframe_received()
|
||||
self._logger.info(f"[{self._id}] first keyframe received, starting stream")
|
||||
|
||||
# Optional timing SEI NAL, inserted before the slice data (and after SPS/PPS on keyframes).
|
||||
sei_nal = self._timing_sei(evta, msg.logMonoTime)
|
||||
|
||||
# Prepend SPS/PPS header to keyframes (required by some decoders)
|
||||
# For non-keyframes, header is optional but safe to include
|
||||
if is_keyframe:
|
||||
payload = self._cached_header + sei_nal + data
|
||||
else:
|
||||
payload = sei_nal + data
|
||||
|
||||
self._pts = ((time.monotonic_ns() - self._t0_ns) * self._clock_rate) // 1_000_000_000
|
||||
|
||||
packet = av.Packet(payload)
|
||||
packet.time_base = self._time_base
|
||||
packet.pts = int(self._pts)
|
||||
packet.dts = int(self._pts)
|
||||
packet.duration = int(self._dt * self._clock_rate)
|
||||
|
||||
if is_keyframe:
|
||||
packet.is_keyframe = True
|
||||
|
||||
self.log_debug("track sending frame %s (keyframe=%s, size=%d)", self._pts, is_keyframe, len(payload))
|
||||
|
||||
return packet
|
||||
|
||||
def codec_preference(self) -> str | None:
|
||||
return "H264"
|
||||
|
||||
|
||||
class DebugVideoStreamTrack(TiciVideoStreamTrack):
|
||||
def __init__(self, camera_type: str):
|
||||
super().__init__(camera_type, 0.05)
|
||||
self._codec = av.CodecContext.create("libx264", "w")
|
||||
self._codec.width = 640
|
||||
self._codec.height = 480
|
||||
self._codec.pix_fmt = "yuv420p"
|
||||
self._codec.time_base = self._time_base
|
||||
self._codec.framerate = 20
|
||||
self._codec.options = {"preset": "ultrafast", "tune": "zerolatency"}
|
||||
self._codec.open()
|
||||
self._pts = 0
|
||||
self.timing_sei_enabled = False
|
||||
|
||||
async def recv(self):
|
||||
await asyncio.sleep(self._dt)
|
||||
frame = av.VideoFrame(self._codec.width, self._codec.height, "yuv420p")
|
||||
frame.planes[0].update(bytes(frame.planes[0].buffer_size))
|
||||
for plane in frame.planes[1:]:
|
||||
plane.update(bytes([128]) * plane.buffer_size)
|
||||
frame.pts = self._pts
|
||||
self._pts += int(self._dt * self._clock_rate)
|
||||
packets = self._codec.encode(frame)
|
||||
if not packets:
|
||||
return await self.recv()
|
||||
packet = av.Packet(b"".join(bytes(encoded) for encoded in packets))
|
||||
packet.pts = frame.pts
|
||||
packet.dts = frame.pts
|
||||
packet.time_base = self._time_base
|
||||
packet.duration = int(self._dt * self._clock_rate)
|
||||
return packet
|
||||
|
||||
def switch_camera(self, camera_type: str) -> None:
|
||||
if camera_type not in LiveStreamVideoStreamTrack.livestream_camera_to_sock_mapping:
|
||||
raise ValueError(f"Unknown camera {camera_type}")
|
||||
Reference in New Issue
Block a user