teleop in connect

.gitmodules

@@ -13,6 +13,7 @@
 [submodule "teleoprtc_repo"]
   path = teleoprtc_repo
   url = ../../commaai/teleoprtc
+  branch = fix/datachannel-double-counting
 [submodule "tinygrad"]
   path = tinygrad_repo
   url = https://github.com/tinygrad/tinygrad.git

cereal/log.capnp

@@ -2356,6 +2356,19 @@ struct AudioFeedback {
   blockNum @1 :UInt16;
 }
 
+struct SoundRequest {
+  sound @0 :Car.CarControl.HUDControl.AudibleAlert;
+}
+
+struct LiveStreamCamera {
+  camera @0 :CameraType;
+
+  enum CameraType {
+    driver @0;
+    wideRoad @1;
+  }
+}
+
 struct Touch {
   sec @0 :Int64;
   usec @1 :Int64;
@@ -2474,6 +2487,11 @@ struct Event {
     livestreamWideRoadEncodeData @121 :EncodeData;
     livestreamDriverEncodeData @122 :EncodeData;
 
+    livestreamCameraEncodeData @151 :EncodeData;
+    livestreamCameraSwitch @152 :LiveStreamCamera;
+
+    soundRequest @153 :SoundRequest;
+
     # *********** Custom: reserved for forks ***********
 
     # DO change the name of the field

cereal/services.py

@@ -85,6 +85,8 @@ def __init__(self, should_log: bool, frequency: float, decimation: Optional[int]
   "rawAudioData": (False, 20.),
   "bookmarkButton": (True, 0., 1),
   "audioFeedback": (True, 0., 1),
+  "soundRequest": (False, 0.),
+  "livestreamCameraSwitch": (False, 0.),
   "roadEncodeData": (False, 20., None, QueueSize.BIG),
   "driverEncodeData": (False, 20., None, QueueSize.BIG),
   "wideRoadEncodeData": (False, 20., None, QueueSize.BIG),
@@ -100,6 +102,7 @@ def __init__(self, should_log: bool, frequency: float, decimation: Optional[int]
   "livestreamWideRoadEncodeData": (False, 20., None, QueueSize.MEDIUM),
   "livestreamRoadEncodeData": (False, 20., None, QueueSize.MEDIUM),
   "livestreamDriverEncodeData": (False, 20., None, QueueSize.MEDIUM),
+  "livestreamCameraEncodeData": (False, 20., None, QueueSize.MEDIUM),
   "customReservedRawData0": (True, 0.),
   "customReservedRawData1": (True, 0.),
   "customReservedRawData2": (True, 0.),

selfdrive/pandad/pandad.cc

@@ -341,9 +341,9 @@ void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control)
     }
 
     if (ir_pwr != prev_ir_pwr || sm.frame % 100 == 0) {
-      int16_t ir_panda = util::map_val(ir_pwr, 0, 100, 0, MAX_IR_PANDA_VAL); 
+      int16_t ir_panda = util::map_val(ir_pwr, 0, 100, 0, MAX_IR_PANDA_VAL);
       panda->set_ir_pwr(ir_panda);
-      Hardware::set_ir_power(ir_pwr); 
+      Hardware::set_ir_power(ir_pwr);
       prev_ir_pwr = ir_pwr;
     }
   }

selfdrive/ui/soundd.py

@@ -3,7 +3,6 @@
 import time
 import wave
 
-
 from cereal import car, messaging
 from openpilot.common.basedir import BASEDIR
 from openpilot.common.filter_simple import FirstOrderFilter
@@ -128,7 +127,9 @@ def update_alert(self, new_alert):
       self.current_sound_frame = 0
 
   def get_audible_alert(self, sm):
-    if sm.updated['selfdriveState']:
+    if sm.updated['soundRequest'] and sm['soundRequest'].sound.raw != AudibleAlert.none:
+        self.update_alert(sm['soundRequest'].sound.raw)
+    elif sm.updated['selfdriveState']:
       new_alert = sm['selfdriveState'].alertSound.raw
       self.update_alert(new_alert)
     elif check_selfdrive_timeout_alert(sm):
@@ -153,7 +154,7 @@ def soundd_thread(self):
     # sounddevice must be imported after forking processes
     import sounddevice as sd
 
-    sm = messaging.SubMaster(['selfdriveState', 'soundPressure'])
+    sm = messaging.SubMaster(['selfdriveState', 'soundPressure', 'soundRequest'])
 
     with self.get_stream(sd) as stream:
       rk = Ratekeeper(20)

system/athena/athenad.py

@@ -28,7 +28,7 @@
                        create_connection)
 
 import cereal.messaging as messaging
