diff --git a/.gitmodules b/.gitmodules index ad6530de9ac910..b01ab88806ef28 100644 --- a/.gitmodules +++ b/.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 diff --git a/cereal/log.capnp b/cereal/log.capnp index 0f8b5470a44d1a..17e9b1a8646752 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2356,6 +2356,10 @@ struct AudioFeedback { blockNum @1 :UInt16; } +struct SoundRequest { + sound @0 :Car.CarControl.HUDControl.AudibleAlert; +} + struct Touch { sec @0 :Int64; usec @1 :Int64; @@ -2474,6 +2478,9 @@ struct Event { livestreamWideRoadEncodeData @121 :EncodeData; livestreamDriverEncodeData @122 :EncodeData; + soundRequest @151 :SoundRequest; + webrtcAudioData @152 :AudioData; + # *********** Custom: reserved for forks *********** # DO change the name of the field diff --git a/cereal/services.py b/cereal/services.py index c8525c0dd3034e..257cafc98ac456 100755 --- a/cereal/services.py +++ b/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), diff --git a/common/params_keys.h b/common/params_keys.h index b81a373d0876ef..883950d0752374 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -78,6 +78,7 @@ inline static std::unordered_map 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}}, diff --git a/selfdrive/debug/bodyview.py b/selfdrive/debug/bodyview.py new file mode 100755 index 00000000000000..0f0b679601fba1 --- /dev/null +++ b/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() diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index 28d459f458aed3..f0be7565162cac 100644 --- a/selfdrive/pandad/pandad.cc +++ b/selfdrive/pandad/pandad.cc @@ -340,10 +340,21 @@ void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control) ir_pwr = 0; } + // turn off IR leds if body + 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(); + if (CP.getNotCar()) { + ir_pwr = 0; + } + } + 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; } } diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 6a294ca8d82786..046f54744a9f0b 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/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) diff --git a/selfdrive/ui/body/__init__.py b/selfdrive/ui/body/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/ui/body/animations.py b/selfdrive/ui/body/animations.py new file mode 100644 index 00000000000000..fd6fbb7bb79ad4 --- /dev/null +++ b/selfdrive/ui/body/animations.py @@ -0,0 +1,282 @@ +from dataclasses import dataclass +from enum import Enum +import time + + +class AnimationMode(Enum): + ONCE_FORWARD = 1 + ONCE_FORWARD_BACKWARD = 2 + REPEAT_FORWARD = 3 + REPEAT_FORWARD_BACKWARD = 4 + +@dataclass +class Animation: + frames: list[list[tuple[int, int]]] + starting_frames: list[list[tuple[int, int]]] | None = None # played once before the main loop + frame_duration: float = 0.15 # seconds each frame is shown + mode: AnimationMode = AnimationMode.REPEAT_FORWARD_BACKWARD + repeat_interval: float = 5.0 # seconds between animation restarts (only for REPEAT modes) + hold_end: float = 0.0 # seconds to hold the last frame before playing backward (only for *_BACKWARD modes) + left_turn_remove: list[tuple[int, int]] | None = None # dots to remove from frame when turning left + right_turn_remove: list[tuple[int, int]] | None = None # dots to remove from frame when turning right + +# --- Animation Helper Functions --- + +def _mirror(dots: list[tuple[int, int]]) -> list[tuple[int, int]]: + """Mirror a component from the left side of the face to the right""" + return [(r, 15 - c) for r, c in dots] + + +def _mirror_no_flip(dots: list[tuple[int, int]]) -> list[tuple[int, int]]: + """Move a component to the mirrored position on the right half without flipping its shape.""" + min_c = min(c for _, c in dots) + max_c = max(c for _, c in dots) + return [(r, 15 - max_c - min_c + c) for r, c in dots] + + +def _shift(dots: list[tuple[int, int]], rc: tuple[int, int]) -> list[tuple[int, int]]: + dr, dc = rc + return [(r + dr, c + dc) for r, c in dots] + + +def _make_frame(left_eye: list[tuple[int, int]], right_eye: list[tuple[int, int]], + left_brow: list[tuple[int, int]], right_brow: list[tuple[int, int]], + mouth: list[tuple[int, int]]) -> list[tuple[int, int]]: + return left_eye + left_brow + right_eye + right_brow + mouth + +# --- Animation Helper Components --- + +# Eyes (left side) +EYE_OPEN = [ + (2, 2), (2, 3), +(3, 1), (3, 2), (3, 3), (3, 4), +(4, 1), (4, 2), (4, 3), (4, 4), + (5, 2), (5, 3) +] +EYE_HALF = [ +(4, 1), (4, 2), (4, 3), (4, 4), + (5, 2), (5, 3) +] +EYE_CLOSED = [ +(4, 1), (4, 4), + (5, 2), (5, 3), +] +EYE_LEFT_LOOK = [ + (2, 2), (2, 3), +(3, 1), (3, 2), +(4, 1), (4, 2), + (5, 2), (5, 3), +] +EYE_RIGHT_LOOK = [ + (2, 2), (2, 3), + (3, 3), (3, 4), + (4, 3), (4, 4), + (5, 2), (5, 3), +] + +# Eyebrows (left side) +BROW_HIGH = [ + (0, 1), (0, 2), +(1, 0), +] +BROW_LOWERED = [ + (1, 1), (1, 2), +(2, 0) +] +BROW_STRAIGHT = [(1, 0), (1, 1), (1, 2)] +BROW_DOWN = [ +(0, 1), (0, 2), + (1, 3) +] + +# Mouths (centered, not mirrored) +MOUTH_SMILE = [ +(6, 6), (6, 9), + (7, 7), (7, 8), +] +MOUTH_NORMAL = [(7, 7), (7, 8)] +MOUTH_SAD = [ + (6, 7), (6, 8), +(7, 6), (7, 9) +] + + +# --- Animations --- + +NORMAL = Animation( + frames=[ + _make_frame(EYE_OPEN, _mirror(EYE_OPEN), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + _make_frame(EYE_HALF, _mirror(EYE_HALF), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + _make_frame(EYE_CLOSED, _mirror(EYE_CLOSED), BROW_LOWERED, _mirror(BROW_LOWERED), MOUTH_SMILE), + ], + left_turn_remove=[ + (3, 3), (3, 4), + (4, 3), (4, 4), + ] + _mirror_no_flip([ + (3, 1), (3, 2), + (4, 1), (4, 2), + ]), + right_turn_remove=[ + (3, 1), (3, 2), + (4, 1), (4, 2), + ] + _mirror_no_flip([ + (3, 3), (3, 4), + (4, 3), (4, 4), + ]) +) + +ASLEEP = Animation( + frames=[ + _make_frame(EYE_CLOSED, _mirror(EYE_CLOSED), [], [], MOUTH_NORMAL), + ], + # frame_duration=0.25, +) + +SLEEPY = Animation( + frames=[ + _make_frame(EYE_CLOSED, _mirror(EYE_CLOSED), _shift(BROW_STRAIGHT, (1,0)), [], MOUTH_NORMAL), + _make_frame(EYE_HALF, _mirror(EYE_CLOSED), BROW_LOWERED, [], MOUTH_NORMAL), + _make_frame(EYE_OPEN, _mirror(EYE_CLOSED), BROW_HIGH, [], MOUTH_NORMAL) + ], + frame_duration=0.25, + mode=AnimationMode.ONCE_FORWARD_BACKWARD, + repeat_interval=10, + hold_end=1.5, +) + +INQUISITIVE = Animation( + frames=[ + _make_frame(EYE_OPEN, _mirror(EYE_OPEN), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + + _make_frame(EYE_LEFT_LOOK, _mirror(EYE_RIGHT_LOOK), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + _make_frame(_shift(EYE_LEFT_LOOK, (0, -1)), _shift(_mirror(EYE_RIGHT_LOOK), (0, -1)), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + _make_frame(_shift(EYE_LEFT_LOOK, (0, -1)), _shift(_mirror(EYE_RIGHT_LOOK), (0, -1)), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + _make_frame(_shift(EYE_LEFT_LOOK, (0, -1)), _shift(_mirror(EYE_RIGHT_LOOK), (0, -1)), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + _make_frame(EYE_LEFT_LOOK, _mirror(EYE_RIGHT_LOOK), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + + # _make_frame(EYE_OPEN, _mirror(EYE_OPEN), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + + _make_frame(EYE_RIGHT_LOOK, _mirror(EYE_LEFT_LOOK), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + _make_frame(_shift(EYE_RIGHT_LOOK, (0, 1)), _shift(_mirror(EYE_LEFT_LOOK), (0, 1)), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + _make_frame(_shift(EYE_RIGHT_LOOK, (0, 1)), _shift(_mirror(EYE_LEFT_LOOK), (0, 1)), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + _make_frame(_shift(EYE_RIGHT_LOOK, (0, 1)), _shift(_mirror(EYE_LEFT_LOOK), (0, 1)), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + _make_frame(EYE_RIGHT_LOOK, _mirror(EYE_LEFT_LOOK), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + + _make_frame(EYE_OPEN, _mirror(EYE_OPEN), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + ], + mode=AnimationMode.REPEAT_FORWARD, + frame_duration=0.15, + repeat_interval=10 +) + +WINK = Animation( + frames=[ + _make_frame(EYE_OPEN, _mirror(EYE_OPEN), BROW_HIGH, _mirror(BROW_HIGH), MOUTH_SMILE), + _make_frame(EYE_OPEN, _mirror(EYE_CLOSED), BROW_HIGH, _mirror(_shift(BROW_DOWN, (0, 2))), MOUTH_SMILE), + ], + mode=AnimationMode.ONCE_FORWARD_BACKWARD, + frame_duration=0.75, +) + + +# --- Face Animator Class --- + +class FaceAnimator: + def __init__(self, animation: Animation): + self._animation = animation + self._next: Animation | None = None + self._start_time = time.monotonic() + self._rewinding = False + self._rewind_start: float = 0.0 + self._rewind_from: int = 0 + self._seen_nonzero = False + + def set_animation(self, animation: Animation): + if animation is not self._animation: + self._next = animation + + def get_dots(self) -> list[tuple[int, int]]: + now = time.monotonic() + elapsed = now - self._start_time + + # Handle rewind for forward-only animations + if self._rewinding: + rewind_elapsed = now - self._rewind_start + frames_back = round(rewind_elapsed / self._animation.frame_duration) + frame_index = self._rewind_from - frames_back + if frame_index <= 0: + return self._switch_to_next(now) + return self._animation.frames[frame_index] + + # Play starting frames first (once) + starting = self._animation.starting_frames or [] + starting_duration = len(starting) * self._animation.frame_duration + if starting and elapsed < starting_duration: + frame_index = min(int(elapsed / self._animation.frame_duration), len(starting) - 1) + return starting[frame_index] + + # Main loop + loop_elapsed = elapsed - starting_duration if starting else elapsed + frame_index = _get_frame_index(self._animation, loop_elapsed, gap_first=bool(starting)) + + if frame_index != 0: + self._seen_nonzero = True + + if self._next is not None: + if frame_index == 0 and (len(self._animation.frames) == 1 or self._seen_nonzero): + return self._switch_to_next(now) + # No natural return to frame 0 — start rewinding + if self._animation.mode in (AnimationMode.ONCE_FORWARD, AnimationMode.REPEAT_FORWARD): + self._rewinding = True + self._rewind_start = now + self._rewind_from = frame_index + + return self._animation.frames[frame_index] + + def _switch_to_next(self, now: float) -> list[tuple[int, int]]: + self._animation = self._next + self._next = None + self._rewinding = False + self._seen_nonzero = False + self._start_time = now + return self._animation.frames[0] + + +def _get_frame_index(animation: Animation, elapsed: float, gap_first: bool = False) -> int: + """Get the current frame index given elapsed time and animation mode.""" + num_frames = len(animation.frames) + if num_frames == 1: + return 0 + + forward_duration = num_frames * animation.frame_duration + has_backward = animation.mode in (AnimationMode.ONCE_FORWARD_BACKWARD, AnimationMode.REPEAT_FORWARD_BACKWARD) + repeats = animation.mode in (AnimationMode.REPEAT_FORWARD, AnimationMode.REPEAT_FORWARD_BACKWARD) + + if has_backward: + backward_frames = max(num_frames - 2, 0) + backward_duration = backward_frames * animation.frame_duration + cycle_duration = forward_duration + animation.hold_end + backward_duration + else: + backward_frames = 0 + backward_duration = 0 + cycle_duration = forward_duration + + if not repeats: + # Play once — clamp elapsed to one cycle + t = min(elapsed, cycle_duration) + else: + adj_elapsed = elapsed + cycle_duration if gap_first else elapsed + t = adj_elapsed % animation.repeat_interval + + if t < forward_duration: + return min(int(t / animation.frame_duration), num_frames - 1) + elif not has_backward: + return num_frames - 1 + elif t < forward_duration + animation.hold_end: + return num_frames - 1 + elif t < forward_duration + animation.hold_end + backward_duration: + backward_elapsed = t - forward_duration - animation.hold_end + backward_index = min(int(backward_elapsed / animation.frame_duration), backward_frames - 1) + return num_frames - 2 - backward_index + else: + return 0 diff --git a/selfdrive/ui/body/layouts/__init__.py b/selfdrive/ui/body/layouts/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/ui/body/layouts/home_mici.py b/selfdrive/ui/body/layouts/home_mici.py new file mode 100644 index 00000000000000..68449288d2d983 --- /dev/null +++ b/selfdrive/ui/body/layouts/home_mici.py @@ -0,0 +1,35 @@ +import pyray as rl +from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.system.ui.lib.application import FontWeight +from openpilot.system.ui.widgets.label import UnifiedLabel +from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout + +PAIR_MESSAGE_FONT_SIZE = 24 +PAIR_MESSAGE_MARGIN = 16 +PAIR_MESSAGE_WIDTH = 260 + + +class MiciBodyHomeLayout(MiciHomeLayout): + def __init__(self): + super().__init__() + self._branch_label.set_visible(False) + self._pair_message = self._child(UnifiedLabel("", font_size=PAIR_MESSAGE_FONT_SIZE, font_weight=FontWeight.SEMI_BOLD, + text_color=rl.Color(255, 255, 255, int(255 * 0.9)), + alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT, + alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM, + text_padding=0, elide=False, wrap_text=True, line_height=0.95)) + + def _get_pair_message(self) -> str: + if not ui_state.prime_state.is_paired(): + return "pair to this device in settings" + if ui_state.ignition: + return "use at: connect.comma.ai" + return "turn on ignition to use" + + def _render(self, rect: rl.Rectangle): + super()._render(rect) + self._pair_message.set_text(self._get_pair_message()) + msg_h = int(self._pair_message.get_content_height(PAIR_MESSAGE_WIDTH) + 6) + msg_x = self._rect.x + self._rect.width - PAIR_MESSAGE_WIDTH - PAIR_MESSAGE_MARGIN + msg_y = self._rect.y + self._rect.height - msg_h - PAIR_MESSAGE_MARGIN + self._pair_message.render(rl.Rectangle(msg_x, msg_y, PAIR_MESSAGE_WIDTH, msg_h)) diff --git a/selfdrive/ui/body/layouts/onroad.py b/selfdrive/ui/body/layouts/onroad.py new file mode 100644 index 00000000000000..40f2e92398027e --- /dev/null +++ b/selfdrive/ui/body/layouts/onroad.py @@ -0,0 +1,101 @@ +from __future__ import annotations + +import time + +import pyray as rl +from openpilot.system.ui.lib.application import gui_app, FontWeight +from openpilot.system.ui.widgets import Widget +from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.selfdrive.ui.body.animations import FaceAnimator, ASLEEP, INQUISITIVE, NORMAL, SLEEPY + +GRID_COLS = 16 +GRID_ROWS = 8 +RADIUS = 50 if gui_app.big_ui() else 10 + +IDLE_TIMEOUT = 30.0 # seconds of no joystick input before playing INQUISITIVE +IDLE_STEER_THRESH = 0.5 # degrees — below this counts as no input +IDLE_SPEED_THRESH = 0.01 # m/s — below this counts as no input + +PAIR_BTN_FONT_SIZE = 60 +PAIR_BTN_MARGIN = 20 + + +class BodyLayout(Widget): + def __init__(self): + super().__init__() + self._animator = FaceAnimator(ASLEEP) + self._turning_left = False + self._turning_right = False + self._last_input_time = time.monotonic() + self._was_active = False + self._font_bold = gui_app.font(FontWeight.BOLD) + self._prev_joystick_debug_mode = False + + def set_settings_callback(self, callback): + pass + + def is_swiping_left(self) -> bool: + return False + + def draw_dot_grid(self, rect: rl.Rectangle, dots: list[tuple[int, int]], color: rl.Color | None = None): + if color is None: + color = rl.WHITE + + spacing = (rect.height) / (GRID_ROWS) + + grid_w = (GRID_COLS - 1) * spacing + grid_h = (GRID_ROWS - 1) * spacing + + offset_x = rect.x + (rect.width - grid_w) / 2 + offset_y = rect.y + (rect.height - grid_h) / 2 + + for row, col in dots: + x = int(offset_x + col * spacing) + y = int(offset_y + row * spacing) + rl.draw_circle(x, y, RADIUS, color) + + def _update_state(self): + sm = ui_state.sm + + active = ui_state.is_onroad() + if active and ui_state.joystick_debug_mode: + if not self._was_active: + self._last_input_time = time.monotonic() + self._was_active = True + + cs = sm['carState'] + has_input = abs(cs.steeringAngleDeg) > IDLE_STEER_THRESH or abs(cs.vEgo) > IDLE_SPEED_THRESH + if has_input: + self._last_input_time = time.monotonic() + + if time.monotonic() - self._last_input_time > IDLE_TIMEOUT: + self._animator.set_animation(INQUISITIVE) + else: + self._animator.set_animation(NORMAL) + else: + self._was_active = False + self._animator.set_animation(ASLEEP) + + if not sm.updated['carState']: + return + + steer = sm['testJoystick'].axes[1] if len(sm['testJoystick'].axes) > 1 else 0 + self._turning_left = steer >= 0.05 + self._turning_right = steer <= -0.05 + + # play animation on screen tap + def _handle_mouse_release(self, mouse_pos): + super()._handle_mouse_release(mouse_pos) + if not self._was_active: + self._animator.set_animation(SLEEPY) + + def _render(self, rect: rl.Rectangle): + dots = self._animator.get_dots() + animation = self._animator._animation + if self._turning_left and animation.left_turn_remove: + remove_set = set(animation.left_turn_remove) + dots = [d for d in dots if d not in remove_set] + elif self._turning_right and animation.right_turn_remove: + remove_set = set(animation.right_turn_remove) + dots = [d for d in dots if d not in remove_set] + self.draw_dot_grid(rect, dots) \ No newline at end of file diff --git a/selfdrive/ui/body/widgets/sidebar.py b/selfdrive/ui/body/widgets/sidebar.py new file mode 100644 index 00000000000000..db3a49fb3154b5 --- /dev/null +++ b/selfdrive/ui/body/widgets/sidebar.py @@ -0,0 +1,150 @@ +import pyray as rl +from openpilot.selfdrive.ui.layouts.sidebar import Sidebar, Colors, MetricData +from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.system.ui.lib.application import FONT_SCALE +from openpilot.system.ui.lib.multilang import tr +from openpilot.system.ui.lib.text_measure import measure_text_cached + +BODY_SIDEBAR_HEIGHT = 200 +METRIC_HEIGHT = 117 +METRIC_WIDTH = 220 +METRIC_MARGIN = 20 +FONT_SIZE = 30 +BATTERY_FONT_SIZE = 26 + +SETTINGS_BTN = rl.Rectangle(50, 35, 200, 117) + +class Colors(Colors): + BATTERY_GREEN = rl.Color(0, 200, 0, 255) + BATTERY_LOW = rl.Color(201, 34, 49, 255) + +class BodySidebar(Sidebar): + """A top-dropping sidebar for the comma body, containing the same info as the regular sidebar.""" + + def __init__(self): + super().__init__() + self.set_visible(False) + + self._battery_percent = 0.0 + self._battery_charging = False + + def _render(self, rect: rl.Rectangle): + rl.draw_rectangle_rec(rect, rl.BLACK) + + self._draw_settings_button(rect) + self._draw_network_indicator(rect) + self._draw_metrics(rect) + self._draw_battery_indicator(rect) + + def _update_battery_status(self): + sm = ui_state.sm + if sm.updated['carState']: + car_state = sm['carState'] + self._battery_percent = max(0.0, min(1.0, car_state.fuelGauge)) + self._battery_charging = car_state.charging + + def _draw_settings_button(self, rect: rl.Rectangle): + mouse_pos = rl.get_mouse_position() + mouse_down = self.is_pressed and rl.is_mouse_button_down(rl.MouseButton.MOUSE_BUTTON_LEFT) + + btn_x = int(rect.x + 30) + btn_y = int(rect.y + 30) + settings_rect = rl.Rectangle(btn_x, btn_y, SETTINGS_BTN.width, SETTINGS_BTN.height) + settings_down = mouse_down and rl.check_collision_point_rec(mouse_pos, settings_rect) + tint = Colors.BUTTON_PRESSED if settings_down else Colors.BUTTON_NORMAL + rl.draw_texture(self._settings_img, btn_x, btn_y, tint) + + def _draw_battery_indicator(self, rect: rl.Rectangle): + # Battery icon dimensions + batt_w = 50 + batt_h = 28 + tip_w = 5 + tip_h = 12 + batt_x = int(rect.x + rect.width - batt_w - tip_w - 30) + batt_y = int(rect.y + 30 + (METRIC_HEIGHT - batt_h) / 2) + + # Choose fill color based on level + pct = self._battery_percent + if pct <= 0.2: + fill_color = Colors.BATTERY_LOW + elif self._battery_charging: + fill_color = Colors.BATTERY_GREEN + else: + fill_color = Colors.WHITE + + # Battery outline + rl.draw_rectangle_rounded_lines_ex(rl.Rectangle(batt_x, batt_y, batt_w, batt_h), 0.2, 6, 2, Colors.WHITE) + + # Battery tip (positive terminal) + tip_x = batt_x + batt_w + tip_y = batt_y + (batt_h - tip_h) / 2 + rl.draw_rectangle_rounded(rl.Rectangle(tip_x, tip_y, tip_w, tip_h), 0.3, 4, Colors.WHITE) + + # Fill level + fill_margin = 4 + fill_max_w = batt_w - 2 * fill_margin + fill_w = max(0, int(fill_max_w * pct)) + if fill_w > 0: + rl.draw_rectangle_rounded( + rl.Rectangle(batt_x + fill_margin, batt_y + fill_margin, fill_w, batt_h - 2 * fill_margin), + 0.15, 4, fill_color + ) + + # Percentage text + pct_text = f"{int(pct * 100)}%" + if self._battery_charging: + pct_text = pct_text + pct_size = measure_text_cached(self._font_bold, pct_text, BATTERY_FONT_SIZE) + pct_pos = rl.Vector2(batt_x + (batt_w - pct_size.x) / 2, batt_y + batt_h + 6) + rl.draw_text_ex(self._font_bold, pct_text, pct_pos, BATTERY_FONT_SIZE, 0, Colors.WHITE) + + def _draw_network_indicator(self, rect: rl.Rectangle): + # Draw network dots horizontally, positioned after the settings button + x_start = rect.x + 260 + y_pos = rect.y + 40 + dot_size = 20 + dot_spacing = 28 + + for i in range(5): + color = Colors.WHITE if i < self._net_strength else Colors.GRAY + x = int(x_start + i * dot_spacing + dot_size // 2) + y = int(y_pos + dot_size // 2) + rl.draw_circle(x, y, dot_size // 2, color) + + # Network type text below dots + text_pos = rl.Vector2(x_start, y_pos + dot_size + 8) + rl.draw_text_ex(self._font_regular, tr(self._net_type), text_pos, FONT_SIZE, 0, Colors.WHITE) + + def _draw_metrics(self, rect: rl.Rectangle): + metrics = [self._temp_status, self._panda_status, self._connect_status] + # Center the 3 metrics in the middle of the bar + total_width = len(metrics) * METRIC_WIDTH + (len(metrics) - 1) * METRIC_MARGIN + start_x = rect.x + (rect.width - total_width) / 2 + + y = rect.y + 30 + + for i, metric in enumerate(metrics): + x = start_x + i * (METRIC_WIDTH + METRIC_MARGIN) + self._draw_metric(metric, x, y) + + def _draw_metric(self, metric: MetricData, x: float, y: float): + r = rl.Rectangle(x, y, METRIC_WIDTH, METRIC_HEIGHT) + + # Colored top edge (clipped rounded rect) + rl.begin_scissor_mode(int(x), int(y + 4), int(METRIC_WIDTH), 18) + rl.draw_rectangle_rounded(rl.Rectangle(x + 4, y + 4, METRIC_WIDTH - 8, 100), 0.3, 10, metric.color) + rl.end_scissor_mode() + + rl.draw_rectangle_rounded_lines_ex(r, 0.3, 10, 2, Colors.METRIC_BORDER) + + # Center label and value below the top edge + text_y = y + 22 + ((METRIC_HEIGHT - 22) / 2 - 2 * FONT_SIZE * FONT_SCALE) + for label in (metric.label, metric.value): + text = tr(label) + size = measure_text_cached(self._font_bold, text, FONT_SIZE) + text_y += size.y + text_pos = rl.Vector2( + x + (METRIC_WIDTH - size.x) / 2, + text_y + ) + rl.draw_text_ex(self._font_bold, text, text_pos, FONT_SIZE, 0, Colors.WHITE) diff --git a/selfdrive/ui/layouts/main.py b/selfdrive/ui/layouts/main.py index 15d44e24da5b37..ff514d8b835c42 100644 --- a/selfdrive/ui/layouts/main.py +++ b/selfdrive/ui/layouts/main.py @@ -2,13 +2,15 @@ from enum import IntEnum import cereal.messaging as messaging from openpilot.system.ui.lib.application import gui_app +from openpilot.system.ui.widgets import Widget from openpilot.selfdrive.ui.layouts.sidebar import Sidebar, SIDEBAR_WIDTH from openpilot.selfdrive.ui.layouts.home import HomeLayout from openpilot.selfdrive.ui.layouts.settings.settings import SettingsLayout, PanelType from openpilot.selfdrive.ui.onroad.augmented_road_view import AugmentedRoadView from openpilot.selfdrive.ui.ui_state import device, ui_state -from openpilot.system.ui.widgets import Widget from openpilot.selfdrive.ui.layouts.onboarding import OnboardingWindow +from openpilot.selfdrive.ui.body.layouts.onroad import BodyLayout +from openpilot.selfdrive.ui.body.widgets.sidebar import BodySidebar, BODY_SIDEBAR_HEIGHT class MainState(IntEnum): @@ -23,12 +25,15 @@ def __init__(self): self._pm = messaging.PubMaster(['bookmarkButton']) - self._sidebar = Sidebar() + self._is_body = ui_state.is_body + self._sidebar = BodySidebar() if self._is_body else Sidebar() self._current_mode = MainState.HOME self._prev_onroad = False # Initialize layouts self._layouts = {MainState.HOME: HomeLayout(), MainState.SETTINGS: SettingsLayout(), MainState.ONROAD: AugmentedRoadView()} + if self._is_body: + self._layouts[MainState.HOME] = BodyLayout() self._sidebar_rect = rl.Rectangle(0, 0, 0, 0) self._content_rect = rl.Rectangle(0, 0, 0, 0) @@ -51,22 +56,28 @@ def _setup_callbacks(self): self._sidebar.set_callbacks(on_settings=self._on_settings_clicked, on_flag=self._on_bookmark_clicked, open_settings=lambda: self.open_settings(PanelType.TOGGLES)) - self._layouts[MainState.HOME]._setup_widget.set_open_settings_callback(lambda: self.open_settings(PanelType.FIREHOSE)) + if not self._is_body: + self._layouts[MainState.HOME]._setup_widget.set_open_settings_callback(lambda: self.open_settings(PanelType.FIREHOSE)) self._layouts[MainState.HOME].set_settings_callback(lambda: self.open_settings(PanelType.TOGGLES)) self._layouts[MainState.SETTINGS].set_callbacks(on_close=self._set_mode_for_state) - self._layouts[MainState.ONROAD].set_click_callback(self._on_onroad_clicked) - device.add_interactive_timeout_callback(self._set_mode_for_state) + self._layouts[MainState.HOME].set_click_callback(self._on_onroad_clicked) - def _update_layout_rects(self): - self._sidebar_rect = rl.Rectangle(self._rect.x, self._rect.y, SIDEBAR_WIDTH, self._rect.height) + if not self._is_body: + device.add_interactive_timeout_callback(self._set_mode_for_state) - x_offset = SIDEBAR_WIDTH if self._sidebar.is_visible else 0 - self._content_rect = rl.Rectangle(self._rect.y + x_offset, self._rect.y, self._rect.width - x_offset, self._rect.height) + def _update_layout_rects(self): + if self._is_body: + self._sidebar_rect = rl.Rectangle(self._rect.x, self._rect.y, self._rect.width, BODY_SIDEBAR_HEIGHT) + y_offset = BODY_SIDEBAR_HEIGHT if self._sidebar.is_visible else 0 + self._content_rect = rl.Rectangle(self._rect.x, self._rect.y + y_offset, self._rect.width, self._rect.height - y_offset) + else: + self._sidebar_rect = rl.Rectangle(self._rect.x, self._rect.y, SIDEBAR_WIDTH, self._rect.height) + x_offset = SIDEBAR_WIDTH if self._sidebar.is_visible else 0 + self._content_rect = rl.Rectangle(self._rect.x + x_offset, self._rect.y, self._rect.width - x_offset, self._rect.height) def _handle_onroad_transition(self): - if ui_state.started != self._prev_onroad: + if ui_state.started != self._prev_onroad and not self._is_body: self._prev_onroad = ui_state.started - self._set_mode_for_state() def _set_mode_for_state(self): @@ -77,7 +88,9 @@ def _set_mode_for_state(self): self._set_current_layout(MainState.ONROAD) else: self._set_current_layout(MainState.HOME) - self._sidebar.set_visible(True) + # Body sidebar always starts closed; regular sidebar starts open + if not self._is_body: + self._sidebar.set_visible(True) def _set_current_layout(self, layout: MainState): if layout != self._current_mode: @@ -102,9 +115,14 @@ def _on_onroad_clicked(self): self._sidebar.set_visible(not self._sidebar.is_visible) def _render_main_content(self): - # Render sidebar - if self._sidebar.is_visible: - self._sidebar.render(self._sidebar_rect) + if self._is_body: # overlay sidebar but recompute boundaries for proper click events + parent_rect = rl.Rectangle(self._rect.x, self._rect.y + BODY_SIDEBAR_HEIGHT, + self._rect.width, self._rect.height - BODY_SIDEBAR_HEIGHT) if self._sidebar.is_visible else None + self._layouts[self._current_mode].set_parent_rect(parent_rect) + self._layouts[self._current_mode].render(self._rect) + else: + content_rect = self._content_rect if self._sidebar.is_visible else self._rect + self._layouts[self._current_mode].render(content_rect) - content_rect = self._content_rect if self._sidebar.is_visible else self._rect - self._layouts[self._current_mode].render(content_rect) + if self._sidebar.is_visible: + self._sidebar.render(self._sidebar_rect) diff --git a/selfdrive/ui/mici/layouts/main.py b/selfdrive/ui/mici/layouts/main.py index 95258e2795d1f4..4fe8a05eed0b0a 100644 --- a/selfdrive/ui/mici/layouts/main.py +++ b/selfdrive/ui/mici/layouts/main.py @@ -6,6 +6,8 @@ from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import AugmentedRoadView from openpilot.selfdrive.ui.ui_state import device, ui_state from openpilot.selfdrive.ui.mici.layouts.onboarding import OnboardingWindow +from openpilot.selfdrive.ui.body.layouts.onroad import BodyLayout +from openpilot.selfdrive.ui.body.layouts.home_mici import MiciBodyHomeLayout from openpilot.system.ui.widgets import Widget from openpilot.system.ui.widgets.scroller import Scroller from openpilot.system.ui.lib.application import gui_app @@ -20,16 +22,21 @@ def __init__(self): self._pm = messaging.PubMaster(['bookmarkButton']) + self._is_body = ui_state.is_body self._prev_onroad = False self._prev_standstill = False + self._prev_joystick_debug_mode = False self._onroad_time_delay: float | None = None self._setup = False # Initialize widgets - self._home_layout = MiciHomeLayout() + self._home_layout = MiciBodyHomeLayout() if self._is_body else MiciHomeLayout() + if self._is_body: + self._onroad_layout = BodyLayout() + else: + self._onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked) self._alerts_layout = MiciOffroadAlerts() self._settings_layout = SettingsLayout() - self._onroad_layout = AugmentedRoadView(bookmark_callback=self._on_bookmark_clicked) # Initialize widget rects for widget in (self._home_layout, self._settings_layout, self._alerts_layout, self._onroad_layout): @@ -94,7 +101,8 @@ def _handle_transitions(self): # FIXME: these two pops can interrupt user interacting in the settings if self._onroad_time_delay is not None and rl.get_time() - self._onroad_time_delay >= ONROAD_DELAY: - gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout)) + if not self._is_body or ui_state.joystick_debug_mode: + gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout)) self._onroad_time_delay = None # When car leaves standstill, pop nav stack and scroll to onroad @@ -103,12 +111,25 @@ def _handle_transitions(self): gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout)) self._prev_standstill = CS.standstill + # On body, react to joystick debug mode changes while onroad: + # - Connected (False->True): scroll to onroad to show awake face + # - Disconnected (True->False): scroll to home so user can reconnect + if self._is_body and ui_state.joystick_debug_mode != self._prev_joystick_debug_mode: + if ui_state.joystick_debug_mode: + gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout)) + else: + self._scroll_to(self._home_layout) + self._prev_joystick_debug_mode = ui_state.joystick_debug_mode + def _on_interactive_timeout(self): # Don't pop if onboarding if gui_app.widget_in_stack(self._onboarding_window): return if ui_state.started: + # On body without joystick, stay on home screen + if self._is_body and not ui_state.joystick_debug_mode: + self._scroll_to(self._onroad_layout) # Don't pop if at standstill if not ui_state.sm["carState"].standstill: gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout)) diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index 8225efabf9af5a..b2783efebd25c6 100644 --- a/selfdrive/ui/soundd.py +++ b/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 @@ -116,7 +115,9 @@ def get_sound_data(self, frames): # get "frames" worth of data from the current def callback(self, data_out: np.ndarray, frames: int, time, status) -> None: if status: cloudlog.warning(f"soundd stream over/underflow: {status}") - data_out[:frames, 0] = self.get_sound_data(frames) + sound = self.get_sound_data(frames) + np.clip(sound, -1.0, 1.0, out=sound) + data_out[:frames, 0] = sound def update_alert(self, new_alert): current_alert_played_once = self.current_alert == AudibleAlert.none or self.current_sound_frame > len(self.loaded_sounds[self.current_alert]) @@ -128,6 +129,11 @@ def update_alert(self, new_alert): self.current_sound_frame = 0 def get_audible_alert(self, sm): + if sm.updated['soundRequest']: + new_alert = sm['soundRequest'].sound.raw + if new_alert != AudibleAlert.none: + self.update_alert(new_alert) + if sm.updated['selfdriveState']: new_alert = sm['selfdriveState'].alertSound.raw self.update_alert(new_alert) @@ -153,7 +159,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) diff --git a/selfdrive/ui/ui.py b/selfdrive/ui/ui.py index e3cac2618e84e1..07f5889b8ec946 100755 --- a/selfdrive/ui/ui.py +++ b/selfdrive/ui/ui.py @@ -21,8 +21,21 @@ def main(): else: MiciMainLayout() + prev_is_body = ui_state.is_body + for should_render in gui_app.render(): ui_state.update() + + # rebuild UI if device fingerprints body + if ui_state.is_body != prev_is_body: + prev_is_body = ui_state.is_body + gui_app._nav_stack.clear() + gui_app._nav_stack_ticks.clear() + if BIG_UI: + MainLayout() + else: + MiciMainLayout() + if should_render: # reaffine after power save offlines our core if TICI and os.sched_getaffinity(0) != cores: diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index 30a656509551e0..73bd839ce8ba37 100644 --- a/selfdrive/ui/ui_state.py +++ b/selfdrive/ui/ui_state.py @@ -54,6 +54,7 @@ def _initialize(self): "carOutput", "carControl", "liveParameters", + "testJoystick", "rawAudioData", ] ) @@ -74,6 +75,7 @@ def _initialize(self): self.started: bool = False self.ignition: bool = False self.recording_audio: bool = False + self.joystick_debug_mode: bool = False self.panda_type: log.PandaState.PandaType = log.PandaState.PandaType.unknown self.personality: log.LongitudinalPersonality = log.LongitudinalPersonality.standard self.has_longitudinal_control: bool = False @@ -103,6 +105,10 @@ def is_onroad(self) -> bool: def is_offroad(self) -> bool: return not self.started + @property + def is_body(self) -> bool: + return self.CP is not None and self.CP.notCar + def update(self) -> None: self.prime_state.start() # start thread after manager forks ui self.sm.update(0) @@ -138,6 +144,7 @@ def _update_state(self) -> None: # Update recording audio state self.recording_audio = self.params.get_bool("RecordAudio") and self.started + self.joystick_debug_mode = self.params.get_bool("JoystickDebugMode") self.is_metric = self.params.get_bool("IsMetric") self.always_on_dm = self.params.get_bool("AlwaysOnDM") diff --git a/selfdrive/ui/widgets/pairing_dialog.py b/selfdrive/ui/widgets/pairing_dialog.py index 1ff550e4b6d524..96a8fb8cebcc35 100644 --- a/selfdrive/ui/widgets/pairing_dialog.py +++ b/selfdrive/ui/widgets/pairing_dialog.py @@ -27,6 +27,12 @@ def __init__(self): self.last_qr_generation = float('-inf') self._close_btn = IconButton(gui_app.texture("icons/close.png", 80, 80)) self._close_btn.set_click_callback(gui_app.pop_widget) + self._title = tr("Pair your device to your comma account") + self._instructions = [ + tr("Go to https://connect.comma.ai on your phone"), + tr("Click \"add new device\" and scan the QR code on the right"), + tr("Bookmark connect.comma.ai to your home screen to use it like an app"), + ] def _get_pairing_url(self) -> str: try: @@ -89,11 +95,10 @@ def _render(self, rect: rl.Rectangle) -> int: y += close_size + 40 # Title - title = tr("Pair your device to your comma account") title_font = gui_app.font(FontWeight.NORMAL) left_width = int(content_rect.width * 0.5 - 15) - title_wrapped = wrap_text(title_font, title, 75, left_width) + title_wrapped = wrap_text(title_font, self._title, 75, left_width) rl.draw_text_ex(title_font, "\n".join(title_wrapped), rl.Vector2(content_rect.x, y), 75, 0.0, rl.BLACK) y += len(title_wrapped) * 75 + 60 @@ -113,16 +118,10 @@ def _render(self, rect: rl.Rectangle) -> int: return -1 def _render_instructions(self, rect: rl.Rectangle) -> None: - instructions = [ - tr("Go to https://connect.comma.ai on your phone"), - tr("Click \"add new device\" and scan the QR code on the right"), - tr("Bookmark connect.comma.ai to your home screen to use it like an app"), - ] - font = gui_app.font(FontWeight.BOLD) y = rect.y - for i, text in enumerate(instructions): + for i, text in enumerate(self._instructions): circle_radius = 25 circle_x = rect.x + circle_radius + 15 text_x = rect.x + circle_radius * 2 + 40 diff --git a/system/athena/athenad.py b/system/athena/athenad.py index b52ef21ba63702..6b925fc2d24df3 100755 --- a/system/athena/athenad.py +++ b/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"], ["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 diff --git a/system/loggerd/encoderd.cc b/system/loggerd/encoderd.cc index 9d4b81a3f90230..656934f4a3d269 100644 --- a/system/loggerd/encoderd.cc +++ b/system/loggerd/encoderd.cc @@ -151,6 +151,76 @@ void encoderd_thread(const LogCameraInfo (&cameras)[N]) { } } +// Map param value to stream camera config +const LogCameraInfo *find_stream_camera(const std::string &name) { + if (name == "driver") return &stream_driver_camera_info; + return &stream_wide_road_camera_info; // default +} + +void stream_encoderd_thread() { + Params params; + + // Wait for cameras to be available + std::set available_streams; + while (!do_exit) { + available_streams = VisionIpcClient::getAvailableStreams("camerad", false); + if (!available_streams.empty()) break; + util::sleep_for(100); + } + + std::string active_camera = params.get("LivestreamCamera"); + if (active_camera.empty()) active_camera = "driver"; + + while (!do_exit) { + const LogCameraInfo *cam_info = find_stream_camera(active_camera); + + // Check that the requested camera stream is available + if (available_streams.find(cam_info->stream_type) == available_streams.end()) { + LOGE("stream encoder: camera %s not available, falling back", active_camera.c_str()); + active_camera = "wideRoad"; + cam_info = find_stream_camera(active_camera); + } + + VisionIpcClient vipc_client("camerad", cam_info->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 %s %zux%zu", active_camera.c_str(), buf_info.width, buf_info.height); + assert(buf_info.width > 0 && buf_info.height > 0); + + const auto &encoder_info = cam_info->encoder_infos[0]; + auto encoder = std::make_unique(encoder_info, buf_info.width, buf_info.height); + encoder->encoder_open(); + + while (!do_exit) { + // Check for camera switch request + std::string requested = params.get("LivestreamCamera"); + if (!requested.empty() && requested != active_camera) { + LOGW("stream encoder switching from %s to %s", active_camera.c_str(), requested.c_str()); + active_camera = requested; + break; // break to reinit encoder with new camera + } + + VisionIpcBufExtra extra; + VisionBuf *buf = vipc_client.recv(&extra); + if (buf == nullptr) continue; + + // detect loop around and drop the frames + if (buf->get_frame_id() != extra.frame_id) continue; + + int out_id = encoder->encode_frame(buf, &extra); + if (out_id == -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 +232,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(); } else { LOGE("Argument '%s' is not supported", arg1.c_str()); } diff --git a/system/loggerd/loggerd.h b/system/loggerd/loggerd.h index 6aa0c8be40b96f..22ad2f88b8abf9 100644 --- a/system/loggerd/loggerd.h +++ b/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}; } }; @@ -169,4 +169,4 @@ const LogCameraInfo stream_driver_camera_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_wide_road_camera_info, stream_driver_camera_info}; diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 7e96b7776a4f5f..1ffa22be3f0906 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -76,11 +76,11 @@ def and_(*fns): PythonProcess("webcamerad", "tools.webcam.camerad", driverview, enabled=WEBCAM), PythonProcess("proclogd", "system.proclogd", only_onroad, enabled=platform.system() != "Darwin"), PythonProcess("journald", "system.journald", only_onroad, platform.system() != "Darwin"), - PythonProcess("micd", "system.micd", iscar), + PythonProcess("micd", "system.micd", only_onroad), PythonProcess("timed", "system.timed", always_run, enabled=not PC), PythonProcess("modeld", "selfdrive.modeld.modeld", only_onroad), - PythonProcess("dmonitoringmodeld", "selfdrive.modeld.dmonitoringmodeld", driverview, enabled=(WEBCAM or not PC)), + PythonProcess("dmonitoringmodeld", "selfdrive.modeld.dmonitoringmodeld", and_(driverview, iscar), enabled=(WEBCAM or not PC)), PythonProcess("sensord", "system.sensord.sensord", only_onroad, enabled=not PC), PythonProcess("ui", "selfdrive.ui.ui", always_run, restart_if_crash=True), @@ -94,7 +94,7 @@ def and_(*fns): PythonProcess("selfdrived", "selfdrive.selfdrived.selfdrived", only_onroad), PythonProcess("card", "selfdrive.car.card", only_onroad), PythonProcess("deleter", "system.loggerd.deleter", always_run), - PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(WEBCAM or not PC)), + PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", and_(driverview, iscar), enabled=(WEBCAM or not PC)), PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI), PythonProcess("pandad", "selfdrive.pandad.pandad", always_run), PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad), @@ -112,10 +112,10 @@ def and_(*fns): PythonProcess("statsd", "system.statsd", always_run), PythonProcess("feedbackd", "selfdrive.ui.feedback.feedbackd", only_onroad), + PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar), + # debug procs NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar), - PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar), - PythonProcess("webjoystick", "tools.bodyteleop.web", notcar), PythonProcess("joystick", "tools.joystick.joystick_control", and_(joystick, iscar)), ] diff --git a/system/webrtc/device/audio.py b/system/webrtc/device/audio.py new file mode 100644 index 00000000000000..440a5f4af6953a --- /dev/null +++ b/system/webrtc/device/audio.py @@ -0,0 +1,188 @@ +import asyncio +import fractions +import logging +import threading +import time +from collections import deque + +import numpy as np +from av import AudioFrame, AudioResampler +from aiortc.mediastreams import AudioStreamTrack, MediaStreamError + +from cereal import car, messaging + +AUDIO_PTIME = 0.020 +MIC_SAMPLE_RATE = 16000 +SPEAKER_SAMPLE_RATE = 48000 +LOGGER = logging.getLogger("webrtcd") + +AudibleAlert = car.CarControl.HUDControl.AudibleAlert +BODY_SOUND_ALERTS = { + "engage": AudibleAlert.engage, + "disengage": AudibleAlert.disengage, + "prompt": AudibleAlert.prompt, + "warning": AudibleAlert.warningImmediate, +} +BODY_SOUND_NAMES = frozenset(BODY_SOUND_ALERTS) + + +class PcmBuffer: + def __init__(self, dtype=np.int16): + self._chunks: deque[np.ndarray] = deque() + self._offset = 0 + self._size = 0 + self._dtype = dtype + + def push(self, samples: np.ndarray): + if samples.size == 0: + return + chunk = np.ascontiguousarray(samples, dtype=self._dtype) + self._chunks.append(chunk) + self._size += chunk.size + + def available(self) -> int: + return self._size + + def pop(self, size: int) -> np.ndarray: + out = np.zeros(size, dtype=self._dtype) + written = 0 + + while written < size and self._chunks: + chunk = self._chunks[0] + remaining = chunk.size - self._offset + take = min(size - written, remaining) + out[written:written + take] = chunk[self._offset:self._offset + take] + written += take + self._offset += take + + if self._offset >= chunk.size: + self._chunks.popleft() + self._offset = 0 + + self._size -= written + return out + + +class DeviceToWebAudioTrack(AudioStreamTrack): + def __init__(self): + super().__init__() + self._loop = asyncio.get_running_loop() + self._buffer = PcmBuffer() + self._buffer_event = asyncio.Event() + self._sample_rate = MIC_SAMPLE_RATE + self._samples_per_frame = int(self._sample_rate * AUDIO_PTIME) + self._time_base = fractions.Fraction(1, self._sample_rate) + self._running = True + self._thread = threading.Thread(target=self._poll_cereal, daemon=True) + self._thread.start() + + def _push_samples(self, samples: np.ndarray): + self._buffer.push(samples) + self._buffer_event.set() + + def _poll_cereal(self): + sm = messaging.SubMaster(['rawAudioData']) + while self._running: + sm.update(20) + if not sm.updated['rawAudioData']: + continue + + raw_bytes = sm['rawAudioData'].data + if not raw_bytes: + continue + + # .copy() required: frombuffer is a view over the cereal message buffer, invalidated by next sm.update() + samples = np.frombuffer(raw_bytes, dtype=np.int16).copy() + self._loop.call_soon_threadsafe(self._push_samples, samples) + + async def _next_frame_samples(self) -> np.ndarray: + while self.readyState == "live": + if self._buffer.available() >= self._samples_per_frame: + return self._buffer.pop(self._samples_per_frame) + + await self._buffer_event.wait() + self._buffer_event.clear() + + raise MediaStreamError + + async def _next_timestamp(self) -> int: + if not hasattr(self, "_timestamp"): + self._start = time.monotonic() + self._timestamp = 0 + return self._timestamp + + self._timestamp += self._samples_per_frame + wait = self._start + (self._timestamp / self._sample_rate) - time.monotonic() + if wait > 0: + await asyncio.sleep(wait) + return self._timestamp + + async def recv(self): + if self.readyState != "live": + raise MediaStreamError + + frame_samples = await self._next_frame_samples() + timestamp = await self._next_timestamp() + + frame = AudioFrame(format="s16", layout="mono", samples=self._samples_per_frame) + frame.planes[0].update(frame_samples.tobytes()) + frame.pts = timestamp + frame.sample_rate = self._sample_rate + frame.time_base = self._time_base + return frame + + def stop(self): + super().stop() + self._running = False + try: + self._loop.call_soon_threadsafe(self._buffer_event.set) + except RuntimeError: + self._buffer_event.set() + + +class WebToDeviceAudioTrack: + def __init__(self): + self._pm = messaging.PubMaster(['soundRequest', 'webrtcAudioData']) + self._task: asyncio.Task | None = None + + def play_sound(self, sound_name: str): + msg = messaging.new_message('soundRequest') + msg.soundRequest.sound = BODY_SOUND_ALERTS[sound_name] + self._pm.send('soundRequest', msg) + + def start_track(self, track): + self._task = asyncio.create_task(self._consume_track(track)) + + def _send_audio_data(self, data: bytes): + msg = messaging.new_message('webrtcAudioData') + msg.webrtcAudioData.data = data + msg.webrtcAudioData.sampleRate = SPEAKER_SAMPLE_RATE + self._pm.send('webrtcAudioData', msg) + + async def _consume_track(self, track): + resampler = AudioResampler(format='s16', layout='mono', rate=SPEAKER_SAMPLE_RATE) + try: + while True: + frame = await track.recv() + for resampled in resampler.resample(frame): + self._send_audio_data(resampled.planes[0].to_bytes()) + except MediaStreamError: + LOGGER.info("Incoming browser audio track ended") + except asyncio.CancelledError: + raise + except Exception: + LOGGER.exception("BodySpeaker track consumption error") + + async def stop(self): + if self._task is not None and not self._task.done(): + self._task.cancel() + try: + await self._task + except asyncio.CancelledError: + pass + self._task = None + + +# Backwards-compatible aliases while older call sites are updated. +BodyMicAudioTrack = DeviceToWebAudioTrack +BodySpeaker = WebToDeviceAudioTrack diff --git a/system/webrtc/device/video.py b/system/webrtc/device/video.py index 50feab4f4a910d..d1a5e60885c287 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,38 +8,95 @@ 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, +]) + + +def _escape_rbsp(data: bytes) -> bytearray: + """ + Prevents frame bytes that might escape into NAL start code. + Adds 0x03 after two consecutive 0x00 0x00 to escape this. + """ + out = bytearray() + zeros = 0 + for b in data: + if zeros >= 2 and b <= 3: + out.append(3) + zeros = 0 + zeros = zeros + 1 if b == 0 else 0 + out.append(b) + return out + + +def create_timing_sei(capture_ms: float, encode_ms: float, send_delay_ms: float, send_wall_ms: float) -> bytes: + """Build an H.264 SEI NAL (user_data_unregistered) carrying frame timing.""" + ts_data = struct.pack('>4d', capture_ms, encode_ms, send_delay_ms, send_wall_ms) + sei_payload = TIMING_SEI_UUID + ts_data # 16 + 32 = 48 bytes + + # payload_type=5, payload_size=48, then RBSP stop bit + rbsp = bytes([5, len(sei_payload)]) + sei_payload + bytes([0x80]) + escaped = _escape_rbsp(rbsp) + + # start-code (4 bytes) + NAL header (forbidden=0, ref_idc=0, type=6 SEI) + return b'\x00\x00\x00\x01\x06' + bytes(escaped) + class LiveStreamVideoStreamTrack(TiciVideoStreamTrack): - camera_to_sock_mapping = { - "driver": "livestreamDriverEncodeData", - "wideRoad": "livestreamWideRoadEncodeData", - "road": "livestreamRoadEncodeData", + camera_config = { + "driver": (DT_DMON, "livestreamDriverEncodeData"), + "wideRoad": (DT_MDL, "livestreamWideRoadEncodeData"), } def __init__(self, camera_type: str): - dt = DT_DMON if camera_type == "driver" else DT_MDL + dt, _ = self.camera_config[camera_type] super().__init__(camera_type, dt) - self._sock = messaging.sub_sock(self.camera_to_sock_mapping[camera_type], conflate=True) - self._pts = 0 + self._camera_type = "" + self._sock = None + self._set_camera(camera_type) self._t0_ns = time.monotonic_ns() + self.timing_sei_enabled = False - async def recv(self): + def _set_camera(self, camera_type: str): + self._camera_type = camera_type + _, sock_name = self.camera_config[camera_type] + self._sock = messaging.sub_sock(sock_name, conflate=True) + + def switch_camera(self, camera_type: str): + if camera_type not in self.camera_config or camera_type == self._camera_type: + return + self._set_camera(camera_type) + + async def _recv_message(self): while True: msg = messaging.recv_one_or_none(self._sock) if msg is not None: - break + return msg await asyncio.sleep(0.005) - evta = getattr(msg, msg.which()) + 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 - packet = av.Packet(evta.header + evta.data) - packet.time_base = self._time_base + capture_ms = (encode_data.idx.timestampEof - encode_data.idx.timestampSof) / 1e6 + encode_ms = (msg.logMonoTime - encode_data.idx.timestampEof) / 1e6 + send_delay_ms = (time.monotonic_ns() - msg.logMonoTime) / 1e6 + send_wall_ms = time.time() * 1000 # noqa: TID251 + sei_nal = create_timing_sei(capture_ms, encode_ms, send_delay_ms, send_wall_ms) + return encode_data.header + sei_nal + encode_data.data - 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) + async def recv(self): + msg = await self._recv_message() + packet = av.Packet(self._build_frame_data(msg)) + packet.time_base = self._time_base + 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: diff --git a/system/webrtc/tests/test_audio.py b/system/webrtc/tests/test_audio.py new file mode 100644 index 00000000000000..760c02929af12e --- /dev/null +++ b/system/webrtc/tests/test_audio.py @@ -0,0 +1,104 @@ +import asyncio +import math +import time +from types import SimpleNamespace + +import numpy as np +import pytest +from aiortc.mediastreams import VideoStreamTrack + +from openpilot.system.webrtc.device import audio as audio_module +from openpilot.system.webrtc.webrtcd import StreamSession + + +AUDIO_RECVONLY_OFFER_SDP = """v=0 +o=- 3910210904 3910210904 IN IP4 0.0.0.0 +s=- +t=0 0 +a=group:BUNDLE 0 +a=msid-semantic:WMS * +m=audio 9 UDP/TLS/RTP/SAVPF 96 0 8 +c=IN IP4 0.0.0.0 +a=recvonly +a=extmap:1 urn:ietf:params:rtp-hdrext:sdes:mid +a=extmap:2 urn:ietf:params:rtp-hdrext:ssrc-audio-level +a=mid:0 +a=msid:eb1d3f1a-569a-465f-b419-319477bfded6 e44eecb2-1a04-4547-97d8-481389f50d5b +a=rtcp:9 IN IP4 0.0.0.0 +a=rtcp-mux +a=ssrc:1233332626 cname:ca4dede8-4994-4a6d-9ae3-923b28177ca5 +a=rtpmap:96 opus/48000/2 +a=rtpmap:0 PCMU/8000 +a=rtpmap:8 PCMA/8000 +a=ice-ufrag:1234 +a=ice-pwd:1234 +a=fingerprint:sha-256 40:4B:14:CF:70:B8:67:E1:B1:FF:7E:F9:22:6E:60:7D:73:B5:1E:38:4B:10:20:9C:CD:1C:47:02:52:ED:45:25 +a=setup:actpass""" + + +def tone_chunk(samples: int = 800, sample_rate: int = audio_module.MIC_SAMPLE_RATE) -> bytes: + t = np.arange(samples, dtype=np.float32) / sample_rate + pcm = (0.4 * np.sin(2 * math.pi * 440.0 * t) * 32767).astype(np.int16) + return pcm.tobytes() + + +class FakeSubMaster: + def __init__(self, payload: bytes): + self.updated = {'rawAudioData': False} + self._payload = payload + self._msg = SimpleNamespace(data=b'', sampleRate=audio_module.MIC_SAMPLE_RATE) + + def update(self, timeout: int): + time.sleep(0.005) + self.updated['rawAudioData'] = True + self._msg.data = self._payload + + def __getitem__(self, key: str): + assert key == 'rawAudioData' + return self._msg + + +async def wait_for_buffer(track: audio_module.DeviceToWebAudioTrack, timeout: float = 1.0): + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + if track._buffer.available() >= track._samples_per_frame: + return + await asyncio.sleep(0.01) + raise TimeoutError("audio track did not buffer a full frame") + + +@pytest.mark.asyncio +async def test_device_to_web_audio_track_reads_raw_audio(monkeypatch): + payload = tone_chunk() + monkeypatch.setattr(audio_module.messaging, "SubMaster", lambda services: FakeSubMaster(payload)) + + track = audio_module.DeviceToWebAudioTrack() + try: + await wait_for_buffer(track) + frame = await asyncio.wait_for(track.recv(), timeout=1) + finally: + track.stop() + track._thread.join(timeout=1) + + pcm = frame.to_ndarray() + assert frame.sample_rate == audio_module.MIC_SAMPLE_RATE + assert frame.samples == int(audio_module.MIC_SAMPLE_RATE * audio_module.AUDIO_PTIME) + assert pcm.shape[-1] == frame.samples + assert np.abs(pcm).sum() > 0 + + +@pytest.mark.asyncio +async def test_stream_session_uses_device_to_web_audio_track(monkeypatch): + payload = tone_chunk() + monkeypatch.setattr(audio_module.messaging, "SubMaster", lambda services: FakeSubMaster(payload)) + monkeypatch.setattr("openpilot.system.webrtc.device.video.LiveStreamVideoStreamTrack", lambda camera_type: VideoStreamTrack()) + monkeypatch.setattr("openpilot.system.webrtc.webrtcd.Params", lambda: SimpleNamespace(get=lambda key: None)) + + session = StreamSession(AUDIO_RECVONLY_OFFER_SDP, [], [], [], audio_output=None, debug_mode=False) + try: + assert isinstance(session.outgoing_audio_track, audio_module.DeviceToWebAudioTrack) + finally: + if session.outgoing_audio_track is not None: + session.outgoing_audio_track.stop() + session.outgoing_audio_track._thread.join(timeout=1) + await session.stream.stop() diff --git a/system/webrtc/tests/test_stream_session.py b/system/webrtc/tests/test_stream_session.py index f44d217d58ced6..21122855a6ba67 100644 --- a/system/webrtc/tests/test_stream_session.py +++ b/system/webrtc/tests/test_stream_session.py @@ -84,4 +84,3 @@ def test_livestream_track(self, mocker): start_pts = packet.pts assert abs(i + packet.pts - (start_pts + (((time.monotonic_ns() - start_ns) * VIDEO_CLOCK_RATE) // 1_000_000_000))) < 450 #5ms assert packet.size == 0 - diff --git a/system/webrtc/webrtcd.py b/system/webrtc/webrtcd.py index d2c90cafb5b2e6..5b45588f88e4dc 100755 --- a/system/webrtc/webrtcd.py +++ b/system/webrtc/webrtcd.py @@ -3,8 +3,11 @@ import argparse import asyncio import json -import uuid import logging +import os +import ssl +import subprocess +import uuid from dataclasses import dataclass, field from typing import Any, TYPE_CHECKING @@ -20,6 +23,7 @@ from openpilot.system.webrtc.schema import generate_field from cereal import messaging, log +from openpilot.common.params import Params class CerealOutgoingMessageProxy: @@ -115,21 +119,55 @@ async def add_services_if_needed(self, services): self.sock[service] = messaging.pub_sock(service) +def validate_body_sound_name(sound_name: Any) -> str: + from openpilot.system.webrtc.device.audio import BODY_SOUND_NAMES + + if sound_name not in BODY_SOUND_NAMES: + raise ValueError(f"unsupported body sound: {sound_name}") + return sound_name + + class StreamSession: shared_pub_master = DynamicPubMaster([]) - def __init__(self, sdp: str, cameras: list[str], incoming_services: list[str], outgoing_services: list[str], debug_mode: bool = False): - from aiortc.mediastreams import VideoStreamTrack + def __init__(self, sdp: str, cameras: list[str], incoming_services: list[str], outgoing_services: list[str], + audio_output=None, debug_mode: bool = False): + from aiortc.mediastreams import AudioStreamTrack, VideoStreamTrack + from openpilot.system.webrtc.device.audio import DeviceToWebAudioTrack, WebToDeviceAudioTrack from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack from teleoprtc import WebRTCAnswerBuilder from teleoprtc.info import parse_info_from_offer + self.logger = logging.getLogger("webrtcd") config = parse_info_from_offer(sdp) builder = WebRTCAnswerBuilder(sdp) - assert len(cameras) == config.n_expected_camera_tracks, "Incoming stream has misconfigured number of video tracks" - for cam in cameras: - builder.add_video_stream(cam, LiveStreamVideoStreamTrack(cam) if not debug_mode else VideoStreamTrack()) + # Use the camera the encoder is currently active on, so reconnects don't get a blank stream + active_camera = Params().get("LivestreamCamera") or "driver" + if active_camera not in ("driver", "wideRoad"): + active_camera = "driver" + self.video_track = LiveStreamVideoStreamTrack(active_camera) if not debug_mode else VideoStreamTrack() + builder.add_video_stream(active_camera, self.video_track) + + self.outgoing_audio_track: DeviceToWebAudioTrack | None = None + if config.expected_audio_track: + try: + if debug_mode: + builder.add_audio_stream(AudioStreamTrack()) + else: + self.outgoing_audio_track = DeviceToWebAudioTrack() + builder.add_audio_stream(self.outgoing_audio_track) + except Exception: + self.logger.exception("Failed to initialize body microphone track") + + self.audio_output: WebToDeviceAudioTrack | None = audio_output if (config.incoming_audio_track or config.incoming_datachannel) else None + if self.audio_output is None and (config.incoming_audio_track or config.incoming_datachannel): + try: + self.audio_output = WebToDeviceAudioTrack() + except Exception: + self.logger.exception("Failed to initialize body speaker output") + if config.incoming_audio_track: + builder.offer_to_receive_audio_stream() self.stream = builder.stream() self.identifier = str(uuid.uuid4()) @@ -145,54 +183,125 @@ def __init__(self, sdp: str, cameras: list[str], incoming_services: list[str], o self.outgoing_bridge_runner = CerealProxyRunner(self.outgoing_bridge) self.run_task: asyncio.Task | None = None - self.logger = logging.getLogger("webrtcd") - self.logger.info("New stream session (%s), cameras %s, incoming services %s, outgoing services %s", - self.identifier, cameras, incoming_services, outgoing_services) + self.cleaned_up = False + self.logger.info( + "New stream session (%s), cameras %s, incoming services %s, outgoing services %s, send audio %s, receive audio %s", + self.identifier, cameras, incoming_services, outgoing_services, config.expected_audio_track, config.incoming_audio_track, + ) def start(self): self.run_task = asyncio.create_task(self.run()) def stop(self): - if self.run_task.done(): + if self.run_task is None or self.run_task.done(): return self.run_task.cancel() self.run_task = None - asyncio.run(self.post_run_cleanup()) + try: + loop = asyncio.get_running_loop() + except RuntimeError: + asyncio.run(self.post_run_cleanup()) + else: + loop.create_task(self.post_run_cleanup()) async def get_answer(self): return await self.stream.start() + def _handle_clock_sync(self, payload: dict): + import time as _time + data = payload.get("data", {}) + if data.get("action") != "ping": + return + pong = json.dumps({ + "type": "clockSync", + "data": { + "action": "pong", + "browserSendTime": data.get("browserSendTime"), + "deviceTime": _time.time() * 1000, # noqa: TID251 + } + }) + try: + if self.stream.has_messaging_channel(): + self.stream.get_messaging_channel().send(pong) + except Exception: + self.logger.warning("Failed to send clockSync pong") + + def _handle_switch_camera(self, payload: dict): + data = payload.get("data") + if not isinstance(data, dict): + return + camera = data.get("camera") + if camera in ("driver", "wideRoad"): + try: + Params().put("LivestreamCamera", camera) + except Exception: + self.logger.warning("Failed to write LivestreamCamera param") + if hasattr(self, 'video_track') and hasattr(self.video_track, 'switch_camera'): + self.video_track.switch_camera(camera) + self.logger.info("Switched livestream camera to %s", camera) + async def message_handler(self, message: bytes): - assert self.incoming_bridge is not None try: - self.incoming_bridge.send(message) + payload = json.loads(message) if isinstance(message, (bytes, str)) else None + if isinstance(payload, dict) and payload.get("type") == "switchCamera": + self._handle_switch_camera(payload) + return + if isinstance(payload, dict) and payload.get("type") == "clockSync": + self._handle_clock_sync(payload) + return + if isinstance(payload, dict) and payload.get("type") == "enableTimingSei": + enabled = bool(payload.get("data", {}).get("enabled")) + if hasattr(self.video_track, 'timing_sei_enabled'): + self.video_track.timing_sei_enabled = enabled + self.logger.info("Timing SEI %s", "enabled" if enabled else "disabled") + return + if self.incoming_bridge is not None: + self.incoming_bridge.send(message) except Exception: self.logger.exception("Cereal incoming proxy failure") async def run(self): try: await self.stream.wait_for_connection() + if self.audio_output is not None and self.stream.has_incoming_audio_track(): + self.audio_output.start_track(self.stream.get_incoming_audio_track()) if self.stream.has_messaging_channel(): if self.incoming_bridge is not None: await self.shared_pub_master.add_services_if_needed(self.incoming_bridge_services) - self.stream.set_message_handler(self.message_handler) + self.stream.set_message_handler(self.message_handler) if self.outgoing_bridge_runner is not None: channel = self.stream.get_messaging_channel() self.outgoing_bridge_runner.proxy.add_channel(channel) self.outgoing_bridge_runner.start() + # Tell the client which camera is currently active + if self.stream.has_messaging_channel(): + try: + active = getattr(self.video_track, '_camera_type', 'driver') + self.stream.get_messaging_channel().send(json.dumps({"type": "activeCamera", "data": {"camera": active}})) + except Exception: + pass self.logger.info("Stream session (%s) connected", self.identifier) await self.stream.wait_for_disconnection() - await self.post_run_cleanup() self.logger.info("Stream session (%s) ended", self.identifier) except Exception: self.logger.exception("Stream session failure") + finally: + await self.post_run_cleanup() async def post_run_cleanup(self): + if self.cleaned_up: + return + self.cleaned_up = True await self.stream.stop() if self.outgoing_bridge is not None: self.outgoing_bridge_runner.stop() + if self.outgoing_audio_track is not None: + self.outgoing_audio_track.stop() + if self.audio_output is not None: + await self.audio_output.stop() + Params().put_bool("JoystickDebugMode", False) @dataclass @@ -203,18 +312,120 @@ class StreamRequestBody: bridge_services_out: list[str] = field(default_factory=list) +@dataclass +class SoundRequestBody: + sound: str + +def _add_cors_headers(request: 'web.Request', response: 'web.Response'): + response.headers["Access-Control-Allow-Origin"] = "*" + response.headers["Access-Control-Allow-Headers"] = "Content-Type" + response.headers["Access-Control-Allow-Methods"] = "POST, OPTIONS" + response.headers["Access-Control-Allow-Private-Network"] = "true" + + +@web.middleware +async def cors_middleware(request: 'web.Request', handler): + try: + response = await handler(request) + except web.HTTPException as ex: + _add_cors_headers(request, ex) + raise + _add_cors_headers(request, response) + return response + + +async def stream_options(request: 'web.Request'): + response = web.Response() + _add_cors_headers(request, response) + return response + + +REQUIRED_VIDEO_CODEC = "H264" + +def _validate_sdp_video_codecs(sdp: str): + import aiortc.sdp + desc = aiortc.sdp.SessionDescription.parse(sdp) + required_mime = f"video/{REQUIRED_VIDEO_CODEC}" + for m in desc.media: + if m.kind != "video": + continue + offered_mimes = {c.mimeType for c in m.rtp.codecs} + if required_mime not in offered_mimes: + raise web.HTTPBadRequest( + text=json.dumps({"error": "unsupported_codec", "message": f"Frontend must offer {REQUIRED_VIDEO_CODEC} via setCodecPreferences()"}), + content_type="application/json", + ) + + +def _cleanup_stale_streams(stream_dict: dict): + stale = [sid for sid, s in stream_dict.items() if s.run_task is None or s.run_task.done()] + for sid in stale: + del stream_dict[sid] + + +def _get_active_streams(stream_dict: dict) -> list[str]: + return [sid for sid, s in stream_dict.items() if s.run_task is not None and not s.run_task.done()] + + async def get_stream(request: 'web.Request'): - stream_dict, debug_mode = request.app['streams'], request.app['debug'] - raw_body = await request.json() - body = StreamRequestBody(**raw_body) + logger = logging.getLogger("webrtcd") + try: + stream_dict, debug_mode = request.app['streams'], request.app['debug'] + + _cleanup_stale_streams(stream_dict) + + active_streams = _get_active_streams(stream_dict) + if active_streams: + raise web.HTTPConflict( + text=json.dumps({"error": "already_connected", "message": "Another device is already connected to the stream"}), + content_type="application/json", + ) + + raw_body = await request.json() + body = StreamRequestBody(**raw_body) + _validate_sdp_video_codecs(body.sdp) + + session = StreamSession(body.sdp, body.cameras, body.bridge_services_in, body.bridge_services_out, + request.app['body_audio_output'], debug_mode) + answer = await session.get_answer() + session.start() + Params().put_bool("JoystickDebugMode", True) + + stream_dict[session.identifier] = session - session = StreamSession(body.sdp, body.cameras, body.bridge_services_in, body.bridge_services_out, debug_mode) - answer = await session.get_answer() - session.start() + active_camera = getattr(session.video_track, '_camera_type', 'driver') + response = web.json_response({"sdp": answer.sdp, "type": answer.type, "activeCamera": active_camera}) + _add_cors_headers(request, response) + return response + except web.HTTPException: + raise + except Exception: + logger.exception("Error in /stream handler") + raise - stream_dict[session.identifier] = session - return web.json_response({"sdp": answer.sdp, "type": answer.type}) +async def post_sound(request: 'web.Request'): + try: + raw_body = await request.json() + body = SoundRequestBody(**raw_body) + sound_name = validate_body_sound_name(body.sound) + except web.HTTPException: + raise + except (TypeError, ValueError, json.JSONDecodeError) as err: + raise web.HTTPBadRequest( + text=json.dumps({"error": "invalid_sound", "message": str(err)}), + content_type="application/json", + ) from err + + audio_output = request.app['body_audio_output'] + if audio_output is None: + raise web.HTTPServiceUnavailable( + text=json.dumps({"error": "audio_unavailable", "message": "Body audio output is unavailable"}), + content_type="application/json", + ) + + audio_output.play_sound(sound_name) + return web.Response(status=200, text="OK") async def get_schema(request: 'web.Request'): @@ -224,6 +435,32 @@ async def get_schema(request: 'web.Request'): schema_dict = {s: generate_field(log.Event.schema.fields[s]) for s in services} return web.json_response(schema_dict) +TRUST_HTML = """ +comma body + +
+

SSL Certificate Accepted

+

You can close this tab and return to the connect app.

+ +
""" + + +async def get_trust(request: 'web.Request'): + return web.Response(content_type="text/html", text=TRUST_HTML) + + async def post_notify(request: 'web.Request'): try: payload = await request.json() @@ -242,25 +479,88 @@ async def post_notify(request: 'web.Request'): async def on_shutdown(app: 'web.Application'): for session in app['streams'].values(): session.stop() + if app.get('body_audio_output') is not None: + await app['body_audio_output'].stop() del app['streams'] +CERT_PATH = "/data/webrtc_cert.pem" +KEY_PATH = "/data/webrtc_key.pem" + + +def create_ssl_cert(): + logger = logging.getLogger("webrtcd") + try: + proc = subprocess.run( + f'openssl req -x509 -newkey rsa:4096 -nodes -out {CERT_PATH} -keyout {KEY_PATH} ' # noqa: ISC002 + f'-days 365 -subj "/C=US/ST=California/O=commaai/OU=comma body"', + capture_output=True, shell=True, + ) + proc.check_returncode() + except subprocess.CalledProcessError as ex: + raise ValueError(f"Error creating SSL certificate:\n[stdout]\n{proc.stdout.decode()}\n[stderr]\n{proc.stderr.decode()}") from ex + logger.info("SSL certificate created") + + +def create_ssl_context(): + logger = logging.getLogger("webrtcd") + if not os.path.exists(CERT_PATH) or not os.path.exists(KEY_PATH): + logger.info("Creating SSL certificate...") + create_ssl_cert() + else: + logger.info("SSL certificate exists") + ssl_ctx = ssl.SSLContext(ssl.PROTOCOL_TLS_SERVER) + ssl_ctx.load_cert_chain(CERT_PATH, KEY_PATH) + return ssl_ctx + + def webrtcd_thread(host: str, port: int, debug: bool): + from openpilot.system.webrtc.device.audio import WebToDeviceAudioTrack + logging.basicConfig(level=logging.CRITICAL, handlers=[logging.StreamHandler()]) logging_level = logging.DEBUG if debug else logging.INFO logging.getLogger("WebRTCStream").setLevel(logging_level) logging.getLogger("webrtcd").setLevel(logging_level) - app = web.Application() + logger = logging.getLogger("webrtcd") + app = web.Application(middlewares=[cors_middleware]) app['streams'] = dict() app['debug'] = debug + try: + app['body_audio_output'] = WebToDeviceAudioTrack() + except Exception: + logger.exception("Failed to initialize shared body audio output") + app['body_audio_output'] = None app.on_shutdown.append(on_shutdown) + app.router.add_route("OPTIONS", "/stream", stream_options) + app.router.add_route("OPTIONS", "/sound", stream_options) app.router.add_post("/stream", get_stream) + app.router.add_post("/sound", post_sound) app.router.add_post("/notify", post_notify) app.router.add_get("/schema", get_schema) + app.router.add_get("/trust", get_trust) + + https_port = port + 1 + + loop = asyncio.new_event_loop() + asyncio.set_event_loop(loop) + + runner = web.AppRunner(app) + + async def start(): + await runner.setup() + + http_site = web.TCPSite(runner, host, port) + await http_site.start() + logger.info("HTTP server running on %s:%d", host, port) + + https_site = web.TCPSite(runner, host, https_port, ssl_context=create_ssl_context()) + await https_site.start() + logger.info("HTTPS server running on %s:%d", host, https_port) - web.run_app(app, host=host, port=port) + loop.run_until_complete(start()) + loop.run_forever() def main(): diff --git a/teleoprtc_repo b/teleoprtc_repo index 389815b8ca5302..2a8e152bc077e5 160000 --- a/teleoprtc_repo +++ b/teleoprtc_repo @@ -1 +1 @@ -Subproject commit 389815b8ca5302ce7c1504b7841d4eb61a8cd51b +Subproject commit 2a8e152bc077e5ba4bfa78144efc5f2eae3e581c diff --git a/tools/bodyteleop/static/index.html b/tools/bodyteleop/static/index.html deleted file mode 100644 index 365476975625c1..00000000000000 --- a/tools/bodyteleop/static/index.html +++ /dev/null @@ -1,103 +0,0 @@ - - - - - commabody - - - - - - - - - - -
-

comma body

- - -
-
-
-
-
- - - -
-

body

-
-
-
- - -
-

you

-
-
-
-
-
-
-
- - -
-

ping time

-
-
-
- - -
-

battery

-
-
-
- -
-
-
-
-
-
W
-
0,0x,y
-
-
-
A
-
S
-
D
-
-
-
- - - - -
-
-
-
-
-
-

Play Sounds

-
-
- - - -
-
-
-
-
-
-
-
-
-
- - - diff --git a/tools/bodyteleop/static/js/controls.js b/tools/bodyteleop/static/js/controls.js deleted file mode 100644 index b1e0e7ee70e340..00000000000000 --- a/tools/bodyteleop/static/js/controls.js +++ /dev/null @@ -1,54 +0,0 @@ -const keyVals = {w: 0, a: 0, s: 0, d: 0} - -export function getXY() { - let x = -keyVals.w + keyVals.s - let y = -keyVals.d + keyVals.a - return {x, y} -} - -export const handleKeyX = (key, setValue) => { - if (['w', 'a', 's', 'd'].includes(key)){ - keyVals[key] = setValue; - let color = "#333"; - if (setValue === 1){ - color = "#e74c3c"; - } - $("#key-"+key).css('background', color); - const {x, y} = getXY(); - $("#pos-vals").text(x+","+y); - } -}; - -export async function executePlan() { - let plan = $("#plan-text").val(); - const planList = []; - plan.split("\n").forEach(function(e){ - let line = e.split(",").map(k=>parseInt(k)); - if (line.length != 5 || line.slice(0, 4).map(e=>[1, 0].includes(e)).includes(false) || line[4] < 0 || line[4] > 10){ - console.log("invalid plan"); - } - else{ - planList.push(line) - } - }); - - async function execute() { - for (var i = 0; i < planList.length; i++) { - let [w, a, s, d, t] = planList[i]; - while(t > 0){ - console.log(w, a, s, d, t); - if(w==1){$("#key-w").mousedown();} - if(a==1){$("#key-a").mousedown();} - if(s==1){$("#key-s").mousedown();} - if(d==1){$("#key-d").mousedown();} - await sleep(50); - $("#key-w").mouseup(); - $("#key-a").mouseup(); - $("#key-s").mouseup(); - $("#key-d").mouseup(); - t = t - 0.05; - } - } - } - execute(); -} \ No newline at end of file diff --git a/tools/bodyteleop/static/js/jsmain.js b/tools/bodyteleop/static/js/jsmain.js deleted file mode 100644 index 83205a876bd4c2..00000000000000 --- a/tools/bodyteleop/static/js/jsmain.js +++ /dev/null @@ -1,27 +0,0 @@ -import { handleKeyX, executePlan } from "./controls.js"; -import { start, stop, lastChannelMessageTime, playSoundRequest } from "./webrtc.js"; - -export var pc = null; -export var dc = null; - -document.addEventListener('keydown', (e)=>(handleKeyX(e.key.toLowerCase(), 1))); -document.addEventListener('keyup', (e)=>(handleKeyX(e.key.toLowerCase(), 0))); -$(".keys").bind("mousedown touchstart", (e)=>handleKeyX($(e.target).attr('id').replace('key-', ''), 1)); -$(".keys").bind("mouseup touchend", (e)=>handleKeyX($(e.target).attr('id').replace('key-', ''), 0)); -$("#plan-button").click(executePlan); -$(".sound").click((e)=>{ - const sound = $(e.target).attr('id').replace('sound-', '') - return playSoundRequest(sound); -}); - -setInterval( () => { - const dt = new Date().getTime(); - if ((dt - lastChannelMessageTime) > 1000) { - $(".pre-blob").removeClass('blob'); - $("#battery").text("-"); - $("#ping-time").text('-'); - $("video")[0].load(); - } -}, 5000); - -start(pc, dc); diff --git a/tools/bodyteleop/static/js/plots.js b/tools/bodyteleop/static/js/plots.js deleted file mode 100644 index 5327bf71bea7fb..00000000000000 --- a/tools/bodyteleop/static/js/plots.js +++ /dev/null @@ -1,53 +0,0 @@ -export const pingPoints = []; -export const batteryPoints = []; - -function getChartConfig(pts, color, title, ymax=100) { - return { - type: 'line', - data: { - datasets: [{ - label: title, - data: pts, - borderWidth: 1, - borderColor: color, - backgroundColor: color, - fill: 'origin' - }] - }, - options: { - scales: { - x: { - type: 'time', - time: { - unit: 'minute', - displayFormats: { - second: 'h:mm a' - } - }, - grid: { - color: '#222', // Grid lines color - }, - ticks: { - source: 'data', - fontColor: 'rgba(255, 255, 255, 1.0)', // Y-axis label color - } - }, - y: { - beginAtZero: true, - max: ymax, - grid: { - color: 'rgba(255, 255, 255, 0.1)', // Grid lines color - }, - ticks: { - fontColor: 'rgba(255, 255, 255, 0.7)', // Y-axis label color - } - } - } - } - } -} - -const ctxPing = document.getElementById('chart-ping'); -const ctxBattery = document.getElementById('chart-battery'); -export const chartPing = new Chart(ctxPing, getChartConfig(pingPoints, 'rgba(192, 57, 43, 0.7)', 'Controls Ping Time (ms)', 250)); -export const chartBattery = new Chart(ctxBattery, getChartConfig(batteryPoints, 'rgba(41, 128, 185, 0.7)', 'Battery %', 100)); diff --git a/tools/bodyteleop/static/js/webrtc.js b/tools/bodyteleop/static/js/webrtc.js deleted file mode 100644 index 165a2ce6c4b9bc..00000000000000 --- a/tools/bodyteleop/static/js/webrtc.js +++ /dev/null @@ -1,209 +0,0 @@ -import { getXY } from "./controls.js"; -import { pingPoints, batteryPoints, chartPing, chartBattery } from "./plots.js"; - -export let controlCommandInterval = null; -export let latencyInterval = null; -export let lastChannelMessageTime = null; - - -export function offerRtcRequest(sdp, type) { - return fetch('/offer', { - body: JSON.stringify({sdp: sdp, type: type}), - headers: {'Content-Type': 'application/json'}, - method: 'POST' - }); -} - - -export function playSoundRequest(sound) { - return fetch('/sound', { - body: JSON.stringify({sound}), - headers: {'Content-Type': 'application/json'}, - method: 'POST' - }); -} - - -export function pingHeadRequest() { - return fetch('/', { - method: 'HEAD' - }); -} - - -export function createPeerConnection(pc) { - var config = { - sdpSemantics: 'unified-plan' - }; - - pc = new RTCPeerConnection(config); - - // connect audio / video - pc.addEventListener('track', function(evt) { - console.log("Adding Tracks!") - if (evt.track.kind == 'video') - document.getElementById('video').srcObject = evt.streams[0]; - else - document.getElementById('audio').srcObject = evt.streams[0]; - }); - return pc; -} - - -export function negotiate(pc) { - return pc.createOffer({offerToReceiveAudio:true, offerToReceiveVideo:true}).then(function(offer) { - return pc.setLocalDescription(offer); - }).then(function() { - return new Promise(function(resolve) { - if (pc.iceGatheringState === 'complete') { - resolve(); - } - else { - function checkState() { - if (pc.iceGatheringState === 'complete') { - pc.removeEventListener('icegatheringstatechange', checkState); - resolve(); - } - } - pc.addEventListener('icegatheringstatechange', checkState); - } - }); - }).then(function() { - var offer = pc.localDescription; - return offerRtcRequest(offer.sdp, offer.type); - }).then(function(response) { - console.log(response); - return response.json(); - }).then(function(answer) { - return pc.setRemoteDescription(answer); - }).catch(function(e) { - alert(e); - }); -} - - -function isMobile() { - let check = false; - (function(a){if(/(android|bb\d+|meego).+mobile|avantgo|bada\/|blackberry|blazer|compal|elaine|fennec|hiptop|iemobile|ip(hone|od)|iris|kindle|lge |maemo|midp|mmp|mobile.+firefox|netfront|opera m(ob|in)i|palm( os)?|phone|p(ixi|re)\/|plucker|pocket|psp|series(4|6)0|symbian|treo|up\.(browser|link)|vodafone|wap|windows ce|xda|xiino/i.test(a)||/1207|6310|6590|3gso|4thp|50[1-6]i|770s|802s|a wa|abac|ac(er|oo|s\-)|ai(ko|rn)|al(av|ca|co)|amoi|an(ex|ny|yw)|aptu|ar(ch|go)|as(te|us)|attw|au(di|\-m|r |s )|avan|be(ck|ll|nq)|bi(lb|rd)|bl(ac|az)|br(e|v)w|bumb|bw\-(n|u)|c55\/|capi|ccwa|cdm\-|cell|chtm|cldc|cmd\-|co(mp|nd)|craw|da(it|ll|ng)|dbte|dc\-s|devi|dica|dmob|do(c|p)o|ds(12|\-d)|el(49|ai)|em(l2|ul)|er(ic|k0)|esl8|ez([4-7]0|os|wa|ze)|fetc|fly(\-|_)|g1 u|g560|gene|gf\-5|g\-mo|go(\.w|od)|gr(ad|un)|haie|hcit|hd\-(m|p|t)|hei\-|hi(pt|ta)|hp( i|ip)|hs\-c|ht(c(\-| |_|a|g|p|s|t)|tp)|hu(aw|tc)|i\-(20|go|ma)|i230|iac( |\-|\/)|ibro|idea|ig01|ikom|im1k|inno|ipaq|iris|ja(t|v)a|jbro|jemu|jigs|kddi|keji|kgt( |\/)|klon|kpt |kwc\-|kyo(c|k)|le(no|xi)|lg( g|\/(k|l|u)|50|54|\-[a-w])|libw|lynx|m1\-w|m3ga|m50\/|ma(te|ui|xo)|mc(01|21|ca)|m\-cr|me(rc|ri)|mi(o8|oa|ts)|mmef|mo(01|02|bi|de|do|t(\-| |o|v)|zz)|mt(50|p1|v )|mwbp|mywa|n10[0-2]|n20[2-3]|n30(0|2)|n50(0|2|5)|n7(0(0|1)|10)|ne((c|m)\-|on|tf|wf|wg|wt)|nok(6|i)|nzph|o2im|op(ti|wv)|oran|owg1|p800|pan(a|d|t)|pdxg|pg(13|\-([1-8]|c))|phil|pire|pl(ay|uc)|pn\-2|po(ck|rt|se)|prox|psio|pt\-g|qa\-a|qc(07|12|21|32|60|\-[2-7]|i\-)|qtek|r380|r600|raks|rim9|ro(ve|zo)|s55\/|sa(ge|ma|mm|ms|ny|va)|sc(01|h\-|oo|p\-)|sdk\/|se(c(\-|0|1)|47|mc|nd|ri)|sgh\-|shar|sie(\-|m)|sk\-0|sl(45|id)|sm(al|ar|b3|it|t5)|so(ft|ny)|sp(01|h\-|v\-|v )|sy(01|mb)|t2(18|50)|t6(00|10|18)|ta(gt|lk)|tcl\-|tdg\-|tel(i|m)|tim\-|t\-mo|to(pl|sh)|ts(70|m\-|m3|m5)|tx\-9|up(\.b|g1|si)|utst|v400|v750|veri|vi(rg|te)|vk(40|5[0-3]|\-v)|vm40|voda|vulc|vx(52|53|60|61|70|80|81|83|85|98)|w3c(\-| )|webc|whit|wi(g |nc|nw)|wmlb|wonu|x700|yas\-|your|zeto|zte\-/i.test(a.substr(0,4))) check = true;})(navigator.userAgent||navigator.vendor||window.opera); - return check; -}; - - -export const constraints = { - audio: { - autoGainControl: false, - sampleRate: 48000, - sampleSize: 16, - echoCancellation: true, - noiseSuppression: true, - channelCount: 1 - }, - video: isMobile() -}; - - -export function start(pc, dc) { - pc = createPeerConnection(pc); - - // add audio track - navigator.mediaDevices.enumerateDevices() - .then(function(devices) { - const hasAudioInput = devices.find((device) => device.kind === "audioinput"); - var modifiedConstraints = {}; - modifiedConstraints.video = constraints.video; - modifiedConstraints.audio = hasAudioInput ? constraints.audio : false; - - return Promise.resolve(modifiedConstraints); - }) - .then(function(constraints) { - if (constraints.audio || constraints.video) { - return navigator.mediaDevices.getUserMedia(constraints); - } else{ - return Promise.resolve(null); - } - }) - .then(function(stream) { - if (stream) { - stream.getTracks().forEach(function(track) { - pc.addTrack(track, stream); - }); - } - - return negotiate(pc); - }) - .catch(function(err) { - alert('Could not acquire media: ' + err); - }); - - var parameters = {"ordered": true}; - dc = pc.createDataChannel('data', parameters); - dc.onclose = function() { - clearInterval(controlCommandInterval); - clearInterval(latencyInterval); - }; - - function sendJoystickOverDataChannel() { - const {x, y} = getXY(); - var message = JSON.stringify({type: "testJoystick", data: {axes: [x, y], buttons: [false]}}) - dc.send(message); - } - function checkLatency() { - const initialTime = new Date().getTime(); - pingHeadRequest().then(function() { - const currentTime = new Date().getTime(); - if (Math.abs(currentTime - lastChannelMessageTime) < 1000) { - const pingtime = currentTime - initialTime; - pingPoints.push({'x': currentTime, 'y': pingtime}); - if (pingPoints.length > 1000) { - pingPoints.shift(); - } - chartPing.update(); - $("#ping-time").text((pingtime) + "ms"); - } - }) - } - dc.onopen = function() { - controlCommandInterval = setInterval(sendJoystickOverDataChannel, 50); - latencyInterval = setInterval(checkLatency, 1000); - sendJoystickOverDataChannel(); - }; - - const textDecoder = new TextDecoder(); - var carStaterIndex = 0; - dc.onmessage = function(evt) { - const text = textDecoder.decode(evt.data); - const msg = JSON.parse(text); - if (carStaterIndex % 100 == 0 && msg.type === 'carState') { - const batteryLevel = Math.round(msg.data.fuelGauge * 100); - $("#battery").text(batteryLevel + "%"); - batteryPoints.push({'x': new Date().getTime(), 'y': batteryLevel}); - if (batteryPoints.length > 1000) { - batteryPoints.shift(); - } - chartBattery.update(); - } - carStaterIndex += 1; - lastChannelMessageTime = new Date().getTime(); - $(".pre-blob").addClass('blob'); - }; -} - - -export function stop(pc, dc) { - if (dc) { - dc.close(); - } - if (pc.getTransceivers) { - pc.getTransceivers().forEach(function(transceiver) { - if (transceiver.stop) { - transceiver.stop(); - } - }); - } - pc.getSenders().forEach(function(sender) { - sender.track.stop(); - }); - setTimeout(function() { - pc.close(); - }, 500); -} diff --git a/tools/bodyteleop/static/main.css b/tools/bodyteleop/static/main.css deleted file mode 100644 index 1bfb5982b4ec98..00000000000000 --- a/tools/bodyteleop/static/main.css +++ /dev/null @@ -1,185 +0,0 @@ -body { - background: #333 !important; - color: #fff !important; - display: flex; - justify-content: center; - align-items: start; -} - -p { - margin: 0px !important; -} - -i { - font-style: normal; -} - -.small { - font-size: 1em !important -} - -.jumbo { - font-size: 8rem; -} - - -@media (max-width: 600px) { - .small { - font-size: 0.5em !important - } - .jumbo { - display: none; - } - -} - -#main { - display: flex; - flex-direction: column; - align-content: center; - justify-content: center; - align-items: center; - font-size: 30px; - width: 100%; - max-width: 1200px; -} - -video { - width: 95%; -} - -.pre-blob { - display: flex; - background: #333; - border-radius: 50%; - margin: 10px; - height: 45px; - width: 45px; - justify-content: center; - align-items: center; - font-size: 1rem; -} - -.blob { - background: rgba(231, 76, 60,1.0); - box-shadow: 0 0 0 0 rgba(231, 76, 60,1.0); - animation: pulse 2s infinite; -} - -@keyframes pulse { - 0% { - box-shadow: 0 0 0 0px rgba(192, 57, 43, 1); - } - 100% { - box-shadow: 0 0 0 20px rgba(192, 57, 43, 0); - } -} - - -.icon-sup-panel { - display: flex; - flex-direction: row; - justify-content: space-around; - align-items: center; - background: #222; - border-radius: 10px; - padding: 5px; - margin: 5px 0px 5px 0px; -} - -.icon-sub-panel { - display: flex; - flex-direction: column; - justify-content: space-between; - align-items: center; -} - -#icon-panel { - display: flex; - width: 100%; - justify-content: space-between; - margin-top: 5px; -} - -.icon-sub-sub-panel { - display: flex; - flex-direction: row; -} - -.keys, #key-val { - background: #333; - padding: 2rem; - margin: 5px; - color: #fff; - display: flex; - justify-content: center; - align-items: center; - border-radius: 10px; - cursor: pointer; -} - -#key-val { - pointer-events: none; - background: #fff; - color: #333; - line-height: 1; - font-size: 20px; - flex-direction: column; -} - -.wasd-row { - display: flex; - flex-direction: row; - justify-content: center; - align-items: stretch; -} - -#wasd { - margin: 5px 0px 5px 0px; - background: #222; - border-radius: 10px; - width: 100%; - padding: 20px; - display: flex; - flex-direction: row; - justify-content: space-around; - align-items: stretch; - - user-select: none; - -webkit-touch-callout: none; - -webkit-user-select: none; - -khtml-user-select: none; - -moz-user-select: none; - -ms-user-select: none; - touch-action: manipulation; -} - -.panel { - display: flex; - justify-content: center; - margin: 5px 0px 5px 0px !important; - background: #222; - border-radius: 10px; - width: 100%; - padding: 10px; -} - -#ping-time, #battery { - font-size: 25px; -} - -#stop { - display: none; -} - -.plan-form { - display: flex; - flex-direction: column; - justify-content: space-between; - align-items: center; -} - -.details { - display: flex; - padding: 0px 10px 0px 10px; -} diff --git a/tools/bodyteleop/static/poster.png b/tools/bodyteleop/static/poster.png deleted file mode 100644 index 2f2b02dd8a2f0d..00000000000000 --- a/tools/bodyteleop/static/poster.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8740da2be7faac198b5e10780c646166056a76ebbe3d64499e0cdc49280c8a4f -size 8297 diff --git a/tools/bodyteleop/web.py b/tools/bodyteleop/web.py deleted file mode 100644 index f91d6a1441cf23..00000000000000 --- a/tools/bodyteleop/web.py +++ /dev/null @@ -1,86 +0,0 @@ -import dataclasses -import json -import logging -import os -import ssl -import subprocess - -from aiohttp import web -from aiohttp import ClientSession - -from openpilot.common.basedir import BASEDIR -from openpilot.system.webrtc.webrtcd import StreamRequestBody -from openpilot.common.params import Params - -logger = logging.getLogger("bodyteleop") -logging.basicConfig(level=logging.INFO) - -TELEOPDIR = f"{BASEDIR}/tools/bodyteleop" -WEBRTCD_HOST, WEBRTCD_PORT = "localhost", 5001 - - -## SSL -def create_ssl_cert(cert_path: str, key_path: str): - try: - proc = subprocess.run(f'openssl req -x509 -newkey rsa:4096 -nodes -out {cert_path} -keyout {key_path} \ - -days 365 -subj "/C=US/ST=California/O=commaai/OU=comma body"', - capture_output=True, shell=True) - proc.check_returncode() - except subprocess.CalledProcessError as ex: - raise ValueError(f"Error creating SSL certificate:\n[stdout]\n{proc.stdout.decode()}\n[stderr]\n{proc.stderr.decode()}") from ex - - -def create_ssl_context(): - cert_path = os.path.join(TELEOPDIR, "cert.pem") - key_path = os.path.join(TELEOPDIR, "key.pem") - if not os.path.exists(cert_path) or not os.path.exists(key_path): - logger.info("Creating certificate...") - create_ssl_cert(cert_path, key_path) - else: - logger.info("Certificate exists!") - ssl_context = ssl.SSLContext(protocol=ssl.PROTOCOL_TLS_SERVER) - ssl_context.load_cert_chain(cert_path, key_path) - - return ssl_context - -## ENDPOINTS -async def index(request: 'web.Request'): - with open(os.path.join(TELEOPDIR, "static", "index.html")) as f: - content = f.read() - return web.Response(content_type="text/html", text=content) - - -async def ping(request: 'web.Request'): - return web.Response(text="pong") - - -async def offer(request: 'web.Request'): - params = await request.json() - body = StreamRequestBody(params["sdp"], ["driver"], ["testJoystick"], ["carState"]) - body_json = json.dumps(dataclasses.asdict(body)) - - logger.info("Sending offer to webrtcd...") - webrtcd_url = f"http://{WEBRTCD_HOST}:{WEBRTCD_PORT}/stream" - async with ClientSession() as session, session.post(webrtcd_url, data=body_json) as resp: - assert resp.status == 200 - answer = await resp.json() - return web.json_response(answer) - - -def main(): - # Enable joystick debug mode - Params().put_bool("JoystickDebugMode", True) - - # App needs to be HTTPS for WebRTC to work on the browser - ssl_context = create_ssl_context() - - app = web.Application() - app.router.add_get("/", index) - app.router.add_get("/ping", ping, allow_head=True) - app.router.add_post("/offer", offer) - app.router.add_static('/static', os.path.join(TELEOPDIR, 'static')) - web.run_app(app, access_log=None, host="0.0.0.0", port=5000, ssl_context=ssl_context) - - -if __name__ == "__main__": - main()