camera view

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'],

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()