-from cereal import log
+from cereal import car, log
 from cereal.services import SERVICE_LIST
 from openpilot.common.api import Api, get_key_pair
 from openpilot.common.utils import CallbackReader, get_upload_stream
@@ -44,6 +44,7 @@
 ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
 HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4"))
 LOCAL_PORT_WHITELIST = {22, }  # SSH
+WEBRTCD_PORT = 5001
 
 LOG_ATTR_NAME = 'user.upload'
 LOG_ATTR_VALUE_MAX_UNIX_TIME = int.to_bytes(2147483647, 4, sys.byteorder)
@@ -536,6 +537,16 @@ def getSshAuthorizedKeys() -> str:
 def getGithubUsername() -> str:
   return cast(str, Params().get("GithubUsername") or "")
 
+
+@dispatcher.add_method
+def getNotCar() -> bool:
+  cp_bytes = Params().get("CarParamsPersistent")
+  if cp_bytes is not None:
+    with car.CarParams.from_bytes(cp_bytes) as CP:
+      return CP.notCar
+  return False
+
+
 @dispatcher.add_method
 def getSimInfo():
   return HARDWARE.get_sim_info()
@@ -557,6 +568,26 @@ def getNetworks():
   return HARDWARE.get_networks()
 
 
+@dispatcher.add_method
+def startJoystickStream(sdp: str) -> dict:
+  from openpilot.system.webrtc.webrtcd import StreamRequestBody
+  body = StreamRequestBody(sdp, ["driver"], ["testJoystick", "soundRequest", "livestreamCameraSwitch"], ["carState"])
+  try:
+    resp = requests.post(f"http://localhost:{WEBRTCD_PORT}/stream",
+                       json=asdict(body), timeout=10)
+    if not resp.ok:
+      try:
+        error_body = resp.json()
+        raise Exception(error_body.get("message", f"webrtcd returned {resp.status_code}"))
+      except ValueError:
+        resp.raise_for_status()
+    return resp.json()
+  except requests.ConnectTimeout:
+    raise Exception("webrtc took too long to respond. is it on?") from None
+  except requests.ConnectionError:
+    raise Exception("webrtc is not running. turn on comma body ignition.") from None
+
+
 @dispatcher.add_method
 def takeSnapshot() -> str | dict[str, str] | None:
   from openpilot.system.camerad.snapshot import jpeg_write, snapshot

system/loggerd/encoderd.cc

@@ -151,6 +151,64 @@ void encoderd_thread(const LogCameraInfo (&cameras)[N]) {
   }
 }
 
