Files
2026-08-03 13:27:23 -05:00

695 lines
25 KiB
Python

import os
import platform
import weakref
import numpy as np
import pyray as rl
from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
from openpilot.common.swaglog import cloudlog
from openpilot.system.hardware import PC, TICI
from openpilot.system.ui.lib.application import gui_app
from openpilot.system.ui.lib.egl import (
init_egl, is_egl_initialized, finish_gl, create_egl_image, destroy_egl_image,
bind_egl_image_to_texture, create_external_texture, destroy_external_texture, EGLImage,
)
from openpilot.system.ui.widgets import Widget
from openpilot.selfdrive.ui.ui_state import ui_state
CONNECTION_RETRY_INTERVAL = 0.2 # seconds between connection attempts
MICI_FORCE_TEXTURE_CAMERA = os.getenv("MICI_FORCE_TEXTURE_CAMERA", "0") == "1"
# One stale frame can be normal ring-buffer reuse; repeated consecutive regressions demote EGL.
EGL_REGRESSIVE_FRAME_FALLBACK_THRESHOLD = 3
VERSION = """
#version 300 es
precision mediump float;
"""
if platform.system() == "Darwin":
VERSION = """
#version 330 core
"""
VERTEX_SHADER = VERSION + """
in vec3 vertexPosition;
in vec2 vertexTexCoord;
in vec3 vertexNormal;
in vec4 vertexColor;
uniform mat4 mvp;
out vec2 fragTexCoord;
out vec4 fragColor;
void main() {
fragTexCoord = vertexTexCoord;
fragColor = vertexColor;
gl_Position = mvp * vec4(vertexPosition, 1.0);
}
"""
FRAME_FRAGMENT_SHADER_EXTERNAL = """
#version 300 es
#extension GL_OES_EGL_image_external_essl3 : enable
precision mediump float;
in vec2 fragTexCoord;
uniform samplerExternalOES texture0;
uniform int enhance_driver;
out vec4 fragColor;
void main() {
vec4 color = texture(texture0, fragTexCoord);
color.rgb = pow(color.rgb, vec3(1.0/1.28));
if (enhance_driver == 1) {
float brightness = 1.1;
color.rgb = color.rgb + 0.15;
color.rgb = clamp((color.rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0);
color.rgb = color.rgb * color.rgb * (3.0 - 2.0 * color.rgb);
color.rgb = pow(color.rgb, vec3(0.8));
}
fragColor = vec4(color.rgb, color.a);
}
"""
FRAME_FRAGMENT_SHADER_YUV = VERSION + """
in vec2 fragTexCoord;
uniform sampler2D texture0;
uniform sampler2D texture1;
uniform int enhance_driver;
out vec4 fragColor;
void main() {
float y = texture(texture0, fragTexCoord).r;
vec2 uv = texture(texture1, fragTexCoord).ra - 0.5;
vec3 rgb = vec3(y + 1.402*uv.y, y - 0.344*uv.x - 0.714*uv.y, y + 1.772*uv.x);
if (enhance_driver == 1) {
float brightness = 1.1;
rgb = rgb + 0.15;
rgb = clamp((rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0);
rgb = rgb * rgb * (3.0 - 2.0 * rgb);
rgb = pow(rgb, vec3(0.8));
}
fragColor = vec4(rgb, 1.0);
}
"""
FRAME_FRAGMENT_SHADER_EXTERNAL_MICI = """
#version 300 es
#extension GL_OES_EGL_image_external_essl3 : enable
precision mediump float;
in vec2 fragTexCoord;
uniform samplerExternalOES texture0;
uniform int enhance_driver;
out vec4 fragColor;
void main() {
vec4 color = texture(texture0, fragTexCoord);
float gray = dot(color.rgb, vec3(0.299, 0.587, 0.114));
color.rgb = mix(vec3(gray), color.rgb, 0.2);
color.rgb = clamp((color.rgb - 0.5) * 1.2 + 0.5, 0.0, 1.0);
color.rgb = pow(color.rgb, vec3(1.0/1.28));
if (enhance_driver == 1) {
float brightness = 1.1;
color.rgb = color.rgb + 0.15;
color.rgb = clamp((color.rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0);
color.rgb = color.rgb * color.rgb * (3.0 - 2.0 * color.rgb);
color.rgb = pow(color.rgb, vec3(0.8));
}
fragColor = vec4(color.rgb, color.a);
}
"""
FRAME_FRAGMENT_SHADER_YUV_MICI = VERSION + """
in vec2 fragTexCoord;
uniform sampler2D texture0;
uniform sampler2D texture1;
uniform int enhance_driver;
out vec4 fragColor;
void main() {
float y = texture(texture0, fragTexCoord).r;
vec2 uv = texture(texture1, fragTexCoord).ra - 0.5;
vec3 rgb = vec3(y + 1.402*uv.y, y - 0.344*uv.x - 0.714*uv.y, y + 1.772*uv.x);
float gray = dot(rgb, vec3(0.299, 0.587, 0.114));
rgb = mix(vec3(gray), rgb, 0.2);
rgb = clamp((rgb - 0.5) * 1.2 + 0.5, 0.0, 1.0);
if (enhance_driver == 1) {
float brightness = 1.1;
rgb = rgb + 0.15;
rgb = clamp((rgb - 0.5) * (brightness * 0.8) + 0.5, 0.0, 1.0);
rgb = rgb * rgb * (3.0 - 2.0 * rgb);
rgb = pow(rgb, vec3(0.8));
}
fragColor = vec4(rgb, 1.0);
}
"""
class CameraView(Widget):
_use_upstream_engaged_color = False
def __init__(self, name: str, stream_type: VisionStreamType):
super().__init__()
self._name = name
# Primary stream
self.client: VisionIpcClient | None = None
self._stream_type = stream_type
self.available_streams: list[VisionStreamType] = []
# Target stream for switching
self._target_client: VisionIpcClient | None = None
self._target_stream_type: VisionStreamType | None = None
self._switching: bool = False
self._texture_needs_update = True
self.last_connection_attempt: float = 0.0
self._use_egl = TICI and not MICI_FORCE_TEXTURE_CAMERA and init_egl()
if TICI and MICI_FORCE_TEXTURE_CAMERA:
cloudlog.warning("CameraView EGL disabled by MICI_FORCE_TEXTURE_CAMERA, using texture rendering")
elif TICI and not self._use_egl:
cloudlog.error("CameraView EGL init failed, falling back to texture rendering")
self._enhance_driver_val = rl.ffi.new("int[1]", [0])
self._load_frame_shader()
if self._use_egl and not self.shader.id:
cloudlog.error("CameraView EGL shader failed, falling back to texture rendering")
self._use_egl = False
self._load_frame_shader()
self.frame: VisionBuf | None = None
self._last_frame_id = -1
self._regressive_frame_count = 0
self.texture_y: rl.Texture | None = None
self.texture_uv: rl.Texture | None = None
# EGL resources
self.egl_images: dict[int, EGLImage] = {}
self.egl_texture: rl.Texture | None = None
self._external_texture_id = 0
self._placeholder_color: rl.Color | None = None
self._closed = False
self._onroad_reentry_pending = False
self._reentry_stream_selected = False
if self._use_egl and not self._create_egl_texture():
cloudlog.error("CameraView EGL texture creation failed, falling back to texture rendering")
self._use_egl = False
if self.shader and self.shader.id:
rl.unload_shader(self.shader)
self.shader.id = 0
self._load_frame_shader()
cloudlog.info(f"CameraView using {'EGL zero-copy' if self._use_egl else 'texture-copy'} rendering for {stream_type}")
self_ref = weakref.ref(self)
def offroad_transition_callback():
if (view := self_ref()) is not None:
view._offroad_transition()
self._offroad_transition_callback = offroad_transition_callback
ui_state.add_offroad_transition_callback(self._offroad_transition_callback)
def _offroad_transition(self):
self._reset_camera_connection()
def _retire_active_client(self) -> None:
"""Release graphics, frame, and client as one camera generation."""
self._clear_textures()
self.frame = None
self.client = None
def _reset_camera_connection(self):
self._cancel_pending_switch()
self._retire_active_client()
self._last_frame_id = -1
self._regressive_frame_count = 0
self.available_streams.clear()
self._texture_needs_update = True
self.last_connection_attempt = 0.0
self._onroad_reentry_pending = ui_state.is_onroad()
self._reentry_stream_selected = False
def _set_placeholder_color(self, color: rl.Color):
"""Set a placeholder color to be drawn when no frame is available."""
self._placeholder_color = color
def _refresh_available_streams(self) -> None:
streams = VisionIpcClient.available_streams(self._name, block=False)
if streams:
self.available_streams = list(streams)
def switch_stream(self, stream_type: VisionStreamType) -> None:
if getattr(self, "_onroad_reentry_pending", False):
if (getattr(self, "_reentry_stream_selected", False) and self._stream_type == stream_type and
(not self._switching or self._target_stream_type == stream_type)):
return
self._select_reentry_stream(stream_type)
return
if self._switching:
if self._target_stream_type == stream_type:
return
self._cancel_pending_switch()
if self._stream_type == stream_type:
return
cloudlog.debug(f'Preparing switch from {self._stream_type} to {stream_type}')
if self._target_client:
del self._target_client
self._target_stream_type = stream_type
self._target_client = VisionIpcClient(self._name, stream_type, conflate=True)
self._switching = True
def _cancel_pending_switch(self) -> None:
if self._target_client is not None:
cloudlog.debug(f"Cancelling pending camera switch to {self._target_stream_type}")
self._target_client = None
self._target_stream_type = None
self._switching = False
def _discard_pending_client(self) -> None:
"""Discard a failed candidate while retaining the requested stream."""
self._target_client = None
self._switching = False
def _select_reentry_stream(self, stream_type: VisionStreamType) -> None:
"""Select the desired stream before displaying any post-transition frame."""
self._cancel_pending_switch()
if self._stream_type != stream_type:
self._retire_active_client()
self._stream_type = stream_type
self.frame = None
self._last_frame_id = -1
self._regressive_frame_count = 0
self._texture_needs_update = True
self._reentry_stream_selected = True
@property
def stream_type(self) -> VisionStreamType:
return self._stream_type
def close(self) -> None:
if self._closed:
return
self._closed = True
callback = getattr(self, "_offroad_transition_callback", None)
if callback is not None:
ui_state.remove_offroad_transition_callback(callback)
self._offroad_transition_callback = None
self._cancel_pending_switch()
self._retire_active_client()
# Clean up shader
if self.shader and self.shader.id:
rl.unload_shader(self.shader)
self.shader.id = 0
self.frame = None
self._last_frame_id = -1
self.available_streams.clear()
self._onroad_reentry_pending = False
self._reentry_stream_selected = False
def __del__(self):
self.close()
def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray:
if not self.frame:
return np.eye(3)
# Calculate aspect ratios
widget_aspect_ratio = rect.width / rect.height
frame_aspect_ratio = self.frame.width / self.frame.height
# Calculate scaling factors to maintain aspect ratio
zx = min(frame_aspect_ratio / widget_aspect_ratio, 1.0)
zy = min(widget_aspect_ratio / frame_aspect_ratio, 1.0)
return np.array([
[zx, 0.0, 0.0],
[0.0, zy, 0.0],
[0.0, 0.0, 1.0]
])
def _render(self, rect: rl.Rectangle):
if self._switching:
self._handle_switch()
if self._onroad_reentry_pending and not self._reentry_stream_selected:
# Standalone CameraView users have no higher-level stream selector.
self._select_reentry_stream(self._stream_type)
if not self._ensure_connection():
self._draw_placeholder(rect)
return
if self._use_egl:
self._observe_displayed_frame()
# Try to get a new buffer without blocking
buffer = self.client.recv(timeout_ms=0)
if buffer:
self._accept_frame(buffer, self.client.frame_id)
elif not self.client.is_connected():
# ensure we clear the displayed frame when the connection is lost
self.frame = None
if not self.frame:
self._draw_placeholder(rect)
return
transform = self._calc_frame_matrix(rect)
src_rect = rl.Rectangle(0, 0, float(self.frame.width), float(self.frame.height))
# Flip driver camera horizontally
if self._stream_type == VisionStreamType.VISION_STREAM_DRIVER:
src_rect.width = -src_rect.width
# Calculate scale
scale_x = rect.width * transform[0, 0] # zx
scale_y = rect.height * transform[1, 1] # zy
# Calculate base position (centered)
x_offset = rect.x + (rect.width - scale_x) / 2
y_offset = rect.y + (rect.height - scale_y) / 2
x_offset += transform[0, 2] * rect.width / 2
y_offset += transform[1, 2] * rect.height / 2
dst_rect = rl.Rectangle(x_offset, y_offset, scale_x, scale_y)
if self._use_egl:
try:
rendered = self._render_egl(src_rect, dst_rect)
except Exception:
cloudlog.exception("CameraView EGL rendering failed")
rendered = False
if not rendered:
self._fallback_to_textures("EGL frame rendering failed")
if not self._use_egl:
self._render_textures(src_rect, dst_rect)
def _draw_placeholder(self, rect: rl.Rectangle):
if self._placeholder_color:
rl.draw_rectangle_rec(rect, self._placeholder_color)
def _load_frame_shader(self) -> None:
if self._use_upstream_engaged_color:
frame_shader = FRAME_FRAGMENT_SHADER_EXTERNAL_MICI if self._use_egl else FRAME_FRAGMENT_SHADER_YUV_MICI
else:
frame_shader = FRAME_FRAGMENT_SHADER_EXTERNAL if self._use_egl else FRAME_FRAGMENT_SHADER_YUV
self.shader = rl.load_shader_from_memory(VERTEX_SHADER, frame_shader)
self._texture1_loc = -1 if self._use_egl else rl.get_shader_location(self.shader, "texture1")
self._enhance_driver_loc = rl.get_shader_location(self.shader, "enhance_driver")
def _update_shader_state(self) -> None:
self._enhance_driver_val[0] = 1 if self._stream_type == VisionStreamType.VISION_STREAM_DRIVER else 0
if self._enhance_driver_loc >= 0:
rl.set_shader_value(self.shader, self._enhance_driver_loc, self._enhance_driver_val,
rl.ShaderUniformDataType.SHADER_UNIFORM_INT)
def _observe_displayed_frame(self) -> None:
if self.frame is not None:
client_frame_id = getattr(self.client, "frame_id", -1) if hasattr(self, "client") and self.client is not None else -1
frame_id = getattr(self.frame, "frame_id", client_frame_id)
self._last_frame_id = max(self._last_frame_id, int(frame_id))
def _accept_frame(self, frame: VisionBuf, packet_frame_id: int) -> bool:
content_frame_id = int(getattr(frame, "frame_id", packet_frame_id))
if content_frame_id != packet_frame_id:
cloudlog.debug(
f"Dropping inconsistent {self._name} frame: content={content_frame_id}, packet={packet_frame_id}"
)
return False
# Device camera frame IDs are monotonic; reject older reusable ring-buffer
# slots there. Desktop replay intentionally lowers IDs when seeking backward.
if not PC and content_frame_id < self._last_frame_id:
self._regressive_frame_count += 1
if self._regressive_frame_count == 1 or self._regressive_frame_count % 100 == 0:
message = f"Dropping regressive {self._name} frame: content={content_frame_id}, packet={packet_frame_id}, "
message += f"displayed={self._last_frame_id}, idx={frame.idx}, count={self._regressive_frame_count}"
cloudlog.warning(message)
if getattr(self, "_use_egl", False) and self._regressive_frame_count >= EGL_REGRESSIVE_FRAME_FALLBACK_THRESHOLD:
self._fallback_to_textures("repeated regressive frames")
return False
self.frame = frame
self._last_frame_id = content_frame_id
self._regressive_frame_count = 0
self._texture_needs_update = True
self._onroad_reentry_pending = False
self._reentry_stream_selected = False
return True
def _render_egl(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> bool:
"""Render using EGL for direct buffer access."""
if self.frame is None or self.egl_texture is None or not self.egl_texture.id or not self._external_texture_id:
return False
idx = self.frame.idx
egl_image = self.egl_images.get(idx)
if egl_image is None:
egl_image = create_egl_image(self.frame.width, self.frame.height, self.frame.stride, self.frame.fd, self.frame.uv_offset)
if egl_image is None:
return False
self.egl_images[idx] = egl_image
self.egl_texture.width = self.frame.width
self.egl_texture.height = self.frame.height
bind_egl_image_to_texture(self._external_texture_id, egl_image)
rl.begin_shader_mode(self.shader)
try:
self._update_shader_state()
rl.draw_texture_pro(self.egl_texture, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE)
finally:
rl.end_shader_mode()
return True
def _fallback_to_textures(self, reason: str) -> None:
if not self._use_egl:
return
cloudlog.error(f"CameraView switching from EGL to texture rendering: {reason}")
self._use_egl = False
try:
self._clear_textures()
except Exception:
cloudlog.exception("CameraView EGL cleanup failed during texture fallback")
if self.shader and self.shader.id:
try:
rl.unload_shader(self.shader)
except Exception:
cloudlog.exception("CameraView EGL shader cleanup failed during texture fallback")
self.shader.id = 0
try:
self._load_frame_shader()
self._initialize_textures()
self._texture_needs_update = True
except Exception:
cloudlog.exception("CameraView texture fallback initialization failed")
def _render_textures(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None:
"""Copy camera data into ordinary Raylib textures before drawing.
Raylib batches camera draws as GL_TEXTURE_2D. Imported EGL images are
GL_TEXTURE_EXTERNAL_OES objects and cannot safely pass through that path;
copying also prevents the GPU from sampling camerad's reusable buffers
after they have been handed back to the producer.
"""
if (self.texture_y is None or not self.texture_y.id or
self.texture_uv is None or not self.texture_uv.id or self.frame is None):
return
# Update textures with new frame data
if self._texture_needs_update:
y_data = self.frame.data[: self.frame.uv_offset]
uv_data = self.frame.data[self.frame.uv_offset:]
rl.update_texture(self.texture_y, rl.ffi.cast("void *", rl.ffi.from_buffer(y_data)))
rl.update_texture(self.texture_uv, rl.ffi.cast("void *", rl.ffi.from_buffer(uv_data)))
self._texture_needs_update = False
# Render with shader
rl.begin_shader_mode(self.shader)
try:
self._update_shader_state()
rl.set_shader_value_texture(self.shader, self._texture1_loc, self.texture_uv)
rl.draw_texture_pro(self.texture_y, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE)
finally:
rl.end_shader_mode()
def _ensure_connection(self) -> bool:
if self.client is not None and self.client.is_connected():
return True
# A pending candidate owns the connection attempt. Poll it until its first
# frame arrives instead of reconnecting the same client in place.
if self._switching:
self._handle_switch()
return self.client is not None and self.client.is_connected()
if self.client is not None:
self._retire_active_client()
self._last_frame_id = -1
self._regressive_frame_count = 0
self.available_streams.clear()
# Throttle connection attempts
current_time = rl.get_time()
if current_time - self.last_connection_attempt < CONNECTION_RETRY_INTERVAL:
return False
self.last_connection_attempt = current_time
stream_type = self._target_stream_type or self._stream_type
self._target_stream_type = stream_type
self._target_client = VisionIpcClient(self._name, stream_type, conflate=True)
self._switching = True
self._handle_switch()
return self.client is not None and self.client.is_connected()
def _handle_switch(self) -> None:
"""Check if target stream is ready and switch immediately."""
if not self._target_client or not self._switching:
return
# Try to connect target if needed
if not self._target_client.is_connected():
if not self._target_client.connect(False) or not self._target_client.num_buffers:
self._discard_pending_client()
return
cloudlog.debug(f"Target stream connected: {self._target_stream_type}")
# Check if target has frames ready
target_frame = self._target_client.recv(timeout_ms=0)
if target_frame:
packet_frame_id = int(getattr(self._target_client, "frame_id", -1))
content_frame_id = int(getattr(target_frame, "frame_id", packet_frame_id))
if content_frame_id != packet_frame_id:
message = f"Discarding inconsistent {self._name} target frame: content={content_frame_id}, "
message += f"packet={packet_frame_id}, stream={self._target_stream_type}"
cloudlog.warning(message)
self._discard_pending_client()
return
self._complete_switch(target_frame)
elif not self._target_client.is_connected():
# A failed recv can invalidate the server/buffer generation. Never
# reconnect this client; the next attempt must use a fresh candidate.
self._discard_pending_client()
def _complete_switch(self, target_frame: VisionBuf) -> None:
"""Instantly switch to target stream."""
cloudlog.debug(f"Switching to {self._target_stream_type}")
target_client = self._target_client
target_stream_type = self._target_stream_type
self._target_client = None
self._target_stream_type = None
self._switching = False
# Retire the old generation before exposing the new client and frame.
self._retire_active_client()
# Switch to target
self.client = target_client
self._stream_type = target_stream_type
self.frame = target_frame
client_frame_id = getattr(self.client, "frame_id", -1) if self.client is not None else -1
self._last_frame_id = int(getattr(self.frame, "frame_id", client_frame_id)) if self.frame is not None else -1
self._regressive_frame_count = 0
self._texture_needs_update = True
self._onroad_reentry_pending = False
self._reentry_stream_selected = False
# Initialize textures for new stream
self._initialize_textures()
available_streams = getattr(self.client, "available_streams", None)
if available_streams is not None:
self.available_streams = available_streams(self._name, block=False)
def _initialize_textures(self):
self._clear_textures()
if self._use_egl:
if not self._create_egl_texture():
self._fallback_to_textures("EGL texture creation failed")
else:
self.texture_y = rl.load_texture_from_image(rl.Image(None, int(self.client.stride),
int(self.client.height), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAYSCALE))
self.texture_uv = rl.load_texture_from_image(rl.Image(None, int(self.client.stride // 2),
int(self.client.height // 2), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAY_ALPHA))
if not self.texture_y.id or not self.texture_uv.id:
cloudlog.error("CameraView texture-copy texture creation failed")
self._clear_textures()
def _create_egl_texture(self) -> bool:
temp_image = None
try:
temp_image = rl.gen_image_color(1, 1, rl.BLACK)
texture = rl.load_texture_from_image(temp_image)
if texture is None or not texture.id:
self.egl_texture = None
return False
self.egl_texture = texture
self._external_texture_id = create_external_texture()
if not self._external_texture_id:
rl.unload_texture(self.egl_texture)
self.egl_texture = None
return False
return True
except Exception:
if self._external_texture_id:
destroy_external_texture(self._external_texture_id)
self._external_texture_id = 0
if self.egl_texture is not None and self.egl_texture.id:
rl.unload_texture(self.egl_texture)
self.egl_texture = None
cloudlog.exception("CameraView failed to create EGL texture")
return False
finally:
if temp_image is not None:
try:
rl.unload_image(temp_image)
except Exception:
cloudlog.exception("CameraView failed to unload temporary EGL image")
def _clear_textures(self):
if ((self._external_texture_id or self.egl_texture is not None or self.egl_images) and is_egl_initialized()):
try:
# Raylib queues draw calls. Submit them before waiting for the GPU so
# no pending batch can still reference an EGL-backed texture.
rl.rl_draw_render_batch_active()
finish_gl()
except Exception:
cloudlog.exception("CameraView failed to synchronize EGL resources")
if self.texture_y is not None:
if self.texture_y.id:
rl.unload_texture(self.texture_y)
self.texture_y = None
if self.texture_uv is not None:
if self.texture_uv.id:
rl.unload_texture(self.texture_uv)
self.texture_uv = None
if self._external_texture_id:
destroy_external_texture(self._external_texture_id)
self._external_texture_id = 0
if self.egl_texture and self.egl_texture.id:
rl.unload_texture(self.egl_texture)
self.egl_texture = None
for data in self.egl_images.values():
destroy_egl_image(data)
self.egl_images = {}
if __name__ == "__main__":
gui_app.init_window("camera view")
road = CameraView("camerad", VisionStreamType.VISION_STREAM_ROAD)
for _ in gui_app.render():
road.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height))