diff --git a/tools/webcam/README.md b/tools/webcam/README.md index 237e07cdb6..bdf39b8145 100644 --- a/tools/webcam/README.md +++ b/tools/webcam/README.md @@ -1,5 +1,3 @@ -# NOTE: this README is outdated. #24590 tracks adding back webcam support - # Run openpilot with webcam on PC What's needed: @@ -24,9 +22,6 @@ git clone https://github.com/commaai/openpilot.git ## Build openpilot for webcam ``` cd ~/openpilot -``` -- check out system/camerad/cameras/camera_webcam.cc lines 72 and 146 before building if any camera is upside down -``` USE_WEBCAM=1 scons -j$(nproc) ``` diff --git a/tools/webcam/camera.py b/tools/webcam/camera.py new file mode 100644 index 0000000000..22438f0c98 --- /dev/null +++ b/tools/webcam/camera.py @@ -0,0 +1,29 @@ +import cv2 as cv +import numpy as np + +class Camera: + def __init__(self, cam_type_state, stream_type, camera_id): + self.cam_type_state = cam_type_state + self.stream_type = stream_type + self.cur_frame_id = 0 + + self.cap = cv.VideoCapture(camera_id) + self.W = self.cap.get(cv.CAP_PROP_FRAME_WIDTH) + self.H = self.cap.get(cv.CAP_PROP_FRAME_HEIGHT) + + @classmethod + def bgr2nv12(self, bgr): + yuv = cv.cvtColor(bgr, cv.COLOR_BGR2YUV_I420) + uv_row_cnt = yuv.shape[0] // 3 + uv_plane = np.transpose(yuv[uv_row_cnt * 2:].reshape(2, -1), [1, 0]) + yuv[uv_row_cnt * 2:] = uv_plane.reshape(uv_row_cnt, -1) + return yuv + + def read_frames(self): + while True: + sts , frame = self.cap.read() + if not sts: + break + yuv = Camera.bgr2nv12(frame) + yield yuv.data.tobytes() + self.cap.release() diff --git a/tools/webcam/camerad.py b/tools/webcam/camerad.py new file mode 100755 index 0000000000..caf044e7b6 --- /dev/null +++ b/tools/webcam/camerad.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 +import threading +import os +from collections import namedtuple + +from cereal.visionipc import VisionIpcServer, VisionStreamType +from cereal import messaging + +from openpilot.tools.webcam.camera import Camera +from openpilot.common.realtime import Ratekeeper + +DUAL_CAM = os.getenv("DUAL_CAMERA") +CameraType = namedtuple("CameraType", ["msg_name", "stream_type", "cam_id"]) +CAMERAS = [ + CameraType("roadCameraState", VisionStreamType.VISION_STREAM_ROAD, os.getenv("CAMERA_ROAD_ID", "0")), + CameraType("driverCameraState", VisionStreamType.VISION_STREAM_DRIVER, os.getenv("CAMERA_DRIVER_ID", "1")), +] +if DUAL_CAM: + CAMERAS.append(CameraType("wideRoadCameraState", VisionStreamType.VISION_STREAM_WIDE_ROAD, DUAL_CAM)) + +class Camerad: + def __init__(self): + self.pm = messaging.PubMaster([c.msg_name for c in CAMERAS]) + self.vipc_server = VisionIpcServer("camerad") + + self.cameras = [] + for c in CAMERAS: + cam = Camera(c.msg_name, c.stream_type, int(c.cam_id)) + assert cam.cap.isOpened(), f"Can't find camera {c}" + self.cameras.append(cam) + self.vipc_server.create_buffers(c.stream_type, 20, False, cam.W, cam.H) + + self.vipc_server.start_listener() + + def _send_yuv(self, yuv, frame_id, pub_type, yuv_type): + eof = int(frame_id * 0.05 * 1e9) + self.vipc_server.send(yuv_type, yuv, frame_id, eof, eof) + dat = messaging.new_message(pub_type, valid=True) + msg = { + "frameId": frame_id, + "transform": [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] + } + setattr(dat, pub_type, msg) + self.pm.send(pub_type, dat) + + def camera_runner(self, cam): + rk = Ratekeeper(20, None) + while cam.cap.isOpened(): + for yuv in cam.read_frames(): + self._send_yuv(yuv, cam.cur_frame_id, cam.cam_type_state, cam.stream_type) + cam.cur_frame_id += 1 + rk.keep_time() + + def run(self): + threads = [] + for cam in self.cameras: + cam_thread = threading.Thread(target=self.camera_runner, args=(cam,)) + cam_thread.start() + threads.append(cam_thread) + + for t in threads: + t.join() + +if __name__ == "__main__": + camerad = Camerad() + camerad.run() diff --git a/tools/webcam/start_camerad.sh b/tools/webcam/start_camerad.sh new file mode 100755 index 0000000000..fbda2be9f3 --- /dev/null +++ b/tools/webcam/start_camerad.sh @@ -0,0 +1,14 @@ +#!/bin/bash + +# export the block below when call manager.py +export BLOCK="${BLOCK},camerad" +export USE_WEBCAM="1" + +# Change camera index according to your setting +export CAMERA_ROAD_ID="0" +export CAMERA_DRIVER_ID="1" +export DUAL_CAMERA="2" # camera index for wide road camera + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" + +$DIR/camerad.py