diff --git a/src/components/BodyTeleop/ControlsBar.jsx b/src/components/BodyTeleop/ControlsBar.jsx new file mode 100644 index 00000000..79d2c267 --- /dev/null +++ b/src/components/BodyTeleop/ControlsBar.jsx @@ -0,0 +1,92 @@ +import React, { useCallback } from 'react'; +import PhotoCamera from '@material-ui/icons/PhotoCamera'; + +const QUICK_SOUNDS = [ + { key: 'engage', label: 'Engage', icon: '😊' }, + { key: 'disengage', label: 'Disengage', icon: '😢' }, + { key: 'prompt', label: 'Prompt', icon: '⚠️' }, + { key: 'warningImmediate', label: 'Warning', icon: '❗' }, +]; + +const CAMERAS = [ + { key: 'wideRoad', label: 'road', num: '1' }, + { key: 'driver', label: 'driver', num: '2' }, +]; + +const btnBase = `h-8 px-2.5 rounded-lg text-[10px] font-bold tracking-[0.2px] uppercase flex items-center justify-center min-w-[32px] cursor-pointer select-none hover:text-white hover:bg-white/20 bg-glass`; +const btnInactive = `${btnBase} bg-white/10 text-white/60`; +const btnActive = `${btnBase} bg-white/30 text-white`; + +const controlsGroupBase = 'absolute bottom-4 left-4 z-10 flex flex-row items-stretch gap-2.5 rounded-[14px] p-2 bg-glass-dark'; +const controlsGroupPortrait = 'relative bottom-auto left-auto transform-none self-stretch rounded-none shrink-0 justify-between gap-1.5'; + +const ControlsBar = ({ + connection, activeCamera, onSwitchCamera, + gamepadConnected, videoRef, isLandscape, +}) => { + const handlePlaySound = useCallback((sound) => { + connection?.playSound(sound).catch((err) => { + console.error('Failed to play body sound:', err); + }); + }, [connection]); + + const handleScreenshot = useCallback(() => { + const video = videoRef?.current; + if (!video || !video.videoWidth) return; + const canvas = document.createElement('canvas'); + canvas.width = video.videoWidth; + canvas.height = video.videoHeight; + canvas.getContext('2d').drawImage(video, 0, 0); + const link = document.createElement('a'); + link.download = `screenshot_${activeCamera}_${Date.now()}.png`; + link.href = canvas.toDataURL('image/png'); + link.click(); + }, [videoRef, activeCamera]); + + return ( +
+ {!gamepadConnected && ( +
+
+ {CAMERAS.map((cam) => ( +
onSwitchCamera(cam.key)} + > + {isLandscape ? cam.label : cam.num} +
+ ))} +
+ Camera +
+ )} +
+
+ {QUICK_SOUNDS.map((sound) => ( +
handlePlaySound(sound.key)} + > + {isLandscape ? sound.label : sound.icon} +
+ ))} +
+ Sounds +
+
+
+ +
+ Screenshot +
+
+ ); +}; + +export default ControlsBar; diff --git a/src/components/BodyTeleop/Joystick.jsx b/src/components/BodyTeleop/Joystick.jsx new file mode 100644 index 00000000..e6e402cf --- /dev/null +++ b/src/components/BodyTeleop/Joystick.jsx @@ -0,0 +1,330 @@ +import React, { useState, useEffect, useRef, useCallback } from 'react'; + + +const TriggerGroup = ({ bumperActive, bumperLabel, bumperKey, cameraActive, triggerValue, triggerColor, triggerKey, directionLabel }) => { + const activeStyle = cameraActive ? { background: 'rgba(59,130,246,0.35)', borderColor: 'rgba(59,130,246,0.5)' } : undefined; + + return ( +
+ + {bumperLabel} + +
+ + {bumperKey} + +
+
+
+ + {triggerKey} + +
+ + {directionLabel} + +
+ ); +} + +const ControllerOverlay = ({ gamepadSteering, gamepadGas, gamepadBrake, gamepadLB, gamepadRB, activeCamera }) => { + const thumbLeft = 50 + gamepadSteering * 34; + + return ( +
+ + +
+ L Stick — Steering +
+
+ {'\u25C0'} + {'\u25B6'} +
+
+
+ + +
+ ); +} + +const TouchJoystick = ({ className, thumbPos, joystickAreaRef, onTouchStart, onTouchMove, onTouchEnd, onMouseDown }) => { + const thumbRange = 45; + const thumbLeft = thumbPos ? `${50 + thumbPos.x * thumbRange}%` : '50%'; + const thumbTop = thumbPos ? `${50 + thumbPos.y * thumbRange}%` : '50%'; + + return ( +
e.preventDefault()} + > +
+
+
+
+
+ ); +} + +const Joystick = ({ + connection, activeCamera, className, + onGamepadChange, onSwitchCamera, gamepadConnected, +}) => { + const [thumbPos, setThumbPos] = useState(null); + const [, setKeys] = useState({ w: false, a: false, s: false, d: false }); + const [gamepadState, setGamepadState] = useState({ + steering: 0, gas: 0, brake: 0, lb: false, rb: false, + }); + + const joystickAreaRef = useRef(null); + const touchIdRef = useRef(null); + const mouseDraggingRef = useRef(false); + const prevBumpersRef = useRef({ lb: false, rb: false }); + const prevGamepadRef = useRef({ steering: 0, gas: 0, brake: 0, lb: false, rb: false }); + const prevThumbRef = useRef(null); + const gamepadFrameRef = useRef(null); + const triggerActivatedRef = useRef({ lt: false, rt: false }); + + const isRearCamera = activeCamera === 'wideRoad'; + + const setFlippedJoystick = useCallback((x, y) => { + const flip = isRearCamera ? -1 : 1; + connection?.setJoystick(flip * x, y); + }, [connection, isRearCamera]); + + // Touch joystick + const applyJoystick = useCallback((clientX, clientY) => { + const area = joystickAreaRef.current; + if (!area) return; + const rect = area.getBoundingClientRect(); + const halfW = rect.width / 2; + const halfH = rect.height / 2; + let dx = (clientX - rect.left - halfW) / halfW; + let dy = (clientY - rect.top - halfH) / halfH; + dx = Math.max(-1, Math.min(1, dx)); + dy = Math.max(-1, Math.min(1, dy)); + setThumbPos({ x: dx, y: dy }); + const cx = Math.sign(dx) * Math.max(Math.abs(dx), 0.20); + const cy = Math.sign(dy) * Math.max(Math.abs(dy), 0.20); + setFlippedJoystick(cy, -cx); + }, [setFlippedJoystick]); + + const resetJoystick = useCallback(() => { + setThumbPos(null); + connection?.setJoystick(0, 0); + }, [connection]); + + const handleTouchStart = useCallback((e) => { + e.preventDefault(); + if (touchIdRef.current !== null) return; + const t = e.changedTouches[0]; + touchIdRef.current = t.identifier; + applyJoystick(t.clientX, t.clientY); + }, [applyJoystick]); + + const handleTouchMove = useCallback((e) => { + e.preventDefault(); + for (let i = 0; i < e.changedTouches.length; i++) { + const t = e.changedTouches[i]; + if (t.identifier === touchIdRef.current) applyJoystick(t.clientX, t.clientY); + } + }, [applyJoystick]); + + const handleTouchEnd = useCallback((e) => { + e.preventDefault(); + for (let i = 0; i < e.changedTouches.length; i++) { + if (e.changedTouches[i].identifier === touchIdRef.current) { + touchIdRef.current = null; + resetJoystick(); + } + } + }, [resetJoystick]); + + const handleMouseMove = useCallback((e) => { + if (mouseDraggingRef.current) applyJoystick(e.clientX, e.clientY); + }, [applyJoystick]); + + const handleMouseUp = useCallback(() => { + mouseDraggingRef.current = false; + resetJoystick(); + document.removeEventListener('mousemove', handleMouseMove); + document.removeEventListener('mouseup', handleMouseUp); + }, [resetJoystick, handleMouseMove]); + + const handleMouseDown = useCallback((e) => { + e.preventDefault(); + mouseDraggingRef.current = true; + applyJoystick(e.clientX, e.clientY); + document.addEventListener('mousemove', handleMouseMove); + document.addEventListener('mouseup', handleMouseUp); + }, [applyJoystick, handleMouseMove, handleMouseUp]); + + // Keyboard input + useEffect(() => { + const arrowMap = { ArrowUp: 'w', ArrowDown: 's', ArrowLeft: 'a', ArrowRight: 'd' }; + + const handleKey = (e, pressed) => { + const k = arrowMap[e.key] || e.key.toLowerCase(); + if ('wasd'.includes(k) && k.length === 1) { + e.preventDefault(); + setKeys((prev) => { + const next = { ...prev, [k]: pressed }; + const x = -(next.d ? 1 : 0) + (next.a ? 1 : 0); + const y = -(next.w ? 1 : 0) + (next.s ? 1 : 0); + setFlippedJoystick(y, x); + const anyKey = next.w || next.a || next.s || next.d; + setThumbPos(anyKey ? { x: -x, y } : null); + return next; + }); + } + if (pressed) { + const cameraKeys = { 1: 'wideRoad', 2: 'driver' }; + if (cameraKeys[e.key]) { + e.preventDefault(); + onSwitchCamera(cameraKeys[e.key]); + } + } + }; + + const onKeyDown = (e) => handleKey(e, true); + const onKeyUp = (e) => handleKey(e, false); + + document.addEventListener('keydown', onKeyDown); + document.addEventListener('keyup', onKeyUp); + return () => { + document.removeEventListener('keydown', onKeyDown); + document.removeEventListener('keyup', onKeyUp); + }; + }, [setFlippedJoystick, onSwitchCamera]); + + // Gamepad polling + const gamepadConnectedRef = useRef(gamepadConnected); + useEffect(() => { gamepadConnectedRef.current = gamepadConnected; }, [gamepadConnected]); + + useEffect(() => { + const activated = triggerActivatedRef.current; + + const pollGamepad = () => { + gamepadFrameRef.current = requestAnimationFrame(pollGamepad); + const gamepads = navigator.getGamepads ? navigator.getGamepads() : []; + const gp = gamepads[0] || gamepads[1] || gamepads[2] || gamepads[3]; + + if (!gp) { + if (gamepadConnectedRef.current) onGamepadChange(false); + return; + } + + if (!gamepadConnectedRef.current) onGamepadChange(true); + + const DEADZONE = 0.15; + let lx = gp.axes[0] || 0; + if (Math.abs(lx) < DEADZONE) lx = 0; + + const rawRt = gp.axes[5] !== undefined ? gp.axes[5] : undefined; + const rawLt = gp.axes[4] !== undefined ? gp.axes[4] : undefined; + if (rawRt !== undefined && rawRt !== 0) activated.rt = true; + if (rawLt !== undefined && rawLt !== 0) activated.lt = true; + const rt = rawRt !== undefined ? ((activated.rt ? rawRt : -1) + 1) / 2 + : gp.buttons[7] ? gp.buttons[7].value : 0; + const lt = rawLt !== undefined ? ((activated.lt ? rawLt : -1) + 1) / 2 + : gp.buttons[6] ? gp.buttons[6].value : 0; + + const throttle = lt - rt; + const lb = gp.buttons[4] && gp.buttons[4].pressed; + const rb = gp.buttons[5] && gp.buttons[5].pressed; + + const pg = prevGamepadRef.current; + if (lx !== pg.steering || rt !== pg.gas || lt !== pg.brake || !!lb !== pg.lb || !!rb !== pg.rb) { + const next = { steering: lx, gas: rt, brake: lt, lb: !!lb, rb: !!rb }; + prevGamepadRef.current = next; + setGamepadState(next); + } + + setFlippedJoystick(throttle, -lx); + + if (lx !== 0 || rt > 0 || lt > 0) { + const pt = prevThumbRef.current; + if (!pt || pt.x !== lx || pt.y !== throttle) { + const next = { x: lx, y: throttle }; + prevThumbRef.current = next; + setThumbPos(next); + } + } else if (touchIdRef.current === null && !mouseDraggingRef.current) { + if (prevThumbRef.current !== null) { + prevThumbRef.current = null; + setThumbPos(null); + } + } + + const prev = prevBumpersRef.current; + if (lb && !prev.lb) onSwitchCamera('driver'); + if (rb && !prev.rb) onSwitchCamera('wideRoad'); + prevBumpersRef.current = { lb, rb }; + }; + + gamepadFrameRef.current = requestAnimationFrame(pollGamepad); + return () => { + if (gamepadFrameRef.current) cancelAnimationFrame(gamepadFrameRef.current); + }; + }, [setFlippedJoystick, onGamepadChange, onSwitchCamera]); + + // Cleanup mouse listeners on unmount + useEffect(() => { + return () => { + document.removeEventListener('mousemove', handleMouseMove); + document.removeEventListener('mouseup', handleMouseUp); + }; + }, [handleMouseMove, handleMouseUp]); + + if (gamepadConnected) { + return ( + + ); + } + + return ( + + ); +}; + +export default Joystick; diff --git a/src/components/BodyTeleop/StatusBar.jsx b/src/components/BodyTeleop/StatusBar.jsx new file mode 100644 index 00000000..8680f512 --- /dev/null +++ b/src/components/BodyTeleop/StatusBar.jsx @@ -0,0 +1,252 @@ +import React, { useState, useEffect, useRef, useCallback } from 'react'; +import BatteryFull from '@material-ui/icons/BatteryFull'; +import BatteryChargingFull from '@material-ui/icons/BatteryChargingFull'; + +const LATENCY_BUFFER_SIZE = 10; +const LATENCY_HISTORY_MAX = 60; + +const STATS_ROWS = [ + { label: 'Resolution', key: 'resolution' }, + { label: 'FPS', key: 'fps' }, + { label: 'Bitrate', key: 'bitrate' }, + { label: 'Jitter', key: 'jitter' }, +]; + +const LATENCY_LAYERS = [ + { label: 'Capture', key: 'captureMs', color: 'rgba(76,175,80,0.55)', labelColor: 'rgba(76,175,80,0.7)' }, + { label: 'Encode', key: 'encodeMs', color: 'rgba(255,183,77,0.55)', labelColor: 'rgba(255,183,77,0.7)' }, + { label: 'Send delay', key: 'sendDelayMs', color: 'rgba(171,71,188,0.45)', labelColor: 'rgba(171,71,188,0.65)' }, + { label: 'Network', key: 'networkMs', color: 'rgba(66,165,245,0.55)', labelColor: 'rgba(66,165,245,0.7)' }, +]; + + +export const useStats = (connection, connectionState, latencyCallbackRef) => { + const [showStats, setShowStats] = useState(false); + const [stats, setStats] = useState(null); + const [latency, setLatency] = useState(null); + const [latencyHistory, setLatencyHistory] = useState([]); + const latencyBufferRef = useRef([]); + const statsPollingRef = useRef({ interval: null, prevTimestamp: null, prevBytes: null, prevFrames: null }); + + useEffect(() => { + latencyCallbackRef.current = (raw) => { + latencyBufferRef.current.push(raw); + if (latencyBufferRef.current.length >= LATENCY_BUFFER_SIZE) { + const buf = latencyBufferRef.current; + latencyBufferRef.current = []; + const avg = {}; + for (const key of ['captureMs', 'encodeMs', 'sendDelayMs', 'devicePipelineMs', 'networkMs', 'totalMs']) { + const vals = buf.map((l) => l[key]).filter((v) => v != null); + avg[key] = vals.length ? vals.reduce((a, b) => a + b, 0) / vals.length : null; + } + setLatency(avg); + setLatencyHistory((prev) => [...prev, avg].slice(-LATENCY_HISTORY_MAX)); + } + }; + return () => { latencyCallbackRef.current = null; }; + }, [latencyCallbackRef]); + + const pollStats = useCallback(async () => { + const pc = connection?.pc; + if (!pc) return; + try { + const report = await pc.getStats(); + let videoStats = null; + report.forEach((stat) => { + if (stat.type === 'inbound-rtp' && stat.kind === 'video') videoStats = stat; + }); + if (!videoStats) return; + + const ref = statsPollingRef.current; + const now = videoStats.timestamp; + let bitrate = 0; + let fps = 0; + if (ref.prevTimestamp !== null) { + const elapsed = (now - ref.prevTimestamp) / 1000; + if (elapsed > 0) { + bitrate = ((videoStats.bytesReceived - ref.prevBytes) * 8) / elapsed; + fps = (videoStats.framesDecoded - ref.prevFrames) / elapsed; + } + } + ref.prevTimestamp = now; + ref.prevBytes = videoStats.bytesReceived; + ref.prevFrames = videoStats.framesDecoded; + + setStats({ + resolution: `${videoStats.frameWidth || '?'}x${videoStats.frameHeight || '?'}`, + fps: fps.toFixed(1), + bitrate: bitrate > 1000000 + ? `${(bitrate / 1000000).toFixed(2)} Mbps` + : `${(bitrate / 1000).toFixed(0)} kbps`, + jitter: videoStats.jitter !== undefined ? `${(videoStats.jitter * 1000).toFixed(1)} ms` : '?', + }); + } catch (err) { + console.warn('pollStats failed:', err); + } + }, [connection]); + + useEffect(() => { + const ref = statsPollingRef.current; + if (connectionState === 'connected') { + ref.prevTimestamp = null; + ref.prevBytes = null; + ref.prevFrames = null; + pollStats(); + ref.interval = setInterval(pollStats, 1000); + } else { + if (ref.interval) clearInterval(ref.interval); + ref.interval = null; + setStats(null); + setLatency(null); + setLatencyHistory([]); + } + return () => { + if (ref.interval) clearInterval(ref.interval); + ref.interval = null; + }; + }, [connectionState, pollStats]); + + const toggleStats = useCallback(() => { + setShowStats((prev) => { + const next = !prev; + connection?.setTimingSei(next); + return next; + }); + }, [connection]); + + return { showStats, toggleStats, stats, latency, latencyHistory }; +} + +function drawLatencyGraph(canvas, latencyHistory) { + const ctx = canvas.getContext('2d'); + const dpr = window.devicePixelRatio || 1; + const w = canvas.clientWidth; + const h = canvas.clientHeight; + const newW = w * dpr; + const newH = h * dpr; + if (canvas.width !== newW || canvas.height !== newH) { + canvas.width = newW; + canvas.height = newH; + } + ctx.setTransform(dpr, 0, 0, dpr, 0, 0); + ctx.clearRect(0, 0, w, h); + + const maxVal = Math.max(10, ...latencyHistory.map((l) => (l.totalMs != null ? l.totalMs : l.devicePipelineMs) || 0)); + const yScale = (h - 2) / (maxVal * 1.15); + const xStep = w / Math.max(latencyHistory.length - 1, 1); + + const cums = latencyHistory.map((l) => { + let sum = 0; + return LATENCY_LAYERS.map(({ key }) => { + const v = l[key]; + sum += (v != null && v > 0) ? v : 0; + return sum; + }); + }); + + for (let li = LATENCY_LAYERS.length - 1; li >= 0; li--) { + ctx.beginPath(); + for (let i = 0; i < cums.length; i++) { + const x = i * xStep; + const y = h - cums[i][li] * yScale; + if (i === 0) ctx.moveTo(x, y); else ctx.lineTo(x, y); + } + ctx.lineTo((cums.length - 1) * xStep, h); + ctx.lineTo(0, h); + ctx.closePath(); + ctx.fillStyle = LATENCY_LAYERS[li].color; + ctx.fill(); + } + + ctx.fillStyle = 'rgba(255,255,255,0.3)'; + ctx.font = '8px monospace'; + ctx.fillText(`${Math.round(maxVal)}ms`, 2, 9); +} + +export const StatsPanel = ({isLandscape, stats, latency, latencyHistory }) => { + const latencyCanvasRef = useRef(null); + + useEffect(() => { + if (!latencyHistory.length) return; + const canvas = latencyCanvasRef.current; + if (!canvas) return; + drawLatencyGraph(canvas, latencyHistory); + }, [latencyHistory]); + + if (!stats) return null; + + const fmtMs = (v) => (v != null ? `${v.toFixed(1)} ms` : '--'); + + return ( +
+
+ {STATS_ROWS.map(({ label, key }) => ( +
+ {label} + {stats[key]} +
+ ))} +
+ {latency && ( + <> +
FRAME LATENCY
+ {LATENCY_LAYERS.map(({ label, key, labelColor }) => ( +
+ {label} + {fmtMs(latency[key])} +
+ ))} +
+ Total + {fmtMs(latency.totalMs)} +
+ +
+ + )} +
+
+ ); +}; + +const StatusBar = ({ + connectionState, battery, className, toggleStats, +}) => { + const dotColor = connectionState === 'connecting' ? '#facc15' + : connectionState === 'connected' ? '#22c967' + : connectionState === 'failed' ? '#da2535' + : '#4b5559'; + + const BatteryIcon = battery?.charging ? BatteryChargingFull : BatteryFull; + + return ( +
+
+
+ {connectionState} +
+ {battery && ( +
+ + {battery.level}% +
+ )} +
+ stats +
+
+ ); +}; + +export default StatusBar; diff --git a/src/components/BodyTeleop/Video.jsx b/src/components/BodyTeleop/Video.jsx new file mode 100644 index 00000000..457ef446 --- /dev/null +++ b/src/components/BodyTeleop/Video.jsx @@ -0,0 +1,72 @@ +import React from 'react'; +import { Button } from '@material-ui/core'; +import Refresh from '@material-ui/icons/Refresh'; +import InfoOutline from '@material-ui/icons/InfoOutline'; + + +const ConnectOverlay = ({ connectionState, error, statusMessage, connectProgress, onConnect }) => { + const connecting = connectionState === 'connecting'; + const failed = connectionState === 'failed'; + + return ( +
+
+ {connecting ? ( + <> +
+
+
+ {statusMessage || 'Connecting...'} + + ) : failed ? ( + + ) : null} + {error && ( +
+ {error} +
+ )} + {!connecting && ( +
+ + Body must be powered on and started. +
+ )} +
+
+ ); +}; + +const Video = ({ + videoRef, connectionState, error, statusMessage, + connectProgress, onConnect, fit = 'contain', +}) => { + const connected = connectionState === 'connected'; + + return ( + <> +
+ { bodyTeleopOpen && } @@ -254,6 +274,7 @@ class ExplorerApp extends Component { const stateToProps = Obstruction({ zoom: 'zoom', pathname: 'router.location.pathname', + search: 'router.location.search', dongleId: 'dongleId', devices: 'devices', currentRoute: 'currentRoute', diff --git a/src/icons/index.jsx b/src/icons/index.jsx index 1dd0da70..b2f5643d 100644 --- a/src/icons/index.jsx +++ b/src/icons/index.jsx @@ -124,6 +124,12 @@ function chevronPath(rotation = 270) { ); } +export const GamepadIcon = (props) => ( + + + +); + export const ChevronIcon = (props) => ( {chevronPath(270)} diff --git a/src/index.css b/src/index.css index d12286c9..4bc35572 100644 --- a/src/index.css +++ b/src/index.css @@ -80,3 +80,20 @@ a:focus { .scrollstyle::-webkit-scrollbar-thumb:active { background-color: rgba(255, 255, 255, 0.2); } + +/* + Tailwind utility classes +*/ + +@layer utilities { + .bg-glass { + @apply bg-black/30 backdrop-blur-[10px] border-[1.5px] border-white/20 shadow-[inset_0_0_20px_rgba(255,255,255,0.1),0_4px_20px_rgba(0,0,0,0.4)]; + } + .bg-glass-dark { + @apply bg-black/40 backdrop-blur-[10px] shadow-[0_2px_12px_rgba(0,0,0,0.3)]; + } + .bg-radial-white { + @apply bg-[radial-gradient(circle_at_30%_30%,rgba(255,255,255,0.25),rgba(255,255,255,0.05))] + } +} + diff --git a/src/utils/bodyteleop.js b/src/utils/bodyteleop.js new file mode 100644 index 00000000..a6c5cd01 --- /dev/null +++ b/src/utils/bodyteleop.js @@ -0,0 +1,317 @@ +import { athena as Athena } from '@commaai/api'; +import { asyncSleep } from '.'; + +const VIDEO_STREAM_NAME = 'camera'; +const wallMs = () => performance.timeOrigin + performance.now(); + +const CLOCK_WINDOW_SIZE = 16; +const CLOCK_PING_MS = 500; + +export class BodyTeleopConnection { + constructor(callbacks) { + this.pc = null; + this.dc = null; + this.joystickInterval = null; + this.clockSyncInterval = null; + this.joystickX = 0; + this.joystickY = 0; + this.callbacks = callbacks; + this.clockSyncSamples = []; + this.clockOffsetMs = null; + this.clockSynced = false; + } + + async connectDirect(address) { + this.directAddress = address; + return this.connect(null); + } + + async connect(dongleId) { + this.cleanup(); + this.callbacks.onConnectionState('connecting'); + const t0 = performance.now(); + const log = (msg) => console.log(`[bodyteleop +${(performance.now() - t0).toFixed(0)}ms] ${msg}`); + + try { + this.pc = new RTCPeerConnection({ + iceServers: [{ urls: 'stun:stun.l.google.com:19302' }], + iceTransportPolicy: 'all', + bundlePolicy: 'max-bundle', + rtcpMuxPolicy: 'require', + encodedInsertableStreams: true, + }); + + this.pc.addEventListener('track', (evt) => { + if (evt.track.kind === 'video') { + if (evt.receiver) { + // Minimize receiver-side buffering for low-latency playback + if ('playoutDelayHint' in evt.receiver) { + evt.receiver.playoutDelayHint = 0; + } + if ('jitterBufferTarget' in evt.receiver) { + evt.receiver.jitterBufferTarget = 0; + } + + // Set up Transform to extract frame-level timing SEI in frames + if (typeof window.RTCRtpScriptTransform !== 'undefined') { + // Standard API (Firefox 117+, future Chrome) + try { + const worker = new Worker( + new URL('./latency-transform-worker.js', import.meta.url), + { type: 'module' }, + ); + worker.onmessage = (e) => { + if (e.data.type === 'timing') { + this._processTimingData(e.data.timing); + } + }; + evt.receiver.transform = new window.RTCRtpScriptTransform(worker); + } catch (e) { + log(e) + } + + + } + } + const stream = new MediaStream([evt.track]); + this.callbacks.onVideoTrack(VIDEO_STREAM_NAME, stream); + } + }); + + this.pc.addEventListener('connectionstatechange', () => { + if (!this.pc) return; + const state = this.pc.connectionState; + if (state === 'connected') { + this.callbacks.onStatusMessage?.('Receiving video...'); + this.callbacks.onConnectionState('connected'); + } + else if (state === 'failed' || state === 'closed') this.callbacks.onConnectionState('failed'); + }); + + const codecs = RTCRtpReceiver.getCapabilities('video')?.codecs || []; + const h264Codecs = codecs.filter((c) => c.mimeType === 'video/H264'); + const transceiver = this.pc.addTransceiver('video', { direction: 'recvonly' }); + if (h264Codecs.length > 0) { + transceiver.setCodecPreferences(h264Codecs); + } + this.dc = this.pc.createDataChannel('data', { ordered: true }); + this.dc.onopen = () => { + this.joystickInterval = setInterval(() => this.sendJoystick(), 50); + this.sendJoystick(); + }; + this.dc.onclose = () => { + this._clearJoystickInterval(); + this._stopClockSync(); + }; + this.dc.onmessage = (evt) => { + try { + const msg = JSON.parse(typeof evt.data === 'string' ? evt.data : new TextDecoder().decode(evt.data)); + if (msg.type === 'carState') this.callbacks.onBatteryLevel({ level: Math.round(msg.data.fuelGauge * 100), charging: !!msg.data.charging }); + if (msg.type === 'connectionReplaced') this.callbacks.onConnectionReplaced?.(msg.data); + if (msg.type === 'clockSync' && msg.data?.action === 'pong') this._handleClockPong(msg.data); + } catch (e) { + console.warn('bodyteleop: ignoring malformed data-channel message', e); + } + }; + + const offer = await this.pc.createOffer(); + await this.pc.setLocalDescription(offer); + this.callbacks.onStatusMessage?.('Preparing connection...'); + + // Trickle ICE: resolve as soon as we get the first candidate rather than + // waiting for all candidates to be gathered + await Promise.race([ + new Promise((resolve) => { + if (this.pc.iceGatheringState === 'complete') return resolve(); + let onCandidate, onComplete; + onCandidate = (evt) => { + if (evt.candidate) { + this.callbacks.onStatusMessage?.('Finding network path...'); + this.pc.removeEventListener('icecandidate', onCandidate); + this.pc.removeEventListener('icegatheringstatechange', onComplete); + resolve(); + } + }; + onComplete = () => { + if (this.pc.iceGatheringState === 'complete') { + this.pc.removeEventListener('icecandidate', onCandidate); + this.pc.removeEventListener('icegatheringstatechange', onComplete); + resolve(); + } + }; + this.pc.addEventListener('icecandidate', onCandidate); + this.pc.addEventListener('icegatheringstatechange', onComplete); + }), + new Promise((_, reject) => setTimeout(() => reject(new Error('ICE gathering timed out')), 5000)), + ]); + + // avoid rtcp-mux error on firefox + // needed until teleoprtc is resolved + await asyncSleep(250); + const sdp = this.pc.localDescription.sdp.replace( + /(m=(audio|video) .*\r?\n)([\s\S]*?)(?=m=|$)/g, + (block) => block.includes('a=rtcp-mux') ? block : block.replace(/(m=(?:audio|video) [^\n]*\n)/, '$1a=rtcp-mux\r\n'), + ); + this.callbacks.onStatusMessage?.('Reaching device...'); + + let answerSdp; + if (dongleId) { + const payload = { + method: 'startStream', + params: { sdp }, + jsonrpc: '2.0', + id: 0, + }; + const resp = await Athena.postJsonRpcPayload(dongleId, payload); + if (!resp?.result || resp.error) { + throw new Error('Could not reach device. Is the ignition on?'); + } + this.callbacks.onStatusMessage?.('Device responded'); + answerSdp = resp.result.sdp; + } else if (this.directAddress) { + let resp; + try { + resp = await fetch(`http://${this.directAddress}:5001/stream`, { + method: 'POST', + headers: { 'Content-Type': 'application/json' }, + body: JSON.stringify({ sdp, initCamera: "wideRoad", bridge_services_in: ["testJoystick", "soundRequest"], bridge_services_out: ['carState'] }), + }); + } catch (_) { + throw new Error('Could not reach device. Is the ignition on?'); + } + if (!resp.ok) { + throw new Error(`Device experienced an error (${resp.status})`); + } + this.callbacks.onStatusMessage?.('Device responded'); + const result = await resp.json(); + answerSdp = result.sdp; + } + + await this.pc.setRemoteDescription({ type: 'answer', sdp: answerSdp }); + this.callbacks.onStatusMessage?.('Establishing connection...'); + } catch (err) { + this.cleanup(); + this.callbacks.onConnectionState('failed'); + throw err; + } + } + + _sendDc(type, data) { + if (this.dc?.readyState === 'open') { + this.dc.send(JSON.stringify({ type, data })); + return true; + } + return false; + } + + async playSound(sound) { + if (!this._sendDc('soundRequest', { sound })) { + throw new Error('Body sound buttons require an active teleop connection.'); + } + } + + switchCamera(cameraName) { + this._sendDc('livestreamCameraSwitch', { camera: cameraName }); + } + + setJoystick(x, y) { + this.joystickX = x; + this.joystickY = y; + } + + sendJoystick() { + this._sendDc('testJoystick', { axes: [this.joystickX, this.joystickY], buttons: [false] }); + } + + _clearJoystickInterval() { + if (this.joystickInterval) { + clearInterval(this.joystickInterval); + this.joystickInterval = null; + } + } + + _stopClockSync() { + if (this.clockSyncInterval) { + clearInterval(this.clockSyncInterval); + this.clockSyncInterval = null; + } + this.clockSyncSamples = []; + this.clockOffsetMs = null; + this.clockSynced = false; + } + + setTimingSei(enabled) { + this._sendDc('enableTimingSei', { enabled }); + if (enabled) { + if (!this.clockSyncInterval) { + this._sendClockPing(); + this.clockSyncInterval = setInterval(() => this._sendClockPing(), CLOCK_PING_MS); + } + } else { + this._stopClockSync(); + } + } + + _processTimingData(timing) { + const browserReceiveMs = wallMs(); + const latency = { + captureMs: timing.captureMs, + encodeMs: timing.encodeMs, + sendDelayMs: timing.sendDelayMs, + devicePipelineMs: timing.captureMs + timing.encodeMs + timing.sendDelayMs, + networkMs: null, + totalMs: null, + }; + + if (this.clockSynced) { + const raw = browserReceiveMs - (timing.deviceSendWallMs - this.clockOffsetMs); + latency.networkMs = Math.max(0, raw); + latency.totalMs = latency.devicePipelineMs + latency.networkMs; + } + + this.callbacks.onLatencyUpdate?.(latency); + } + + _sendClockPing() { + this._sendDc('clockSync', { action: 'ping', browserSendTime: wallMs() }); + } + + _handleClockPong(data) { + const now = wallMs(); + const rttMs = now - data.browserSendTime; + const offsetMs = data.deviceTime - (data.browserSendTime + now) / 2; + + this.clockSyncSamples.push({ offsetMs, rttMs }); + if (this.clockSyncSamples.length > CLOCK_WINDOW_SIZE) this.clockSyncSamples.shift(); + + // pick the smallest-RTT sample: congestion can only inflate RTT, never deflate it, + // so min-RTT is the least biased estimate of one-way offset + let best = this.clockSyncSamples[0]; + for (const s of this.clockSyncSamples) if (s.rttMs < best.rttMs) best = s; + this.clockOffsetMs = best.offsetMs; + this.clockSynced = true; + } + + disconnect() { + this.cleanup(); + this.callbacks.onConnectionState('disconnected'); + } + + cleanup() { + this._clearJoystickInterval(); + this._stopClockSync(); + if (this.dc) { + this.dc.close(); + this.dc = null; + } + if (this.pc) { + if (this.pc.getTransceivers) { + this.pc.getTransceivers().forEach((t) => { + if (t.stop) t.stop(); + }); + } + this.pc.close(); + this.pc = null; + } + } +} diff --git a/src/utils/browser.js b/src/utils/browser.js index cd4030e5..7626be01 100644 --- a/src/utils/browser.js +++ b/src/utils/browser.js @@ -5,3 +5,11 @@ export function isIos() { export function isFirefox() { return navigator.userAgent.toLowerCase().includes('firefox'); } + +export function isChrome() { + return /chrome/i.test(navigator.userAgent) && !/edg/i.test(navigator.userAgent); +} + +export function isSafari() { + return /safari/i.test(navigator.userAgent) && !/chrome|chromium|edg/i.test(navigator.userAgent); +} \ No newline at end of file diff --git a/src/utils/latency-transform-worker.js b/src/utils/latency-transform-worker.js new file mode 100644 index 00000000..cac1d866 --- /dev/null +++ b/src/utils/latency-transform-worker.js @@ -0,0 +1,46 @@ +// Worker script for RTCRtpScriptTransform (Firefox / standard path). +// Receives encoded video frames, posts timing SEI data back to main thread, +// and forwards frames unchanged. + +// Must match _SEI_PREFIX in openpilot system/webrtc/device/video.py +const SEI_PREFIX = new Uint8Array([ + 0x00, 0x00, 0x00, 0x01, 0x06, 0x05, 0x30, + 0xa5, 0xe0, 0xc4, 0xa4, 0x5b, 0x6e, 0x4e, 0x1e, + 0x9c, 0x7e, 0x12, 0x34, 0x56, 0x78, 0x9a, 0xbc, +]); + +export function extractTimingSei(frameBuffer) { + const data = new Uint8Array(frameBuffer); + + outer: + for (let i = 0; i <= data.length - SEI_PREFIX.length - 32; i++) { + for (let k = 0; k < SEI_PREFIX.length; k++) { + if (data[i + k] !== SEI_PREFIX[k]) continue outer; + } + const tsOffset = i + SEI_PREFIX.length; + const view = new DataView(data.buffer, data.byteOffset + tsOffset, 32); + return { + captureMs: view.getFloat64(0), + encodeMs: view.getFloat64(8), + sendDelayMs: view.getFloat64(16), + deviceSendWallMs: view.getFloat64(24), + }; + } + return null; +} + +// Handle RTCRtpScriptTransform event +// eslint-disable-next-line no-restricted-globals +self.onrtctransform = (event) => { + const { readable, writable } = event.transformer; + readable.pipeThrough(new TransformStream({ + transform(frame, controller) { + const timing = extractTimingSei(frame.data); + if (timing) { + // eslint-disable-next-line no-restricted-globals + self.postMessage({ type: 'timing', timing }); + } + controller.enqueue(frame); + }, + })).pipeTo(writable); +};