From 6233e7b2c0b491c9b368a73f7252c55f6785c325 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 14 Jul 2026 16:33:42 -0500 Subject: [PATCH] camera view --- selfdrive/ui/onroad/augmented_road_view.py | 101 ++++++++++++++++-- .../onroad/starpilot/starpilot_onroad_view.py | 8 +- 2 files changed, 97 insertions(+), 12 deletions(-) diff --git a/selfdrive/ui/onroad/augmented_road_view.py b/selfdrive/ui/onroad/augmented_road_view.py index 6189bd656b..dae2fd0243 100644 --- a/selfdrive/ui/onroad/augmented_road_view.py +++ b/selfdrive/ui/onroad/augmented_road_view.py @@ -1,7 +1,8 @@ import time import numpy as np import pyray as rl -from cereal import log, messaging +from cereal import car, log, messaging +from opendbc.car import structs from msgq.visionipc import VisionStreamType from openpilot.selfdrive.ui import UI_BORDER_SIZE from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus @@ -20,8 +21,16 @@ OpState = log.SelfdriveState.OpenpilotState CALIBRATED = log.LiveCalibrationData.Status.calibrated ROAD_CAM = VisionStreamType.VISION_STREAM_ROAD WIDE_CAM = VisionStreamType.VISION_STREAM_WIDE_ROAD +DRIVER_CAM = VisionStreamType.VISION_STREAM_DRIVER +GEAR_SHIFTER_REVERSE = structs.CarState.GearShifter.reverse DEFAULT_DEVICE_CAMERA = DEVICE_CAMERAS["tici", "ar0231"] +CAMERA_VIEW_AUTO = 0 +CAMERA_VIEW_DRIVER = 1 +CAMERA_VIEW_STANDARD = 2 +CAMERA_VIEW_WIDE = 3 +CAMERA_VIEW_NONE = 4 + BORDER_COLORS = { UIStatus.DISENGAGED: rl.Color(0x12, 0x28, 0x39, 0xFF), # Blue for disengaged state UIStatus.OVERRIDE: rl.Color(0x89, 0x92, 0x8D, 0xFF), # Gray for override state @@ -31,6 +40,7 @@ BORDER_COLORS = { WIDE_CAM_MAX_SPEED = 10.0 # m/s (22 mph) ROAD_CAM_MIN_SPEED = 15.0 # m/s (34 mph) INF_POINT = np.array([1000.0, 0.0, 0.0]) +REVERSE_DRIVER_CAMERA_DELAY_FRAMES = max(1, int(round(gui_app.target_fps * 0.5))) class AugmentedRoadView(CameraView): @@ -45,6 +55,12 @@ class AugmentedRoadView(CameraView): self._matrix_cache_key = (0, 0.0, 0.0, stream_type) self._cached_matrix: np.ndarray | None = None self._content_rect = rl.Rectangle() + self._reverse_driver_camera_frames = 0 + self._reverse_driver_camera_active = False + self._camera_view_none = False + self._driver_stream_active = False + self._draw_road_overlays = True + self._draw_hud_controls = True self.model_renderer = ModelRenderer() self._hud_renderer = HudRenderer() @@ -60,7 +76,13 @@ class AugmentedRoadView(CameraView): if not ui_state.started: return - self._switch_stream_if_needed(ui_state.sm) + camera_view = self._camera_view() + self._camera_view_none = camera_view == CAMERA_VIEW_NONE + self._switch_stream_if_needed(ui_state.sm, camera_view) + in_reverse = self._is_in_reverse() + self._driver_stream_active = self.stream_type == DRIVER_CAM + self._draw_road_overlays = not in_reverse and not self._driver_stream_active and not self._camera_view_none + self._draw_hud_controls = self._camera_view_none or (not in_reverse and not self._driver_stream_active) # Update calibration before rendering self._update_calibration() @@ -85,11 +107,16 @@ class AugmentedRoadView(CameraView): ) # Render the base camera view - super()._render(self._content_rect) + if self._camera_view_none: + rl.draw_rectangle_rec(self._content_rect, rl.BLACK) + else: + super()._render(self._content_rect) # Draw all UI overlays - self.model_renderer.render(self._content_rect) - self._hud_renderer.render(self._content_rect) + if self._draw_road_overlays: + self.model_renderer.render(self._content_rect) + if self._draw_hud_controls: + self._hud_renderer.render(self._content_rect) self.alert_renderer.render(self._content_rect) self.driver_state_renderer.render(self._content_rect) @@ -127,16 +154,69 @@ class AugmentedRoadView(CameraView): rect.width - 2 * border_width, rect.height - 2 * border_width) rl.draw_rectangle_rounded_lines_ex(border_rect, border_roundness, 10, border_width, border_color) - def _switch_stream_if_needed(self, sm): - if sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams: + @staticmethod + def _is_in_reverse() -> bool: + if ui_state.sm.recv_frame["carState"] < ui_state.started_frame: + return False + + try: + gear = ui_state.sm["carState"].gearShifter + except Exception: + return False + + if gear == GEAR_SHIFTER_REVERSE: + return True + + reverse_enum = getattr(car.CarState.GearShifter, "reverse", None) + if reverse_enum is not None and gear == reverse_enum: + return True + + return str(gear).split(".")[-1].lower() == "reverse" + + def is_in_reverse(self) -> bool: + return self._is_in_reverse() + + def _update_reverse_driver_camera_state(self) -> bool: + should_force_driver = ui_state.started and ui_state.params.get_bool("DriverCamera") and self._is_in_reverse() + if not should_force_driver: + self._reverse_driver_camera_frames = 0 + self._reverse_driver_camera_active = False + return False + + self._reverse_driver_camera_frames = min(self._reverse_driver_camera_frames + 1, REVERSE_DRIVER_CAMERA_DELAY_FRAMES) + self._reverse_driver_camera_active = self._reverse_driver_camera_frames >= REVERSE_DRIVER_CAMERA_DELAY_FRAMES + return self._reverse_driver_camera_active + + @staticmethod + def _camera_view() -> int: + camera_view = ui_state.params.get_int("CameraView", return_default=True, default=CAMERA_VIEW_WIDE) + if camera_view not in (CAMERA_VIEW_AUTO, CAMERA_VIEW_DRIVER, CAMERA_VIEW_STANDARD, CAMERA_VIEW_WIDE, CAMERA_VIEW_NONE): + return CAMERA_VIEW_WIDE + return camera_view + + def _switch_stream_if_needed(self, sm, camera_view: int): + if camera_view == CAMERA_VIEW_NONE: + self._reverse_driver_camera_frames = 0 + self._reverse_driver_camera_active = False + return + + if self._update_reverse_driver_camera_state(): + target = DRIVER_CAM + elif camera_view == CAMERA_VIEW_DRIVER: + target = DRIVER_CAM + elif camera_view == CAMERA_VIEW_STANDARD: + target = ROAD_CAM + elif camera_view == CAMERA_VIEW_WIDE: + target = WIDE_CAM if WIDE_CAM in self.available_streams else ROAD_CAM + elif sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams: v_ego = sm['carState'].vEgo if v_ego < WIDE_CAM_MAX_SPEED: target = WIDE_CAM elif v_ego > ROAD_CAM_MIN_SPEED: target = ROAD_CAM else: - # Hysteresis zone - keep current stream - target = self.stream_type + # Hysteresis zone - keep current road camera selection. + target = WIDE_CAM if self.stream_type == WIDE_CAM else ROAD_CAM else: target = ROAD_CAM @@ -167,6 +247,9 @@ class AugmentedRoadView(CameraView): 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: + if self.stream_type == DRIVER_CAM: + return CameraView._calc_frame_matrix(self, rect) + # Check if we can use cached matrix cache_key = ( ui_state.sm.recv_frame['liveCalibration'], diff --git a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py index 696b44088b..9b41cabacd 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py @@ -44,9 +44,11 @@ class StarPilotOnroadView(AugmentedRoadView): if not ui_state.started: return - self._render_slc() - self._render_overlays() - self._render_path_features(rect) + if self._draw_hud_controls: + self._render_slc() + self._render_overlays() + if self._draw_road_overlays: + self._render_path_features(rect) def _render_slc(self): alert_showing, _ = self.alert_renderer.will_render()