diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 1a812130f9..519adaacef 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -23,7 +23,7 @@ repos: exclude: '^(third_party/)|(body/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)|(selfdrive/ui/translations/.*.ts)|(poetry.lock)' args: # if you've got a short variable name that's getting flagged, add it here - - -L bu,ro,te,ue,alo,hda,ois,nam,nams,ned,som,parm,setts,inout,warmup + - -L bu,ro,te,ue,alo,hda,ois,nam,nams,ned,som,parm,setts,inout,warmup,bumb,nd,sie - --builtins clear,rare,informal,usage,code,names,en-GB_to_en-US - repo: local hooks: diff --git a/release/files_common b/release/files_common index b66a076b2b..90ee2ccd6f 100644 --- a/release/files_common +++ b/release/files_common @@ -60,6 +60,7 @@ release/* tools/__init__.py tools/lib/* +tools/bodyteleop/* tools/joystick/* tools/replay/*.cc tools/replay/*.h diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index f151a51d10..2fd786875c 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -72,7 +72,7 @@ procs = [ # debug procs NativeProcess("bridge", "cereal/messaging", ["./bridge"], onroad=False, callback=notcar), - PythonProcess("webjoystick", "tools.joystick.web", onroad=False, callback=notcar), + PythonProcess("webjoystick", "tools.bodyteleop.web", onroad=False, callback=notcar), ] managed_processes = {p.name: p for p in procs} diff --git a/tools/bodyteleop/.gitignore b/tools/bodyteleop/.gitignore new file mode 100644 index 0000000000..adeab99a95 --- /dev/null +++ b/tools/bodyteleop/.gitignore @@ -0,0 +1,4 @@ +av +av-10.0.0/* +key.pem +cert.pem \ No newline at end of file diff --git a/tools/bodyteleop/bodyav.py b/tools/bodyteleop/bodyav.py new file mode 100644 index 0000000000..cb2ebb061a --- /dev/null +++ b/tools/bodyteleop/bodyav.py @@ -0,0 +1,158 @@ +import asyncio +import io +import numpy as np +import pyaudio +import wave + +from aiortc.contrib.media import MediaBlackhole +from aiortc.mediastreams import AudioStreamTrack, MediaStreamError, MediaStreamTrack +from aiortc.mediastreams import VIDEO_CLOCK_RATE, VIDEO_TIME_BASE +from aiortc.rtcrtpsender import RTCRtpSender +from av import CodecContext, Packet +from pydub import AudioSegment +import cereal.messaging as messaging + +AUDIO_RATE = 16000 +SOUNDS = { + 'engage': '../../selfdrive/assets/sounds/engage.wav', + 'disengage': '../../selfdrive/assets/sounds/disengage.wav', + 'error': '../../selfdrive/assets/sounds/warning_immediate.wav', +} + + +def force_codec(pc, sender, forced_codec='video/VP9', stream_type="video"): + codecs = RTCRtpSender.getCapabilities(stream_type).codecs + codec = [codec for codec in codecs if codec.mimeType == forced_codec] + transceiver = next(t for t in pc.getTransceivers() if t.sender == sender) + transceiver.setCodecPreferences(codec) + + +class EncodedBodyVideo(MediaStreamTrack): + kind = "video" + + _start: float + _timestamp: int + + def __init__(self): + super().__init__() + sock_name = 'livestreamDriverEncodeData' + messaging.context = messaging.Context() + self.sock = messaging.sub_sock(sock_name, None, conflate=True) + self.pts = 0 + + async def recv(self) -> Packet: + while True: + msg = messaging.recv_one_or_none(self.sock) + if msg is not None: + break + await asyncio.sleep(0.005) + + evta = getattr(msg, msg.which()) + self.last_idx = evta.idx.encodeId + + packet = Packet(evta.header + evta.data) + packet.time_base = VIDEO_TIME_BASE + packet.pts = self.pts + self.pts += 0.05 * VIDEO_CLOCK_RATE + return packet + + +class WebClientSpeaker(MediaBlackhole): + def __init__(self): + super().__init__() + self.p = pyaudio.PyAudio() + self.buffer = io.BytesIO() + self.channels = 2 + self.stream = self.p.open(format=pyaudio.paInt16, channels=self.channels, rate=48000, frames_per_buffer=9600, output=True, stream_callback=self.pyaudio_callback) + + def pyaudio_callback(self, in_data, frame_count, time_info, status): + if self.buffer.getbuffer().nbytes < frame_count * self.channels * 2: + buff = np.zeros((frame_count, 2), dtype=np.int16).tobytes() + elif self.buffer.getbuffer().nbytes > 115200: # 3x the usual read size + self.buffer.seek(0) + buff = self.buffer.read(frame_count * self.channels * 4) + buff = buff[:frame_count * self.channels * 2] + self.buffer.seek(2) + else: + self.buffer.seek(0) + buff = self.buffer.read(frame_count * self.channels * 2) + self.buffer.seek(2) + return (buff, pyaudio.paContinue) + + async def consume(self, track): + while True: + try: + frame = await track.recv() + except MediaStreamError: + return + bio = bytes(frame.planes[0]) + self.buffer.write(bio) + + async def start(self): + for track, task in self._MediaBlackhole__tracks.items(): # pylint: disable=access-member-before-definition + if task is None: + self._MediaBlackhole__tracks[track] = asyncio.ensure_future(self.consume(track)) + + async def stop(self): + for task in self._MediaBlackhole__tracks.values(): # pylint: disable=access-member-before-definition + if task is not None: + task.cancel() + self._MediaBlackhole__tracks = {} + self.stream.stop_stream() + self.stream.close() + self.p.terminate() + + +class BodyMic(AudioStreamTrack): + def __init__(self): + super().__init__() + + self.sample_rate = AUDIO_RATE + self.AUDIO_PTIME = 0.020 # 20ms audio packetization + self.samples = int(self.AUDIO_PTIME * self.sample_rate) + self.FORMAT = pyaudio.paInt16 + self.CHANNELS = 2 + self.RATE = self.sample_rate + self.CHUNK = int(AUDIO_RATE * 0.020) + self.p = pyaudio.PyAudio() + self.mic_stream = self.p.open(format=self.FORMAT, channels=1, rate=self.RATE, input=True, frames_per_buffer=self.CHUNK) + + self.codec = CodecContext.create('pcm_s16le', 'r') + self.codec.sample_rate = self.RATE + self.codec.channels = 2 + self.audio_samples = 0 + self.chunk_number = 0 + + async def recv(self): + mic_data = self.mic_stream.read(self.CHUNK) + mic_sound = AudioSegment(mic_data, sample_width=2, channels=1, frame_rate=self.RATE) + mic_sound = AudioSegment.from_mono_audiosegments(mic_sound, mic_sound) + mic_sound += 3 # increase volume by 3db + packet = Packet(mic_sound.raw_data) + frame = self.codec.decode(packet)[0] + frame.pts = self.audio_samples + self.audio_samples += frame.samples + self.chunk_number = self.chunk_number + 1 + return frame + + +async def play_sound(sound): + chunk = 5120 + with wave.open(SOUNDS[sound], 'rb') as wf: + def callback(in_data, frame_count, time_info, status): + data = wf.readframes(frame_count) + return data, pyaudio.paContinue + + p = pyaudio.PyAudio() + stream = p.open(format=p.get_format_from_width(wf.getsampwidth()), + channels=wf.getnchannels(), + rate=wf.getframerate(), + output=True, + frames_per_buffer=chunk, + stream_callback=callback) + stream.start_stream() + while stream.is_active(): + await asyncio.sleep(0) + stream.stop_stream() + stream.close() + p.terminate() diff --git a/tools/bodyteleop/static/index.html b/tools/bodyteleop/static/index.html new file mode 100644 index 0000000000..3654769756 --- /dev/null +++ b/tools/bodyteleop/static/index.html @@ -0,0 +1,103 @@ + + + + + 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 new file mode 100644 index 0000000000..b1e0e7ee70 --- /dev/null +++ b/tools/bodyteleop/static/js/controls.js @@ -0,0 +1,54 @@ +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 new file mode 100644 index 0000000000..f521905724 --- /dev/null +++ b/tools/bodyteleop/static/js/jsmain.js @@ -0,0 +1,23 @@ +import { handleKeyX, executePlan } from "./controls.js"; +import { start, stop, last_ping } 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); + +setInterval( () => { + const dt = new Date().getTime(); + if ((dt - last_ping) > 1000) { + $(".pre-blob").removeClass('blob'); + $("#battery").text("-"); + $("#ping-time").text('-'); + $("video")[0].load(); + } +}, 5000); + +start(pc, dc); \ No newline at end of file diff --git a/tools/bodyteleop/static/js/plots.js b/tools/bodyteleop/static/js/plots.js new file mode 100644 index 0000000000..5327bf71be --- /dev/null +++ b/tools/bodyteleop/static/js/plots.js @@ -0,0 +1,53 @@ +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 new file mode 100644 index 0000000000..8bc8e77317 --- /dev/null +++ b/tools/bodyteleop/static/js/webrtc.js @@ -0,0 +1,217 @@ +import { getXY } from "./controls.js"; +import { pingPoints, batteryPoints, chartPing, chartBattery } from "./plots.js"; + +export let dcInterval = null; +export let batteryInterval = null; +export let last_ping = null; + + +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 fetch('/offer', { + body: JSON.stringify({ + sdp: offer.sdp, + type: offer.type, + }), + headers: { + 'Content-Type': 'application/json' + }, + method: 'POST' + }); + }).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 createDummyVideoTrack() { + const canvas = document.createElement('canvas'); + const context = canvas.getContext('2d'); + + const frameWidth = 5; // Set the width of the frame + const frameHeight = 5; // Set the height of the frame + canvas.width = frameWidth; + canvas.height = frameHeight; + + context.fillStyle = 'black'; + context.fillRect(0, 0, frameWidth, frameHeight); + + const stream = canvas.captureStream(); + const videoTrack = stream.getVideoTracks()[0]; + + return videoTrack; +} + + +export function start(pc, dc) { + pc = createPeerConnection(pc); + + if (constraints.audio || constraints.video) { + // add audio track + navigator.mediaDevices.getUserMedia(constraints).then(function(stream) { + stream.getTracks().forEach(function(track) { + pc.addTrack(track, stream); + // only audio? + // if (track.kind === 'audio'){ + // pc.addTrack(track, stream); + // } + }); + return negotiate(pc); + }, function(err) { + alert('Could not acquire media: ' + err); + }); + + // add a fake video? + // const dummyVideoTrack = createDummyVideoTrack(); + // const dummyMediaStream = new MediaStream(); + // dummyMediaStream.addTrack(dummyVideoTrack); + // pc.addTrack(dummyVideoTrack, dummyMediaStream); + + } else { + negotiate(pc); + } + + // setInterval(() => {pc.getStats(null).then((stats) => {stats.forEach((report) => console.log(report))})}, 10000) + // var video = document.querySelector('video'); + // var print = function (e, f){console.log(e, f); video.requestVideoFrameCallback(print);}; + // video.requestVideoFrameCallback(print); + + + var parameters = {"ordered": true}; + dc = pc.createDataChannel('data', parameters); + dc.onclose = function() { + console.log("data channel closed"); + clearInterval(dcInterval); + clearInterval(batteryInterval); + }; + function controlCommand() { + const {x, y} = getXY(); + const dt = new Date().getTime(); + var message = JSON.stringify({type: 'control_command', x, y, dt}); + dc.send(message); + } + + function batteryLevel() { + var message = JSON.stringify({type: 'battery_level'}); + dc.send(message); + } + + dc.onopen = function() { + dcInterval = setInterval(controlCommand, 50); + batteryInterval = setInterval(batteryLevel, 10000); + controlCommand(); + batteryLevel(); + $(".sound").click((e)=>{ + const sound = $(e.target).attr('id').replace('sound-', '') + dc.send(JSON.stringify({type: 'play_sound', sound})); + }); + }; + + let val_print_idx = 0; + dc.onmessage = function(evt) { + const data = JSON.parse(evt.data); + if(val_print_idx == 0 && data.type === 'ping_time') { + const dt = new Date().getTime(); + const pingtime = dt - data.incoming_time; + pingPoints.push({'x': dt, 'y': pingtime}); + if (pingPoints.length > 1000) { + pingPoints.shift(); + } + chartPing.update(); + $("#ping-time").text((pingtime) + "ms"); + last_ping = dt; + $(".pre-blob").addClass('blob'); + } + val_print_idx = (val_print_idx + 1 ) % 20; + if(data.type === 'battery_level') { + $("#battery").text(data.value + "%"); + batteryPoints.push({'x': new Date().getTime(), 'y': data.value}); + if (batteryPoints.length > 1000) { + batteryPoints.shift(); + } + chartBattery.update(); + } + }; +} + + +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 new file mode 100644 index 0000000000..1bfb5982b4 --- /dev/null +++ b/tools/bodyteleop/static/main.css @@ -0,0 +1,185 @@ +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 new file mode 100644 index 0000000000..2f2b02dd8a --- /dev/null +++ b/tools/bodyteleop/static/poster.png @@ -0,0 +1,3 @@ +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 new file mode 100644 index 0000000000..c8f5dfd76e --- /dev/null +++ b/tools/bodyteleop/web.py @@ -0,0 +1,201 @@ +import asyncio +import json +import logging +import os +import ssl +import uuid +import time + +from common.basedir import BASEDIR +from aiohttp import web +from aiortc import RTCPeerConnection, RTCSessionDescription + +import cereal.messaging as messaging +from tools.bodyteleop.bodyav import BodyMic, WebClientSpeaker, force_codec, play_sound, MediaBlackhole, EncodedBodyVideo + +logger = logging.getLogger("pc") +logging.basicConfig(level=logging.INFO) + +pcs = set() +pm, sm = None, None +TELEOPDIR = f"{BASEDIR}/tools/bodyteleop" + + +async def index(request): + content = open(TELEOPDIR + "/static/index.html", "r").read() + now = time.monotonic() + request.app['mutable_vals']['last_send_time'] = now + request.app['mutable_vals']['last_override_time'] = now + request.app['mutable_vals']['prev_command'] = [] + request.app['mutable_vals']['find_person'] = False + + return web.Response(content_type="text/html", text=content) + + +async def control_body(data, app): + now = time.monotonic() + if (data['type'] == 'dummy_controls') and (now < (app['mutable_vals']['last_send_time'] + 0.2)): + return + if (data['type'] == 'control_command') and (app['mutable_vals']['prev_command'] == [data['x'], data['y']] and data['x'] == 0 and data['y'] == 0): + return + + logger.info(str(data)) + x = max(-1.0, min(1.0, data['x'])) + y = max(-1.0, min(1.0, data['y'])) + dat = messaging.new_message('testJoystick') + dat.testJoystick.axes = [x, y] + dat.testJoystick.buttons = [False] + pm.send('testJoystick', dat) + app['mutable_vals']['last_send_time'] = now + if (data['type'] == 'control_command'): + app['mutable_vals']['last_override_time'] = now + app['mutable_vals']['prev_command'] = [data['x'], data['y']] + + +async def dummy_controls_msg(app): + while True: + if 'last_send_time' in app['mutable_vals']: + this_time = time.monotonic() + if (app['mutable_vals']['last_send_time'] + 0.2) < this_time: + await control_body({'type': 'dummy_controls', 'x': 0, 'y': 0}, app) + await asyncio.sleep(0.2) + + +async def start_background_tasks(app): + app['bgtask_dummy_controls_msg'] = asyncio.create_task(dummy_controls_msg(app)) + + +async def stop_background_tasks(app): + app['bgtask_dummy_controls_msg'].cancel() + await app['bgtask_dummy_controls_msg'] + + +async def offer(request): + logger.info("\n\n\nnewoffer!\n\n") + + params = await request.json() + offer = RTCSessionDescription(sdp=params["sdp"], type=params["type"]) + speaker = WebClientSpeaker() + blackhole = MediaBlackhole() + + pc = RTCPeerConnection() + pc_id = "PeerConnection(%s)" % uuid.uuid4() + pcs.add(pc) + + def log_info(msg, *args): + logger.info(pc_id + " " + msg, *args) + + log_info("Created for %s", request.remote) + + @pc.on("datachannel") + def on_datachannel(channel): + request.app['mutable_vals']['remote_channel'] = channel + + @channel.on("message") + async def on_message(message): + data = json.loads(message) + if data['type'] == 'control_command': + await control_body(data, request.app) + times = { + 'type': 'ping_time', + 'incoming_time': data['dt'], + 'outgoing_time': int(time.time() * 1000), + } + channel.send(json.dumps(times)) + if data['type'] == 'battery_level': + sm.update(timeout=0) + if sm.updated['carState']: + channel.send(json.dumps({'type': 'battery_level', 'value': int(sm['carState'].fuelGauge * 100)})) + if data['type'] == 'play_sound': + logger.info(f"Playing sound: {data['sound']}") + await play_sound(data['sound']) + if data['type'] == 'find_person': + request.app['mutable_vals']['find_person'] = data['value'] + + @pc.on("connectionstatechange") + async def on_connectionstatechange(): + log_info("Connection state is %s", pc.connectionState) + if pc.connectionState == "failed": + await pc.close() + pcs.discard(pc) + + @pc.on('track') + def on_track(track): + logger.info(f"Track received: {track.kind}") + if track.kind == "audio": + speaker.addTrack(track) + elif track.kind == "video": + blackhole.addTrack(track) + + @track.on("ended") + async def on_ended(): + log_info("Remote %s track ended", track.kind) + if track.kind == "audio": + await speaker.stop() + elif track.kind == "video": + await blackhole.stop() + + video_sender = pc.addTrack(EncodedBodyVideo()) + force_codec(pc, video_sender, forced_codec='video/H264') + _ = pc.addTrack(BodyMic()) + + await pc.setRemoteDescription(offer) + await speaker.start() + await blackhole.start() + answer = await pc.createAnswer() + await pc.setLocalDescription(answer) + + return web.Response( + content_type="application/json", + text=json.dumps( + {"sdp": pc.localDescription.sdp, "type": pc.localDescription.type} + ), + ) + + +async def on_shutdown(app): + coros = [pc.close() for pc in pcs] + await asyncio.gather(*coros) + pcs.clear() + + +async def run(cmd): + proc = await asyncio.create_subprocess_shell( + cmd, + stdout=asyncio.subprocess.PIPE, + stderr=asyncio.subprocess.PIPE + ) + stdout, stderr = await proc.communicate() + logger.info("Created key and cert!") + if stdout: + logger.info(f'[stdout]\n{stdout.decode()}') + if stderr: + logger.info(f'[stderr]\n{stderr.decode()}') + + +def main(): + global pm, sm + pm = messaging.PubMaster(['testJoystick']) + sm = messaging.SubMaster(['carState', 'logMessage']) + # App needs to be HTTPS for microphone and audio autoplay to work on the browser + cert_path = TELEOPDIR + '/cert.pem' + key_path = TELEOPDIR + '/key.pem' + if (not os.path.exists(cert_path)) or (not os.path.exists(key_path)): + asyncio.run(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"')) + else: + logger.info("Certificate exists!") + ssl_context = ssl.SSLContext() + ssl_context.load_cert_chain(cert_path, key_path) + app = web.Application() + app['mutable_vals'] = {} + app.on_shutdown.append(on_shutdown) + app.router.add_post("/offer", offer) + app.router.add_get("/", index) + app.router.add_static('/static', TELEOPDIR + '/static') + app.on_startup.append(start_background_tasks) + app.on_cleanup.append(stop_background_tasks) + web.run_app(app, access_log=None, host="0.0.0.0", port=5000, ssl_context=ssl_context) + + +if __name__ == "__main__": + main() diff --git a/tools/joystick/web.py b/tools/joystick/web.py deleted file mode 100755 index 5cba4e938d..0000000000 --- a/tools/joystick/web.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python3 -import time -import threading -from flask import Flask - -import cereal.messaging as messaging - -app = Flask(__name__) -pm = messaging.PubMaster(['testJoystick']) - -index = """ - - - - - -
- -""" - -@app.route("/") -def hello_world(): - return index - -last_send_time = time.monotonic() -@app.route("/control//") -def control(x, y): - global last_send_time - x,y = float(x), float(y) - x = max(-1, min(1, x)) - y = max(-1, min(1, y)) - dat = messaging.new_message('testJoystick') - dat.testJoystick.axes = [y,x] - dat.testJoystick.buttons = [False] - pm.send('testJoystick', dat) - last_send_time = time.monotonic() - return "" - -def handle_timeout(): - while 1: - this_time = time.monotonic() - if (last_send_time+0.5) < this_time: - #print("timeout, no web in %.2f s" % (this_time-last_send_time)) - dat = messaging.new_message('testJoystick') - dat.testJoystick.axes = [0,0] - dat.testJoystick.buttons = [False] - pm.send('testJoystick', dat) - time.sleep(0.1) - -def main(): - threading.Thread(target=handle_timeout, daemon=True).start() - app.run(host="0.0.0.0") - -if __name__ == '__main__': - main()