mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-28 19:33:44 +08:00
Bhavya Gada
86 lines
3.1 KiB
Python
86 lines
3.1 KiB
Python
import numpy as np
|
|
|
|
from openpilot.cereal.visionipc import VisionStreamType
|
|
from msgq.visionipc import VisionIpcServer
|
|
from openpilot.cereal import messaging
|
|
|
|
from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
|
|
from openpilot.tools.sim.lib.common import W, H
|
|
|
|
|
|
def rgb_to_nv12(rgb):
|
|
"""Convert RGB image to NV12 (YUV420) format using BT.601 coefficients."""
|
|
h, w = rgb.shape[:2]
|
|
r = rgb[:, :, 0].astype(np.int32)
|
|
g = rgb[:, :, 1].astype(np.int32)
|
|
b = rgb[:, :, 2].astype(np.int32)
|
|
|
|
# Y plane - BT.601 coefficients (matches original OpenCL kernel)
|
|
y = (((b * 13 + g * 65 + r * 33) + 64) >> 7) + 16
|
|
y = np.clip(y, 0, 255).astype(np.uint8)
|
|
|
|
# Subsample RGB for UV (2x2 box filter)
|
|
r_sub = (r[0::2, 0::2] + r[0::2, 1::2] + r[1::2, 0::2] + r[1::2, 1::2] + 2) >> 2
|
|
g_sub = (g[0::2, 0::2] + g[0::2, 1::2] + g[1::2, 0::2] + g[1::2, 1::2] + 2) >> 2
|
|
b_sub = (b[0::2, 0::2] + b[0::2, 1::2] + b[1::2, 0::2] + b[1::2, 1::2] + 2) >> 2
|
|
|
|
# Interleave U and V planes for NV12 format
|
|
u = np.clip((b_sub * 56 - g_sub * 37 - r_sub * 19 + 0x8080) >> 8, 0, 255).astype(np.uint8)
|
|
v = np.clip((r_sub * 56 - g_sub * 47 - b_sub * 9 + 0x8080) >> 8, 0, 255).astype(np.uint8)
|
|
uv = np.stack((u, v), axis=-1).reshape(h // 2, w)
|
|
|
|
# Copy the visible image into the aligned NV12 buffer
|
|
stride, y_height, uv_height, size = get_nv12_info(w, h)
|
|
nv12 = np.zeros(size, dtype=np.uint8)
|
|
planes = nv12[:stride * (y_height + uv_height)].reshape(-1, stride)
|
|
|
|
planes[:h, :w] = y
|
|
planes[y_height:y_height + h // 2, :w] = uv
|
|
|
|
return nv12.tobytes()
|
|
|
|
class Camerad:
|
|
"""Simulates the camerad daemon"""
|
|
def __init__(self, dual_camera):
|
|
self.pm = messaging.PubMaster(['narrowRoadCameraState', 'wideRoadCameraState'])
|
|
|
|
self.frame_road_id = 0
|
|
self.frame_wide_id = 0
|
|
self.vipc_server = VisionIpcServer("camerad")
|
|
|
|
stride, y_height, _, size = get_nv12_info(W, H)
|
|
buffer_args = (5, W, H, size, stride, stride * y_height)
|
|
|
|
self.vipc_server.create_buffers_with_sizes(VisionStreamType.VISION_STREAM_NARROW_ROAD, *buffer_args)
|
|
if dual_camera:
|
|
self.vipc_server.create_buffers_with_sizes(VisionStreamType.VISION_STREAM_WIDE_ROAD, *buffer_args)
|
|
|
|
self.vipc_server.start_listener()
|
|
|
|
def cam_send_yuv_road(self, yuv):
|
|
self._send_yuv(yuv, self.frame_road_id, 'narrowRoadCameraState', VisionStreamType.VISION_STREAM_NARROW_ROAD)
|
|
self.frame_road_id += 1
|
|
|
|
def cam_send_yuv_wide_road(self, yuv):
|
|
self._send_yuv(yuv, self.frame_wide_id, 'wideRoadCameraState', VisionStreamType.VISION_STREAM_WIDE_ROAD)
|
|
self.frame_wide_id += 1
|
|
|
|
def rgb_to_yuv(self, rgb):
|
|
"""Convert RGB to NV12 YUV format."""
|
|
assert rgb.shape == (H, W, 3), f"{rgb.shape}"
|
|
assert rgb.dtype == np.uint8
|
|
return rgb_to_nv12(rgb)
|
|
|
|
def _send_yuv(self, yuv, frame_id, pub_type, yuv_type):
|
|
dat = messaging.new_message(pub_type, valid=True)
|
|
self.vipc_server.send(yuv_type, yuv, frame_id, dat.logMonoTime, dat.logMonoTime)
|
|
|
|
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)
|