+template <size_t N>
+void stream_encoderd_thread(const LogCameraInfo (&cameras)[N]) {
+  // Wait for camerad to publish at least one stream
+  while (!do_exit) {
+    if (!VisionIpcClient::getAvailableStreams("camerad", false).empty()) break;
+    util::sleep_for(100);
+  }
+
+  SubMaster sm({"livestreamCameraSwitch"});
+
+  const LogCameraInfo *active_cam = &cameras[0];
+
+  while (!do_exit) {
+    VisionIpcClient vipc_client("camerad", active_cam->stream_type, false);
+    if (!vipc_client.connect(false)) {
+      util::sleep_for(5);
+      continue;
+    }
+
+    const VisionBuf &buf_info = vipc_client.buffers[0];
+    LOGW("stream encoder init %zux%zu", buf_info.width, buf_info.height);
+    assert(buf_info.width > 0 && buf_info.height > 0);
+
+    // Each stream camera has exactly one encoder
+    auto encoder = std::make_unique<Encoder>(active_cam->encoder_infos[0], buf_info.width, buf_info.height);
+    encoder->encoder_open();
+
+    while (!do_exit) {
+      sm.update(0);
+      if (sm.updated("livestreamCameraSwitch")) {
+        auto requested = sm["livestreamCameraSwitch"].getLivestreamCameraSwitch().getCamera();
+        VisionStreamType requested_stream = requested == cereal::LiveStreamCamera::CameraType::DRIVER
+                                                ? VISION_STREAM_DRIVER : VISION_STREAM_WIDE_ROAD;
+        // Switch camera if the request differs from the current one
+        if (requested_stream != active_cam->stream_type) {
+          LOGW("stream encoder switching camera");
+          auto it = std::find_if(std::begin(cameras), std::end(cameras),
+                                 [requested_stream](const auto &cam) { return cam.stream_type == requested_stream; });
+          if (it != std::end(cameras)) active_cam = &(*it);
+          break;  // Reinit encoder with the new camera
+        }
+      }
+
+      VisionIpcBufExtra extra;
+      VisionBuf *buf = vipc_client.recv(&extra);
+      if (buf == nullptr) continue;
+
+      if (buf->get_frame_id() != extra.frame_id) continue;
+
+      if (encoder->encode_frame(buf, &extra) == -1) {
+        LOGE("stream encoder: failed to encode frame. frame_id: %d", extra.frame_id);
+      }
+    }
+
+    encoder->encoder_close();
+  }
+}
+
 int main(int argc, char* argv[]) {
   if (!Hardware::PC()) {
     int ret;
@@ -162,7 +220,7 @@ int main(int argc, char* argv[]) {
   if (argc > 1) {
     std::string arg1(argv[1]);
     if (arg1 == "--stream") {
-      encoderd_thread(stream_cameras_logged);
+      stream_encoderd_thread(stream_cameras_logged);
     } else {
       LOGE("Argument '%s' is not supported", arg1.c_str());
     }

system/loggerd/loggerd.h

@@ -47,8 +47,8 @@ struct EncoderSettings {
   }
 
   static EncoderSettings StreamEncoderSettings() {
-    int _stream_bitrate = getenv("STREAM_BITRATE") ? atoi(getenv("STREAM_BITRATE")) : 1'000'000;
-    return EncoderSettings{.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264, .bitrate = _stream_bitrate , .gop_size = 15};
+    int _stream_bitrate = getenv("STREAM_BITRATE") ? atoi(getenv("STREAM_BITRATE")) : 4'000'000;
+    return EncoderSettings{.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264, .bitrate = _stream_bitrate , .gop_size = 5};
   }
 };
 
@@ -100,28 +100,13 @@ const EncoderInfo main_driver_encoder_info = {
   INIT_ENCODE_FUNCTIONS(DriverEncode),
 };
 
-const EncoderInfo stream_road_encoder_info = {
-  .publish_name = "livestreamRoadEncodeData",
-  //.thumbnail_name = "thumbnail",
-  .record = false,
-  .get_settings = [](int){return EncoderSettings::StreamEncoderSettings();},
-  INIT_ENCODE_FUNCTIONS(LivestreamRoadEncode),
-};
-
-const EncoderInfo stream_wide_road_encoder_info = {
-  .publish_name = "livestreamWideRoadEncodeData",
+const EncoderInfo stream_encoder_info = {
+  .publish_name = "livestreamCameraEncodeData",
   .record = false,
   .get_settings = [](int){return EncoderSettings::StreamEncoderSettings();},
   INIT_ENCODE_FUNCTIONS(LivestreamWideRoadEncode),
 };
 
-const EncoderInfo stream_driver_encoder_info = {
-  .publish_name = "livestreamDriverEncodeData",
-  .record = false,
-  .get_settings = [](int){return EncoderSettings::StreamEncoderSettings();},
-  INIT_ENCODE_FUNCTIONS(LivestreamDriverEncode),
-};
-
 const EncoderInfo qcam_encoder_info = {
   .publish_name = "qRoadEncodeData",
   .filename = "qcamera.ts",
@@ -153,20 +138,20 @@ const LogCameraInfo driver_camera_info{
 const LogCameraInfo stream_road_camera_info{
   .thread_name = "road_cam_encoder",
   .stream_type = VISION_STREAM_ROAD,
-  .encoder_infos = {stream_road_encoder_info}
+  .encoder_infos = {stream_encoder_info}
 };
 
 const LogCameraInfo stream_wide_road_camera_info{
   .thread_name = "wide_road_cam_encoder",
   .stream_type = VISION_STREAM_WIDE_ROAD,
-  .encoder_infos = {stream_wide_road_encoder_info}
+  .encoder_infos = {stream_encoder_info}
 };
 
 const LogCameraInfo stream_driver_camera_info{
   .thread_name = "driver_cam_encoder",
   .stream_type = VISION_STREAM_DRIVER,
-  .encoder_infos = {stream_driver_encoder_info}
+  .encoder_infos = {stream_encoder_info}
 };
 
 const LogCameraInfo cameras_logged[] = {road_camera_info, wide_road_camera_info, driver_camera_info};
-const LogCameraInfo stream_cameras_logged[] = {stream_road_camera_info, stream_wide_road_camera_info, stream_driver_camera_info};
+const LogCameraInfo stream_cameras_logged[] = {stream_driver_camera_info, stream_wide_road_camera_info};

system/webrtc/device/video.py

@@ -1,4 +1,5 @@
 import asyncio
+import struct
 import time
 
 import av
@@ -7,21 +8,36 @@
 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 = {
-    "driver": "livestreamDriverEncodeData",
-    "wideRoad": "livestreamWideRoadEncodeData",
-    "road": "livestreamRoadEncodeData",
-  }
 
+class LiveStreamVideoStreamTrack(TiciVideoStreamTrack):
   def __init__(self, camera_type: str):
     dt = DT_DMON if camera_type == "driver" else DT_MDL
     super().__init__(camera_type, dt)
 
-    self._sock = messaging.sub_sock(self.camera_to_sock_mapping[camera_type], conflate=True)
-    self._pts = 0
+    self._sock = messaging.sub_sock("livestreamCameraEncodeData", conflate=True)
     self._t0_ns = time.monotonic_ns()
+    self.timing_sei_enabled = False
+
+  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:
@@ -30,15 +46,12 @@ 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
-    packet.pts = self._pts
-    self.log_debug("track sending frame %d", self._pts)
-
+    pts = ((time.monotonic_ns() - self._t0_ns) * self._clock_rate) // 1_000_000_000
+    packet.pts = pts
+    self.log_debug("track sending frame %d", pts)
     return packet
 
   def codec_preference(self) -> str | None:

system/webrtc/utils.py

@@ -0,0 +1,15 @@
+import json
+
+def clock_sync_build_json(payload: dict) -> str | None:
+  import time as _time
+  data = payload.get("data", {})
+  if data.get("action") != "ping":
+    raise ValueError
+  return json.dumps({
+    "type": "clockSync",
+    "data": {
+      "action": "pong",
+      "browserSendTime": data.get("browserSendTime"),
+      "deviceTime": _time.time() * 1000,  # noqa: TID251
+    }
+  })