diff --git a/msgq_repo b/msgq_repo index a16cf1f608538d..6d9160994bbb9a 160000 --- a/msgq_repo +++ b/msgq_repo @@ -1 +1 @@ -Subproject commit a16cf1f608538d14f66bd6142230d8728f2d0abc +Subproject commit 6d9160994bbb9a2184feeab287fcd9eb0101f3e2 diff --git a/selfdrive/ui/mici/layouts/onboarding.py b/selfdrive/ui/mici/layouts/onboarding.py index abf772ce5806a5..953b7ba42b3981 100644 --- a/selfdrive/ui/mici/layouts/onboarding.py +++ b/selfdrive/ui/mici/layouts/onboarding.py @@ -34,7 +34,7 @@ def _render(self, rect): rl.begin_scissor_mode(int(rect.x), int(rect.y), int(rect.width), int(rect.height)) self._camera_view._render(rect) - if not self._camera_view.frame: + if not self._camera_view.connected: gui_label(rect, tr("camera starting"), font_size=64, font_weight=FontWeight.BOLD, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER) rl.end_scissor_mode() diff --git a/selfdrive/ui/mici/onroad/augmented_road_view.py b/selfdrive/ui/mici/onroad/augmented_road_view.py index 71ca03cccfac94..d8471a7aa95839 100644 --- a/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/selfdrive/ui/mici/onroad/augmented_road_view.py @@ -293,7 +293,7 @@ def _update_calibration(self): wide_from_device = rot_from_euler(calib.wideFromDeviceEuler) self.view_from_wide_calib = view_frame_from_device_frame @ wide_from_device @ device_from_calib - def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: + def _calc_frame_matrix(self, frame_width: int, frame_height: int, rect: rl.Rectangle) -> np.ndarray: # Get camera configuration # TODO: cache with vEgo? calib_time = ui_state.sm.recv_frame['liveCalibration'] diff --git a/selfdrive/ui/mici/onroad/cameraview.py b/selfdrive/ui/mici/onroad/cameraview.py index 89a4926ce9af7e..aab60bfe92443b 100644 --- a/selfdrive/ui/mici/onroad/cameraview.py +++ b/selfdrive/ui/mici/onroad/cameraview.py @@ -2,15 +2,14 @@ import numpy as np import pyray as rl -from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf -from openpilot.common.swaglog import cloudlog +from msgq.visionipc import VisionStreamType, VisionBuf +from openpilot.selfdrive.ui.mici.onroad.vipc_thread import VisionIpcThread +from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus from openpilot.system.hardware import TICI from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.lib.egl import init_egl, create_egl_image, destroy_egl_image, bind_egl_image_to_texture, EGLImage from openpilot.system.ui.widgets import Widget -from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus -CONNECTION_RETRY_INTERVAL = 0.2 # seconds between connection attempts VERSION = """ #version 300 es @@ -104,30 +103,13 @@ """ -class CameraView(Widget): - def __init__(self, name: str, stream_type: VisionStreamType): +class BaseCameraView(Widget): + def __init__(self, name: str, stream_type: VisionStreamType, fragment_shader: str): super().__init__() - self._name = name - # Primary stream - self.client = VisionIpcClient(name, stream_type, conflate=True) 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.shader = rl.load_shader_from_memory(VERTEX_SHADER, FRAME_FRAGMENT_SHADER) + self.shader = rl.load_shader_from_memory(VERTEX_SHADER, fragment_shader) self._texture1_loc: int = rl.get_shader_location(self.shader, "texture1") if not TICI else -1 - self._engaged_loc = rl.get_shader_location(self.shader, "engaged") - self._engaged_val = rl.ffi.new("int[1]", [1]) - self._enhance_driver_loc = rl.get_shader_location(self.shader, "enhance_driver") - self._enhance_driver_val = rl.ffi.new("int[1]", [1 if stream_type == VisionStreamType.VISION_STREAM_DRIVER else 0]) - self.frame: VisionBuf | None = None self.texture_y: rl.Texture | None = None self.texture_uv: rl.Texture | None = None @@ -148,70 +130,61 @@ def __init__(self, name: str, stream_type: VisionStreamType): rl.unload_image(temp_image) ui_state.add_offroad_transition_callback(self._offroad_transition) + self._vipc_thread = VisionIpcThread(name, stream_type) + + def start(self): + self._vipc_thread.start() + + def stop(self): + self._vipc_thread.stop() def _offroad_transition(self): - # Reconnect if not first time going onroad - if ui_state.is_onroad() and self.frame is not None: - # Prevent old frames from showing when going onroad. Qt has a separate thread - # which drains the VisionIpcClient SubSocket for us. Re-connecting is not enough - # and only clears internal buffers, not the message queue. - self.frame = None - self.available_streams.clear() - if self.client: - del self.client - self.client = VisionIpcClient(self._name, self._stream_type, conflate=True) + if ui_state.is_offroad(): + self.stop() + else: + self.start() 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 switch_stream(self, stream_type: VisionStreamType) -> None: - if self._stream_type == stream_type: - return - - if self._switching and self._target_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 + self._vipc_thread.switch_stream(stream_type) @property def stream_type(self) -> VisionStreamType: - return self._stream_type + return self._vipc_thread._stream_type def close(self) -> None: + self._vipc_thread.stop() self._clear_textures() - # Clean up EGL texture if TICI and self.egl_texture: rl.unload_texture(self.egl_texture) self.egl_texture = None - # Clean up shader if self.shader and self.shader.id: rl.unload_shader(self.shader) self.shader.id = 0 - self.frame = None - self.available_streams.clear() - self.client = None - def __del__(self): self.close() - def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: - if not self.frame: + @property + def connected(self) -> bool: + return self._vipc_thread.connected + + @property + def available_streams(self) -> list[VisionStreamType]: + return self._vipc_thread._available_streams + + def _calc_frame_matrix(self, frame_width: int, frame_height: int, rect: rl.Rectangle) -> np.ndarray: + if frame_width == 0 or frame_height == 0: return np.eye(3) # Calculate aspect ratios widget_aspect_ratio = rect.width / rect.height - frame_aspect_ratio = self.frame.width / self.frame.height + frame_aspect_ratio = frame_width / frame_height # Calculate scaling factors to maintain aspect ratio zx = min(frame_aspect_ratio / widget_aspect_ratio, 1.0) @@ -224,32 +197,25 @@ def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: ]) def _render(self, rect: rl.Rectangle): - if self._switching: - self._handle_switch() - - if not self._ensure_connection(): - self._draw_placeholder(rect) - return - - # Try to get a new buffer without blocking - buffer = self.client.recv(timeout_ms=0) - if buffer: - self._texture_needs_update = True - self.frame = buffer - 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)) + with self._vipc_thread.lock: + frame = self._vipc_thread.get_frame() + if not frame: + self._draw_placeholder(rect) + return + + if self._vipc_thread.just_connected(): + self._initialize_textures(frame) + + self._draw_frame(frame, rect) + + def _draw_frame(self, frame: VisionBuf, rect: rl.Rectangle): + src_rect = rl.Rectangle(0, 0, float(frame.width), float(frame.height)) # Flip driver camera horizontally if self._stream_type == VisionStreamType.VISION_STREAM_DRIVER: src_rect.width = -src_rect.width + transform = self._calc_frame_matrix(frame.width, frame.height, rect) + # Calculate scale scale_x = rect.width * transform[0, 0] # zx scale_y = rect.height * transform[1, 1] # zy @@ -265,134 +231,67 @@ def _render(self, rect: rl.Rectangle): # Render with appropriate method if TICI: - self._render_egl(src_rect, dst_rect) + self._render_egl(frame, src_rect, dst_rect) else: - self._render_textures(src_rect, dst_rect) + self._render_textures(frame, src_rect, dst_rect) def _draw_placeholder(self, rect: rl.Rectangle): if self._placeholder_color: rl.draw_rectangle_rec(rect, self._placeholder_color) - def _render_egl(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None: + def _render_egl(self, frame: VisionBuf, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None: """Render using EGL for direct buffer access""" - if self.frame is None or self.egl_texture is None: - return - - idx = self.frame.idx - egl_image = self.egl_images.get(idx) + assert self.egl_texture # Create EGL image if needed + egl_image = self.egl_images.get(frame.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: - self.egl_images[idx] = egl_image - else: + egl_image = create_egl_image(frame.width, frame.height, frame.stride, frame.fd, frame.uv_offset) + if not egl_image: return + self.egl_images[frame.idx] = egl_image # Update texture dimensions to match current frame - self.egl_texture.width = self.frame.width - self.egl_texture.height = self.frame.height + self.egl_texture.width = frame.width + self.egl_texture.height = frame.height # Bind the EGL image to our texture bind_egl_image_to_texture(self.egl_texture.id, egl_image) # Render with shader rl.begin_shader_mode(self.shader) - self._update_texture_color_filtering() + self._update_shader_uniforms() rl.draw_texture_pro(self.egl_texture, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE) rl.end_shader_mode() - def _render_textures(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None: + def _render_textures(self, frame: VisionBuf, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None: """Render using texture copies""" - if not self.texture_y or not self.texture_uv or self.frame is None: - return + assert self.texture_y and self.texture_uv # 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 *", y_data.ctypes.data)) - rl.update_texture(self.texture_uv, rl.ffi.cast("void *", uv_data.ctypes.data)) - self._texture_needs_update = False + frame_addr = frame.data.ctypes.data + if getattr(self, '_last_frame_addr', None) != frame_addr: + rl.update_texture(self.texture_y, rl.ffi.cast("void *", frame_addr)) + rl.update_texture(self.texture_uv, rl.ffi.cast("void *", frame_addr + frame.uv_offset)) + self._last_frame_addr = frame_addr # Render with shader rl.begin_shader_mode(self.shader) - self._update_texture_color_filtering() + self._update_shader_uniforms() 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) rl.end_shader_mode() - def _update_texture_color_filtering(self): - self._engaged_val[0] = 1 if ui_state.status != UIStatus.DISENGAGED else 0 - rl.set_shader_value(self.shader, self._engaged_loc, self._engaged_val, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) - rl.set_shader_value(self.shader, self._enhance_driver_loc, self._enhance_driver_val, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) - - def _ensure_connection(self) -> bool: - if not self.client.is_connected(): - self.frame = None - 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 - - if not self.client.connect(False) or not self.client.num_buffers: - return False - - cloudlog.debug(f"Connected to {self._name} stream: {self._stream_type}, buffers: {self.client.num_buffers}") - self._initialize_textures() - self.available_streams = self.client.available_streams(self._name, block=False) - - return True - - 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: - return + def _update_shader_uniforms(self): + pass - 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: - self.frame = target_frame # Update current frame to target frame - self._complete_switch() - - def _complete_switch(self) -> None: - """Instantly switch to target stream.""" - cloudlog.debug(f"Switching to {self._target_stream_type}") - # Clean up current resources - if self.client: - del self.client - - # Switch to target - self.client = self._target_client - self._stream_type = self._target_stream_type - self._texture_needs_update = True - - # Reset state - self._target_client = None - self._target_stream_type = None - self._switching = False - - # Initialize textures for new stream - self._initialize_textures() - - def _initialize_textures(self): - self._clear_textures() - if not TICI: - 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)) + def _initialize_textures(self, frame: VisionBuf): + self._clear_textures() + if not TICI: + self.texture_y = rl.load_texture_from_image(rl.Image(None, int(frame.stride), + int(frame.height), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAYSCALE)) + self.texture_uv = rl.load_texture_from_image(rl.Image(None, int(frame.stride // 2), + int(frame.height // 2), 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_GRAY_ALPHA)) def _clear_textures(self): if self.texture_y and self.texture_y.id: @@ -403,15 +302,30 @@ def _clear_textures(self): rl.unload_texture(self.texture_uv) self.texture_uv = None - # Clean up EGL resources if TICI: for data in self.egl_images.values(): destroy_egl_image(data) self.egl_images = {} +class CameraView(BaseCameraView): + def __init__(self, name: str, stream_type: VisionStreamType): + super().__init__(name, stream_type, FRAME_FRAGMENT_SHADER) + self._engaged_loc = rl.get_shader_location(self.shader, "engaged") + self._engaged_val = rl.ffi.new("int[1]", [1]) + self._enhance_driver_loc = rl.get_shader_location(self.shader, "enhance_driver") + self._enhance_driver_val = rl.ffi.new("int[1]", [1 if stream_type == VisionStreamType.VISION_STREAM_DRIVER else 0]) + + def _update_shader_uniforms(self): + """Update shader uniforms based on UI state.""" + self._engaged_val[0] = 1 if ui_state.status != UIStatus.DISENGAGED else 0 + rl.set_shader_value(self.shader, self._engaged_loc, self._engaged_val, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) + rl.set_shader_value(self.shader, self._enhance_driver_loc, self._enhance_driver_val, rl.ShaderUniformDataType.SHADER_UNIFORM_INT) + + if __name__ == "__main__": gui_app.init_window("camera view") road = CameraView("camerad", VisionStreamType.VISION_STREAM_ROAD) + road.start() for _ in gui_app.render(): road.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) diff --git a/selfdrive/ui/mici/onroad/driver_camera_dialog.py b/selfdrive/ui/mici/onroad/driver_camera_dialog.py index 9adb660d8b1c97..c20e4b33307756 100644 --- a/selfdrive/ui/mici/onroad/driver_camera_dialog.py +++ b/selfdrive/ui/mici/onroad/driver_camera_dialog.py @@ -16,8 +16,8 @@ class DriverCameraView(CameraView): - def _calc_frame_matrix(self, rect: rl.Rectangle): - base = super()._calc_frame_matrix(rect) + def _calc_frame_matrix(self, frame_width: int, frame_height: int, rect: rl.Rectangle): + base = super()._calc_frame_matrix(frame_width, frame_height, rect) driver_view_ratio = 1.5 base[0, 0] *= driver_view_ratio base[1, 1] *= driver_view_ratio @@ -49,6 +49,7 @@ def __init__(self, no_escape=False): def show_event(self): super().show_event() + self._camera_view.start() ui_state.params.put_bool("IsDriverViewEnabled", True) self._publish_alert_sound(None) device.reset_interactive_timeout(300) @@ -57,6 +58,7 @@ def show_event(self): def hide_event(self): super().hide_event() + self._camera_view.stop() ui_state.params.put_bool("IsDriverViewEnabled", False) device.reset_interactive_timeout() @@ -82,7 +84,7 @@ def _render(self, rect): rl.begin_scissor_mode(int(rect.x), int(rect.y), int(rect.width), int(rect.height)) self._camera_view._render(rect) - if not self._camera_view.frame: + if not self._camera_view.connected: gui_label(rect, tr("camera starting"), font_size=54, font_weight=FontWeight.BOLD, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER) rl.end_scissor_mode() diff --git a/selfdrive/ui/mici/onroad/vipc_thread.py b/selfdrive/ui/mici/onroad/vipc_thread.py new file mode 100644 index 00000000000000..e2222655161d9f --- /dev/null +++ b/selfdrive/ui/mici/onroad/vipc_thread.py @@ -0,0 +1,108 @@ +import threading +from dataclasses import dataclass +from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf +from openpilot.common.swaglog import cloudlog + + +CONNECTION_RETRY_INTERVAL = 0.2 # seconds between connection attempts + + +@dataclass +class Frame: + frame: VisionBuf + _client: VisionIpcClient # reference to keep buffers valid + + +class VisionIpcThread: + def __init__(self, name: str, stream_type: VisionStreamType): + self._name = name + self._stream_type = stream_type + self._switch_type: VisionStreamType | None = None + self._client: VisionIpcClient | None = None + self._frame: Frame | None = None + self._available_streams: list[VisionStreamType] = [] + self._stop_event = threading.Event() + self._just_connected = True + self.lock = threading.Lock() + self._thread: threading.Thread | None = None + self.connected: bool = False + + def __del__(self): + self.stop() + + def get_frame(self) -> Frame | None: + return self._frame.frame if self._frame else None + + @property + def available_streams(self) -> list[VisionStreamType]: + with self.lock: + return self._available_streams.copy() + + @property + def stream_type(self) -> VisionStreamType: + with self.lock: + return self._stream_type + + def just_connected(self) -> bool: + if self._just_connected: + self._just_connected = False + return True + return False + + def start(self): + self._stop_event.clear() + if self._thread is None or not self._thread.is_alive(): + self._thread = threading.Thread(target=self._thread_func, daemon=True) + self._thread.start() + + def stop(self): + self._stop_event.set() + if self._thread: + self._thread.join() + self._thread = None + + def switch_stream(self, stream_type: VisionStreamType) -> None: + with self.lock: + if self._stream_type != stream_type: + self._switch_type = stream_type + + def _thread_func(self): + client = VisionIpcClient(self._name, self._stream_type, conflate=True) + while not self._stop_event.is_set(): + with self.lock: + if self._switch_type is not None: + cloudlog.debug(f'Switching from {self._stream_type} to {self._switch_type}') + self._stream_type = self._switch_type + client = VisionIpcClient(self._name, self._stream_type, conflate=True) + self._just_connected = True + self._switch_type = None + + if not self._ensure_connection(client): + self.connected = False + self._stop_event.wait(CONNECTION_RETRY_INTERVAL) + continue + + self.connected = True + if buffer := client.recv(timeout_ms=20): + with self.lock: + self._frame = Frame(buffer, client) + + def _ensure_connection(self, client: VisionIpcClient) -> bool: + if client.is_connected(): + return True + + # Check if we need to clear the frame before reconnecting + with self.lock: + if self._frame and client is self._frame._client: + self._frame = None + + if not client.connect(False) or not client.num_buffers: + return False + + cloudlog.debug(f"Connected to {self._name} stream: {self._stream_type}, buffers: {client.num_buffers}") + + with self.lock: + self._just_connected = True + self._available_streams = client.available_streams(self._name, block=False) + + return True diff --git a/selfdrive/ui/onroad/augmented_road_view.py b/selfdrive/ui/onroad/augmented_road_view.py index 1f202141c3806b..f774c4cadaf419 100644 --- a/selfdrive/ui/onroad/augmented_road_view.py +++ b/selfdrive/ui/onroad/augmented_road_view.py @@ -158,7 +158,7 @@ def _update_calibration(self): wide_from_device = rot_from_euler(calib.wideFromDeviceEuler) self.view_from_wide_calib = view_frame_from_device_frame @ wide_from_device @ device_from_calib - def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: + def _calc_frame_matrix(self, frame_width: int, frame_height: int, rect: rl.Rectangle) -> np.ndarray: # Check if we can use cached matrix cache_key = ( ui_state.sm.recv_frame['liveCalibration'], diff --git a/selfdrive/ui/onroad/cameraview.py b/selfdrive/ui/onroad/cameraview.py index 544394846595de..6f177793b5ebd9 100644 --- a/selfdrive/ui/onroad/cameraview.py +++ b/selfdrive/ui/onroad/cameraview.py @@ -1,41 +1,10 @@ -import platform -import numpy as np import pyray as rl -from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf -from openpilot.common.swaglog import cloudlog +from msgq.visionipc import VisionStreamType from openpilot.system.hardware import TICI from openpilot.system.ui.lib.application import gui_app -from openpilot.system.ui.lib.egl import init_egl, create_egl_image, destroy_egl_image, bind_egl_image_to_texture, EGLImage -from openpilot.system.ui.widgets import Widget -from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.selfdrive.ui.mici.onroad.cameraview import BaseCameraView, VERSION -CONNECTION_RETRY_INTERVAL = 0.2 # seconds between connection attempts - -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); -} -""" # Choose fragment shader based on platform capabilities if TICI: @@ -65,302 +34,14 @@ """ -class CameraView(Widget): +class CameraView(BaseCameraView): def __init__(self, name: str, stream_type: VisionStreamType): - super().__init__() - self._name = name - # Primary stream - self.client = VisionIpcClient(name, stream_type, conflate=True) - 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.shader = rl.load_shader_from_memory(VERTEX_SHADER, FRAME_FRAGMENT_SHADER) - self._texture1_loc: int = rl.get_shader_location(self.shader, "texture1") if not TICI else -1 - - self.frame: VisionBuf | None = None - 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._placeholder_color: rl.Color | None = None - - # Initialize EGL for zero-copy rendering on TICI - if TICI: - if not init_egl(): - raise RuntimeError("Failed to initialize EGL") - - # Create a 1x1 pixel placeholder texture for EGL image binding - temp_image = rl.gen_image_color(1, 1, rl.BLACK) - self.egl_texture = rl.load_texture_from_image(temp_image) - rl.unload_image(temp_image) - - ui_state.add_offroad_transition_callback(self._offroad_transition) - - def _offroad_transition(self): - # Reconnect if not first time going onroad - if ui_state.is_onroad() and self.frame is not None: - # Prevent old frames from showing when going onroad. Qt has a separate thread - # which drains the VisionIpcClient SubSocket for us. Re-connecting is not enough - # and only clears internal buffers, not the message queue. - self.frame = None - self.available_streams.clear() - if self.client: - del self.client - self.client = VisionIpcClient(self._name, self._stream_type, conflate=True) - - 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 switch_stream(self, stream_type: VisionStreamType) -> None: - if self._stream_type == stream_type: - return - - if self._switching and self._target_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 - - @property - def stream_type(self) -> VisionStreamType: - return self._stream_type - - def close(self) -> None: - self._clear_textures() - - # Clean up EGL texture - if TICI and self.egl_texture: - rl.unload_texture(self.egl_texture) - self.egl_texture = None - - # Clean up shader - if self.shader and self.shader.id: - rl.unload_shader(self.shader) - - self.frame = None - self.available_streams.clear() - self.client = None - - 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 not self._ensure_connection(): - self._draw_placeholder(rect) - return - - # Try to get a new buffer without blocking - buffer = self.client.recv(timeout_ms=0) - if buffer: - self._texture_needs_update = True - self.frame = buffer - 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) - - # Render with appropriate method - if TICI: - self._render_egl(src_rect, dst_rect) - else: - 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 _render_egl(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None: - """Render using EGL for direct buffer access""" - if self.frame is None or self.egl_texture is None: - return - - idx = self.frame.idx - egl_image = self.egl_images.get(idx) - - # Create EGL image if needed - 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: - self.egl_images[idx] = egl_image - else: - return - - # Update texture dimensions to match current frame - self.egl_texture.width = self.frame.width - self.egl_texture.height = self.frame.height - - # Bind the EGL image to our texture - bind_egl_image_to_texture(self.egl_texture.id, egl_image) - - # Render with shader - rl.begin_shader_mode(self.shader) - rl.draw_texture_pro(self.egl_texture, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE) - rl.end_shader_mode() - - def _render_textures(self, src_rect: rl.Rectangle, dst_rect: rl.Rectangle) -> None: - """Render using texture copies""" - if not self.texture_y or not self.texture_uv 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 *", y_data.ctypes.data)) - rl.update_texture(self.texture_uv, rl.ffi.cast("void *", uv_data.ctypes.data)) - self._texture_needs_update = False - - # Render with shader - rl.begin_shader_mode(self.shader) - 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) - rl.end_shader_mode() - - def _ensure_connection(self) -> bool: - if not self.client.is_connected(): - self.frame = None - 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 - - if not self.client.connect(False) or not self.client.num_buffers: - return False - - cloudlog.debug(f"Connected to {self._name} stream: {self._stream_type}, buffers: {self.client.num_buffers}") - self._initialize_textures() - self.available_streams = self.client.available_streams(self._name, block=False) - - return True - - 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: - 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: - self.frame = target_frame # Update current frame to target frame - self._complete_switch() - - def _complete_switch(self) -> None: - """Instantly switch to target stream.""" - cloudlog.debug(f"Switching to {self._target_stream_type}") - # Clean up current resources - if self.client: - del self.client - - # Switch to target - self.client = self._target_client - self._stream_type = self._target_stream_type - self._texture_needs_update = True - - # Reset state - self._target_client = None - self._target_stream_type = None - self._switching = False - - # Initialize textures for new stream - self._initialize_textures() - - def _initialize_textures(self): - self._clear_textures() - if not TICI: - 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)) - - def _clear_textures(self): - if self.texture_y and self.texture_y.id: - rl.unload_texture(self.texture_y) - self.texture_y = None - - if self.texture_uv and self.texture_uv.id: - rl.unload_texture(self.texture_uv) - self.texture_uv = None - - # Clean up EGL resources - if TICI: - for data in self.egl_images.values(): - destroy_egl_image(data) - self.egl_images = {} + super().__init__(name, stream_type, FRAME_FRAGMENT_SHADER) if __name__ == "__main__": gui_app.init_window("camera view") road = CameraView("camerad", VisionStreamType.VISION_STREAM_ROAD) + road.start() for _ in gui_app.render(): road.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) diff --git a/selfdrive/ui/onroad/driver_camera_dialog.py b/selfdrive/ui/onroad/driver_camera_dialog.py index 543ea35e812cb6..f960c9434ec646 100644 --- a/selfdrive/ui/onroad/driver_camera_dialog.py +++ b/selfdrive/ui/onroad/driver_camera_dialog.py @@ -16,8 +16,10 @@ def __init__(self): # TODO: this can grow unbounded, should be given some thought device.add_interactive_timeout_callback(self.stop_dmonitoringmodeld) ui_state.params.put_bool("IsDriverViewEnabled", True) + self.start() def stop_dmonitoringmodeld(self): + self.stop() ui_state.params.put_bool("IsDriverViewEnabled", False) gui_app.set_modal_overlay(None) @@ -28,7 +30,7 @@ def _handle_mouse_release(self, _): def _render(self, rect): super()._render(rect) - if not self.frame: + if not self.connected: gui_label( rect, tr("camera starting"), @@ -73,17 +75,11 @@ def _draw_face_detection(self, rect: rl.Rectangle) -> None: line_color, ) - def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: + def _calc_frame_matrix(self, frame_width:int, frame_height: int, rect: rl.Rectangle) -> np.ndarray: driver_view_ratio = 2.0 - # Get stream dimensions - if self.frame: - stream_width = self.frame.width - stream_height = self.frame.height - else: - # Default values if frame not available - stream_width = 1928 - stream_height = 1208 + stream_width = frame_width + stream_height = frame_height yscale = stream_height * driver_view_ratio / stream_width xscale = yscale * rect.height / rect.width * stream_width / stream_height diff --git a/selfdrive/ui/watch3.py b/selfdrive/ui/watch3.py index bb64cdc4d543fa..6b5ca7d2e99762 100755 --- a/selfdrive/ui/watch3.py +++ b/selfdrive/ui/watch3.py @@ -11,6 +11,9 @@ road = CameraView("camerad", VisionStreamType.VISION_STREAM_ROAD) driver = CameraView("camerad", VisionStreamType.VISION_STREAM_DRIVER) wide = CameraView("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD) + road.start() + driver.start() + wide.start() for _ in gui_app.render(): road.render(rl.Rectangle(gui_app.width // 4, 0, gui_app.width // 2, gui_app.height // 2)) driver.render(rl.Rectangle(0, gui_app.height // 2, gui_app.width // 2, gui_app.height // 2))