Python camerad for webcam (#30930)

* webcamd

* remove destroy windows

* block

* shebang execute

* executaleeeee

* executableeeeeee

* she bang for camerad.py

* listener

* no signal?

* good

* ruff fix

* delete listener

* recover sim

* remove assert

* refactor

* fix shell

* fix namedtuple

* little change

* to tuple

* cleanup

* update readme

---------

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 7fdd5c4a7d

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)
 ```
 

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()

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()

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