128 lines
3.9 KiB
Python
128 lines
3.9 KiB
Python
import asyncio
|
|
import dataclasses
|
|
import json
|
|
import logging
|
|
import os
|
|
import ssl
|
|
import subprocess
|
|
|
|
import pyaudio
|
|
import wave
|
|
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
|
|
|
|
|
|
## UTILS
|
|
async def play_sound(sound: str):
|
|
SOUNDS = {
|
|
"engage": "selfdrive/assets/sounds/engage.wav",
|
|
"disengage": "selfdrive/assets/sounds/disengage.wav",
|
|
"error": "selfdrive/assets/sounds/warning_immediate.wav",
|
|
}
|
|
assert sound in SOUNDS
|
|
|
|
chunk = 5120
|
|
with wave.open(os.path.join(BASEDIR, 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()
|
|
|
|
## 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 sound(request: 'web.Request'):
|
|
params = await request.json()
|
|
sound_to_play = params["sound"]
|
|
|
|
await play_sound(sound_to_play)
|
|
return web.json_response({"status": "ok"})
|
|
|
|
|
|
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 microphone and audio autoplay 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_post("/sound", sound)
|
|
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()
|