a better remote body teleoperation experience

.gitmodules

@@ -1,9 +1,11 @@
 [submodule "panda"]
   path = panda
   url = ../../commaai/panda.git
+  branch = body-hoverboard
 [submodule "opendbc"]
   path = opendbc_repo
   url = ../../commaai/opendbc.git
+  branch = body
 [submodule "msgq"]
   path = msgq_repo
   url = ../../commaai/msgq.git
@@ -13,6 +15,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

@@ -2515,6 +2515,10 @@ struct AudioFeedback {
   blockNum @1 :UInt16;
 }
 
+struct SoundRequest {
+  sound @0 :Car.CarControl.HUDControl.AudibleAlert;
+}
+
 struct Touch {
   sec @0 :Int64;
   usec @1 :Int64;
@@ -2618,6 +2622,8 @@ struct Event {
     userBookmark @93 :UserBookmark;
     bookmarkButton @148 :UserBookmark;
     audioFeedback @149 :AudioFeedback;
+    soundRequest @150 :SoundRequest;
+    webrtcAudioData @151 :AudioData;
 
     lateralManeuverPlan @150 :LateralManeuverPlan;
 

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.),
+  "webrtcAudioData": (False, 0.),
   "roadEncodeData": (False, 20., None, QueueSize.BIG),
   "driverEncodeData": (False, 20., None, QueueSize.BIG),
   "wideRoadEncodeData": (False, 20., None, QueueSize.BIG),

common/api.py

@@ -7,6 +7,10 @@
 
 API_HOST = os.getenv('API_HOST', 'https://api.commadotai.com')
 
+CONNECT_HOST = os.getenv('CONNECT_HOST', 'https://connect.comma.ai')
+CONNECT_HOST_DISPLAY = CONNECT_HOST.removeprefix("https://").removeprefix("http://")
+CONNECT_CLIENT = os.getenv('CONNECT_CLIENT', 'no domain name set')
+
 # name: jwt signature algorithm
 KEYS = {"id_rsa": "RS256",
         "id_ecdsa": "ES256"}

common/params_keys.h

@@ -78,6 +78,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LastUpdateTime", {PERSISTENT, TIME}},
     {"LastUpdateUptimeOnroad", {PERSISTENT, FLOAT, "0.0"}},
     {"LiveDelay", {PERSISTENT, BYTES}},
+    {"LivestreamCamera", {CLEAR_ON_MANAGER_START, STRING, "driver"}},
     {"LiveParameters", {PERSISTENT, JSON}},
     {"LiveParametersV2", {PERSISTENT, BYTES}},
     {"LiveTorqueParameters", {PERSISTENT | DONT_LOG, BYTES}},

selfdrive/debug/bodyview.py

@@ -0,0 +1,93 @@
+#!/usr/bin/env python3
+"""Launch the comma UI, but simulating a comma body."""
+import argparse
+import os
+import time
+import threading
+
+
+import pyray as rl
+from cereal import car, log, messaging
+from openpilot.common.params import Params
+
+
+def send_messages():
+  pm = messaging.PubMaster(['deviceState', 'pandaStates', 'carParams', 'carState', 'selfdriveState'])
+
+  car_params_msg = messaging.new_message('carParams')
+  car_params_msg.carParams.brand = "body"
+  car_params_msg.carParams.notCar = True
+
+  device_state_msg = messaging.new_message('deviceState')
+  device_state_msg.deviceState.started = False
+
+  panda_msg = messaging.new_message('pandaStates', 1)
+  panda_msg.pandaStates[0].ignitionLine = True
+  panda_msg.pandaStates[0].pandaType = log.PandaState.PandaType.uno
+
+  car_state_msg = messaging.new_message('carState')
+  car_state_msg.carState.charging = True
+  car_state_msg.carState.fuelGauge = 0.80
+
+  selfdrive_state_msg = messaging.new_message('selfdriveState')
+  selfdrive_state_msg.selfdriveState.enabled = True
+
+  while True:
+    pm.send('carParams', car_params_msg)
+    pm.send('deviceState', device_state_msg)
+    pm.send('pandaStates', panda_msg)
+    pm.send('carState', car_state_msg)
+    pm.send('selfdriveState', selfdrive_state_msg)
+    time.sleep(0.01)
+
+
+def main():
+  parser = argparse.ArgumentParser(description="Launch body view UI")
+  parser.add_argument("--big", action="store_true", help="Launch in big UI mode (comma 3X)")
+  parser.add_argument("--joystick", action="store_true", help="Wait for joystick_control before going onroad")
+  parser.add_argument("--monitor", type=int, default=None, help="Pin window to specified monitor index (e.g. 0, 1)")
+  args = parser.parse_args()
+
+  if args.big:
+    os.environ["BIG"] = "1"
+  from openpilot.system.ui.lib.application import gui_app
+
+  if args.monitor is not None:
+    # Pin window to specified monitor after init
+    monitor_index = args.monitor
+    _orig_init_window = gui_app.init_window
+    def _init_window_on_monitor(*a, **kw):
+      _orig_init_window(*a, **kw)
+      pos = rl.get_monitor_position(monitor_index)
+      rl.set_window_position(int(pos.x), int(pos.y))
+    gui_app.init_window = _init_window_on_monitor
+
+  # Set CarParamsPersistent so ui_state.CP.notCar is True on startup
+  params = Params()
+  CP = car.CarParams.new_message(notCar=True, brand="body", wheelbase=1, steerRatio=10)
+  params.put("CarParamsPersistent", CP.to_bytes())
+  params.put_bool("JoystickDebugMode", True)
+
+  if args.joystick:
+    params.put_bool("IsOffroad", True)
+
+    # Wait for joystick_control to start before going "onroad"
+    sm = messaging.SubMaster(['testJoystick'])
+    print("Waiting for joystick_control to start (run: python tools/joystick/joystick_control.py --keyboard) ...")
+    while sm.recv_frame['testJoystick'] == 0:
+      params.put_bool("IsOffroad", True)
+      sm.update(100)
+    print("Joystick connected, starting body view.")
+    params.remove("IsOffroad")
+
+  # Start message sender in background
+  t = threading.Thread(target=send_messages, daemon=True)
+  t.start()
+
+  # Import after env is set so BIG_UI picks it up
+  from openpilot.selfdrive.ui.ui import main as ui_main
+  ui_main()
+
+
+if __name__ == "__main__":
+  main()

selfdrive/pandad/pandad.cc

@@ -302,6 +302,7 @@ void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control)
 
   {
     sm.update(0);
+
     if (sm.updated("deviceState") && !no_fan_control) {
       // Fan speed
       uint16_t fan_speed = sm["deviceState"].getDeviceState().getFanSpeedPercentDesired();
@@ -341,9 +342,19 @@ 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); 
+      std::string cp_bytes = params.get("CarParams");
+      if (cp_bytes.size() > 0) {
+        AlignedBuffer aligned_buf;
+        capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
+        cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
+        if (CP.getNotCar()) {
+          ir_pwr = 0;
+        }
+      }
+
+      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/selfdrived/selfdrived.py

@@ -75,6 +75,8 @@ def __init__(self, CP=None):
     self.car_state_sock = messaging.sub_sock('carState', timeout=20)
 
     ignore = self.sensor_packets + self.gps_packets + ['alertDebug', 'lateralManeuverPlan']
+    if self.CP.notCar:
+      ignore += ['driverMonitoringState']
     if SIMULATION:
       ignore += ['driverCameraState', 'managerState']
     if REPLAY:
@@ -192,7 +194,7 @@ def update_events(self, CS):
 
       if self.CP.notCar:
         # wait for everything to init first
-        if self.sm.frame > int(5. / DT_CTRL) and self.initialized:
+        if self.sm.frame > int(2. / DT_CTRL) and self.initialized:
           # body always wants to enable
           self.events.add(EventName.pcmEnable)