diff --git a/selfdrive/ui/onroad/augmented_road_view.py b/selfdrive/ui/onroad/augmented_road_view.py index d727337e31..47769f268e 100644 --- a/selfdrive/ui/onroad/augmented_road_view.py +++ b/selfdrive/ui/onroad/augmented_road_view.py @@ -16,6 +16,8 @@ from openpilot.common.transformations.orientation import rot_from_euler OpState = log.SelfdriveState.OpenpilotState CALIBRATED = log.LiveCalibrationData.Status.calibrated +ROAD_CAM = VisionStreamType.VISION_STREAM_ROAD +WIDE_CAM = VisionStreamType.VISION_STREAM_WIDE_ROAD DEFAULT_DEVICE_CAMERA = DEVICE_CAMERAS["tici", "ar0231"] BORDER_COLORS = { @@ -36,6 +38,7 @@ class AugmentedRoadView(CameraView): self._last_calib_time: float = 0 self._last_rect_dims = (0.0, 0.0) + self._last_stream_type = stream_type self._cached_matrix: np.ndarray | None = None self._content_rect = rl.Rectangle() @@ -120,6 +123,7 @@ class AugmentedRoadView(CameraView): current_dims = (self._content_rect.width, self._content_rect.height) if (self._last_calib_time == calib_time and self._last_rect_dims == current_dims and + self._last_stream_type == self.stream_type and self._cached_matrix is not None): return self._cached_matrix @@ -155,9 +159,10 @@ class AugmentedRoadView(CameraView): except (ZeroDivisionError, OverflowError): x_offset, y_offset = 0, 0 - # Update cache values + # Cache the computed transformation matrix to avoid recalculations self._last_calib_time = calib_time self._last_rect_dims = current_dims + self._last_stream_type = self.stream_type self._cached_matrix = np.array([ [zoom * 2 * cx / w, 0, -x_offset / w * 2], [0, zoom * 2 * cy / h, -y_offset / h * 2], @@ -176,14 +181,15 @@ class AugmentedRoadView(CameraView): if __name__ == "__main__": gui_app.init_window("OnRoad Camera View") - road_camera_view = AugmentedRoadView(VisionStreamType.VISION_STREAM_ROAD) + road_camera_view = AugmentedRoadView(ROAD_CAM) print("***press space to switch camera view***") try: for _ in gui_app.render(): ui_state.update() if rl.is_key_released(rl.KeyboardKey.KEY_SPACE): - is_wide = road_camera_view.stream_type == VisionStreamType.VISION_STREAM_WIDE_ROAD - road_camera_view.switch_stream(VisionStreamType.VISION_STREAM_ROAD if is_wide else VisionStreamType.VISION_STREAM_WIDE_ROAD) + if WIDE_CAM in road_camera_view.available_streams: + stream = ROAD_CAM if road_camera_view.stream_type == WIDE_CAM else WIDE_CAM + road_camera_view.switch_stream(stream) road_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) finally: road_camera_view.close() diff --git a/selfdrive/ui/onroad/cameraview.py b/selfdrive/ui/onroad/cameraview.py index 2bc49fec03..9f58133abf 100644 --- a/selfdrive/ui/onroad/cameraview.py +++ b/selfdrive/ui/onroad/cameraview.py @@ -57,9 +57,17 @@ else: class CameraView: def __init__(self, name: str, stream_type: VisionStreamType): - self.client = VisionIpcClient(name, stream_type, conflate=True) 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 @@ -91,12 +99,20 @@ class CameraView: self._placeholder_color = color def switch_stream(self, stream_type: VisionStreamType) -> None: - if self._stream_type != stream_type: - cloudlog.debug(f'switching stream from {self._stream_type} to {stream_type}') - self._clear_textures() - self.frame = None - self._stream_type = stream_type - self.client = VisionIpcClient(self._name, stream_type, conflate=True) + 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: @@ -135,6 +151,9 @@ class CameraView: ]) def render(self, rect: rl.Rectangle): + if self._switching: + self._handle_switch() + if not self._ensure_connection(): self._draw_placeholder(rect) return @@ -226,6 +245,7 @@ class CameraView: 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() @@ -237,16 +257,57 @@ class CameraView: return False cloudlog.debug(f"Connected to {self._name} stream: {self._stream_type}, buffers: {self.client.num_buffers}") - self._clear_textures() + 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)) - return True - def _clear_textures(self): if self.texture_y and self.texture_y.id: rl.unload_texture(self.texture_y)