""" Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ """ import time import numpy as np import pyray as rl from cereal import messaging, car, log from openpilot.common.params import Params from msgq.visionipc import VisionStreamType from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus from openpilot.selfdrive.ui.mici.onroad import SIDE_PANEL_WIDTH from openpilot.selfdrive.ui.mici.onroad.alert_renderer import AlertRenderer from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer from openpilot.selfdrive.ui.mici.onroad.hud_renderer import HudRenderer from openpilot.selfdrive.ui.mici.onroad.model_renderer import ModelRenderer from openpilot.selfdrive.ui.mici.onroad.confidence_ball import ConfidenceBall from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView from openpilot.system.ui.lib.application import FontWeight, gui_app, MousePos, MouseEvent from openpilot.system.ui.widgets.label import UnifiedLabel from openpilot.system.ui.widgets import Widget from openpilot.common.issue_debug import log_issue_limited from openpilot.common.filter_simple import BounceFilter from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCameraConfig, view_frame_from_device_frame from openpilot.common.transformations.orientation import rot_from_euler from openpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy from enum import IntEnum from openpilot.iqpilot.ui.onroad.augmented_road_view import BORDER_COLORS_IQ if gui_app.iqpilot_ui(): from openpilot.iqpilot.ui.mici.onroad.hud_renderer import IQMiciHudRenderer as HudRenderer from openpilot.iqpilot.ui.mici.onroad.road_label import RoadNameRendererMici from openpilot.selfdrive.ui.ui_state import OnroadTimerStatus 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"] class BookmarkState(IntEnum): HIDDEN = 0 DRAGGING = 1 TRIGGERED = 2 WIDE_CAM_MAX_SPEED = 5.0 # m/s (10 mph) ROAD_CAM_MIN_SPEED = 10 # m/s (25 mph) CAM_Y_OFFSET = 20 MICI_BORDER_COLOR = rl.Color(0x0C, 0x94, 0x96, 0xFF) MICI_BORDER_THICKNESS = 50 MICI_BORDER_ROUNDNESS = 0.2 * 1.02 MICI_BORDER_BOTTOM_ONLY_HEIGHT = 95 MICI_EXPERIMENTAL_ICON_SIZE = 28 MICI_EXPERIMENTAL_ICON_SPACING = 8 class BookmarkIcon(Widget): PEEK_THRESHOLD = 50 # If icon peeks out this much, snap it fully visible FULL_VISIBLE_OFFSET = 200 # How far onscreen when fully visible HIDDEN_OFFSET = -50 # How far offscreen when hidden def __init__(self, bookmark_callback): super().__init__() self._bookmark_callback = bookmark_callback self._icon = gui_app.texture("icons_mici/onroad/bookmark.png", 180, 180) self._icon_fill = gui_app.texture("icons_mici/onroad/bookmark_fill.png", 180, 180) self._active_icon = self._icon self._offset_filter = BounceFilter(0.0, 0.1, 1 / gui_app.target_fps) # State self._interacting = False self._state = BookmarkState.HIDDEN self._swipe_start_x = 0.0 self._swipe_current_x = 0.0 self._is_swiping = False self._is_swiping_left: bool = False self._triggered_time: float = 0.0 def is_swiping_left(self) -> bool: """Check if currently swiping left (for scroller to disable).""" return self._is_swiping_left def interacting(self): interacting, self._interacting = self._interacting, False return interacting def _update_state(self): if self._state == BookmarkState.DRAGGING: # Allow pulling past activated position with rubber band effect swipe_offset = self._swipe_start_x - self._swipe_current_x swipe_offset = min(swipe_offset, self.FULL_VISIBLE_OFFSET + 50) self._offset_filter.update(swipe_offset) elif self._state == BookmarkState.TRIGGERED: # Continue animating to fully visible self._offset_filter.update(self.FULL_VISIBLE_OFFSET) # Stay in TRIGGERED state for 1 second if rl.get_time() - self._triggered_time >= 1.5: self._state = BookmarkState.HIDDEN elif self._state == BookmarkState.HIDDEN: self._offset_filter.update(self.HIDDEN_OFFSET) if self._offset_filter.x < 1e-3: self._interacting = False self._active_icon = self._icon def _handle_mouse_event(self, mouse_event: MouseEvent): if not ui_state.started: return if mouse_event.left_pressed: # Store relative position within widget self._swipe_start_x = mouse_event.pos.x self._swipe_current_x = mouse_event.pos.x self._is_swiping = True self._is_swiping_left = False self._state = BookmarkState.DRAGGING self._active_icon = self._icon elif mouse_event.left_down and self._is_swiping: self._swipe_current_x = mouse_event.pos.x swipe_offset = self._swipe_start_x - self._swipe_current_x self._is_swiping_left = swipe_offset > 0 if self._is_swiping_left: self._interacting = True elif mouse_event.left_released: if self._is_swiping: swipe_distance = self._swipe_start_x - self._swipe_current_x # If peeking past threshold, transition to animating to fully visible and bookmark if swipe_distance > self.PEEK_THRESHOLD: self._state = BookmarkState.TRIGGERED self._triggered_time = rl.get_time() self._active_icon = self._icon_fill self._bookmark_callback() else: # Otherwise, transition back to hidden self._state = BookmarkState.HIDDEN # Reset swipe state self._is_swiping = False self._is_swiping_left = False def _render(self, _): """Render the bookmark icon.""" if self._offset_filter.x > 0: icon_x = self.rect.x + self.rect.width - round(self._offset_filter.x) icon_y = self.rect.y + (self.rect.height - self._active_icon.height) / 2 # Vertically centered rl.draw_texture(self._active_icon, int(icon_x), int(icon_y), rl.WHITE) class AugmentedRoadView(CameraView): def __init__(self, bookmark_callback=None, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD): super().__init__("camerad", stream_type) self._bookmark_callback = bookmark_callback self._set_placeholder_color(rl.BLACK) self.device_camera: DeviceCameraConfig | None = None self.view_from_calib = view_frame_from_device_frame.copy() self.view_from_wide_calib = view_frame_from_device_frame.copy() self._matrix_cache_key: tuple | None = None self._cached_matrix: np.ndarray | None = None self._content_rect = rl.Rectangle() self._last_click_time = 0.0 # Bookmark icon with swipe gesture self._bookmark_icon = BookmarkIcon(bookmark_callback) self._params = Params() self._iq_dynamic_mode: bool = False self._iq_dynamic_refresh: int = 0 self._model_renderer = ModelRenderer() self._hud_renderer = HudRenderer() self._alert_renderer = AlertRenderer() self._driver_state_renderer = DriverStateRenderer() self._confidence_ball = ConfidenceBall() self._road_name = RoadNameRendererMici() if gui_app.iqpilot_ui() else None self._experimental_txt = gui_app.texture("icons_mici/experimental_mode_mici.png", MICI_EXPERIMENTAL_ICON_SIZE, MICI_EXPERIMENTAL_ICON_SIZE) self._iqdynamic_txt = gui_app.texture("icons_mici/iqdynamic_mode_mici.png", MICI_EXPERIMENTAL_ICON_SIZE, MICI_EXPERIMENTAL_ICON_SIZE) self._iqstandard_txt = gui_app.texture("icons_mici/iqstandard_mode_mici.png", MICI_EXPERIMENTAL_ICON_SIZE, MICI_EXPERIMENTAL_ICON_SIZE) self._offroad_label = UnifiedLabel("start the car to\nuse IQ.Pilot", 54, FontWeight.DISPLAY, text_color=rl.Color(255, 255, 255, int(255 * 0.9)), alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE) # debug self._pm = messaging.PubMaster(['uiDebug']) def is_swiping_left(self) -> bool: """Check if currently swiping left (for scroller to disable).""" return self._bookmark_icon.is_swiping_left() def _update_state(self): super()._update_state() # IQDynamicMode only changes from the settings UI; don't pay a Params syscall every frame on # the onroad hot path. Refresh ~1s (60 frames), matching model_renderer's throttled reads. self._iq_dynamic_refresh -= 1 if self._iq_dynamic_refresh <= 0: self._iq_dynamic_refresh = 60 self._iq_dynamic_mode = self._params.get_bool("IQDynamicMode") # update offroad label if ui_state.panda_type == log.PandaState.PandaType.unknown: self._offroad_label.set_text("system booting") else: self._offroad_label.set_text("start the car to\nuse IQ.Pilot") def _handle_mouse_release(self, mouse_pos: MousePos): # Don't trigger click callback if bookmark was triggered if not self._bookmark_icon.interacting(): super()._handle_mouse_release(mouse_pos) def _render(self, _): start_draw = time.monotonic() self._switch_stream_if_needed(ui_state.sm) # Update calibration before rendering self._update_calibration() # Create inner content area with border padding self._content_rect = rl.Rectangle( self.rect.x, self.rect.y, self.rect.width - SIDE_PANEL_WIDTH, self.rect.height, ) # Enable scissor mode to clip all rendering within content rectangle boundaries # This creates a rendering viewport that prevents graphics from drawing outside the border rl.begin_scissor_mode( int(self._content_rect.x), int(self._content_rect.y), int(self._content_rect.width), int(self._content_rect.height) ) # Render the base camera view super()._render(self._content_rect) # Draw all UI overlays self._model_renderer.render(self._content_rect) alert_to_render, not_animating_out = self._alert_renderer.will_render() # Hide DMoji when disengaged unless AlwaysOnDM is enabled should_draw_dmoji = (not self._hud_renderer.drawing_top_icons() and ui_state.is_onroad() and (ui_state.status != UIStatus.DISENGAGED or ui_state.always_on_dm)) self._driver_state_renderer.set_should_draw(should_draw_dmoji) self._driver_state_renderer.set_position(self._rect.x + 16, self._rect.y + 10) self._driver_state_renderer.render() self._hud_renderer.set_can_draw_top_icons(alert_to_render is None) self._hud_renderer.set_wheel_critical_icon(alert_to_render is not None and not not_animating_out and alert_to_render.visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired) # TODO: have alert renderer draw offroad mici label below if ui_state.started: self._alert_renderer.render(self._content_rect) self._hud_renderer.render(self._content_rect) if self._road_name is not None and alert_to_render is None: self._road_name.update() self._road_name.render(self._content_rect) # don't draw the experimental/IQ.Dynamic icon over alert text (it falls back to the # top-left alert anchor when the DMoji is hidden while disengaged) if alert_to_render is None: self._draw_experimental_icon(should_draw_dmoji) # End clipping region rl.end_scissor_mode() self._draw_border() # Custom UI extension point - add custom overlays here # Use self._content_rect for positioning within camera bounds self._confidence_ball.render(self.rect) self._bookmark_icon.render(self.rect) draw_time_ms = (time.monotonic() - start_draw) * 1000 if draw_time_ms > 40.0: log_issue_limited( "ui_draw_slow_mici", "ui", f"mici onroad draw slow drawTimeMillis={draw_time_ms:.2f} navActive={getattr(ui_state.sm['iqNavState'], 'active', False)}", interval_sec=1.0, ) msg = messaging.new_message('uiDebug') msg.uiDebug.drawTimeMillis = draw_time_ms self._pm.send('uiDebug', msg) # Draw darkened background and text if not onroad if not ui_state.started: rl.draw_rectangle(int(self.rect.x), int(self.rect.y), int(self.rect.width), int(self.rect.height), rl.Color(0, 0, 0, 175)) self._offroad_label.render(self._content_rect) def _draw_experimental_icon(self, draw_below_driver_state: bool) -> None: if not ui_state.started: return if not ui_state.sm['carParams'].openpilotLongitudinalControl: return if ui_state.sm['selfdriveState'].experimentalMode: icon = self._iqdynamic_txt if self._iq_dynamic_mode else self._experimental_txt else: icon = self._iqstandard_txt if draw_below_driver_state: pos_x = self._rect.x + 16 + (self._driver_state_renderer.rect.width - icon.width) / 2 pos_y = self._rect.y + 10 + self._driver_state_renderer.rect.height + MICI_EXPERIMENTAL_ICON_SPACING else: pos_x = self._rect.x + 18 pos_y = self._rect.y + 18 rl.draw_texture(icon, int(pos_x), int(pos_y), rl.WHITE) def _draw_border(self): rl.draw_rectangle_rounded_lines_ex(self._content_rect, MICI_BORDER_ROUNDNESS, 10, MICI_BORDER_THICKNESS, rl.BLACK) aol = ui_state.sm["iqState"].aol ss_enabled = ui_state.sm["selfdriveState"].enabled if aol.active and ss_enabled: rl.draw_rectangle_rounded_lines_ex(self._content_rect, MICI_BORDER_ROUNDNESS, 10, MICI_BORDER_THICKNESS, MICI_BORDER_COLOR) self._reblacken_border_edges() elif aol.active and not ss_enabled: clip_y = int(self._content_rect.y + self._content_rect.height - MICI_BORDER_BOTTOM_ONLY_HEIGHT) rl.begin_scissor_mode(int(self._content_rect.x), clip_y, int(self._content_rect.width), MICI_BORDER_BOTTOM_ONLY_HEIGHT) border_color = BORDER_COLORS_IQ[UIStatus.LAT_ONLY] if ui_state.status != UIStatus.OVERRIDE else rl.Color(0x89, 0x92, 0x8D, 0xFF) rl.draw_rectangle_rounded_lines_ex(self._content_rect, MICI_BORDER_ROUNDNESS, 10, MICI_BORDER_THICKNESS, border_color) rl.end_scissor_mode() self._reblacken_border_edges() def _reblacken_border_edges(self): cr = self._content_rect r = int(MICI_BORDER_ROUNDNESS * min(cr.width, cr.height) / 2) + MICI_BORDER_THICKNESS + 6 regions = ( (cr.x, cr.y, r, r), # top-left corner (cr.x, cr.y + cr.height - r, r, r), # bottom-left corner (cr.x + cr.width - r, cr.y, r + SIDE_PANEL_WIDTH, cr.height), # right edge + both right corners ) for rx, ry, rw, rh in regions: rl.begin_scissor_mode(int(rx), int(ry), int(rw), int(rh)) rl.draw_rectangle_rounded_lines_ex(cr, MICI_BORDER_ROUNDNESS, 10, MICI_BORDER_THICKNESS, rl.BLACK) rl.end_scissor_mode() def _switch_stream_if_needed(self, sm): if 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 else: target = ROAD_CAM if self.stream_type != target: self.switch_stream(target) def _update_calibration(self): # Update device camera if not already set sm = ui_state.sm if not self.device_camera and sm.seen['roadCameraState'] and sm.seen['deviceState']: self.device_camera = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] if not sm.seen["liveCalibration"]: return calib = sm['liveCalibration'] calib_rpy = get_calibrated_rpy(calib) if calib_rpy is None: return # Update view_from_calib matrix prev_view_from_calib = self.view_from_calib.copy() prev_view_from_wide_calib = self.view_from_wide_calib.copy() device_from_calib = rot_from_euler(calib_rpy) self.view_from_calib = view_frame_from_device_frame @ device_from_calib # Update wide calibration if available if hasattr(calib, 'wideFromDeviceEuler') and len(calib.wideFromDeviceEuler) == 3: wide_from_device = rot_from_euler(calib.wideFromDeviceEuler) self.view_from_wide_calib = view_frame_from_device_frame @ wide_from_device @ device_from_calib if (not np.allclose(self.view_from_calib, prev_view_from_calib) or not np.allclose(self.view_from_wide_calib, prev_view_from_wide_calib)): self._matrix_cache_key = (0, 0, 0, self.stream_type, 0.0) self._cached_matrix = None def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: # Early-return the cached matrix when nothing that affects it changed. The key deliberately # excludes rect.x/y — those are applied as a draw-time offset in ModelRenderer (below), so the # cache stays hot while the onroad view translates during a scroll/transition (stock PR #37948). cache_key = ( ui_state.sm.recv_frame['liveCalibration'], int(self._content_rect.width), int(self._content_rect.height), self.stream_type, round(ui_state.sm['carState'].vEgo, 1), ) if cache_key == self._matrix_cache_key and self._cached_matrix is not None: return self._cached_matrix # Get camera configuration device_camera = self.device_camera or DEFAULT_DEVICE_CAMERA is_wide_camera = self.stream_type == WIDE_CAM intrinsic = device_camera.ecam.intrinsics if is_wide_camera else device_camera.fcam.intrinsics calibration = self.view_from_wide_calib if is_wide_camera else self.view_from_calib if is_wide_camera: zoom = 0.7 * 1.5 else: zoom = np.interp(ui_state.sm['carState'].vEgo, [10, 30], [0.8, 1.0]) # Calculate transforms for vanishing point inf_point = np.array([1000.0, 0.0, 0.0]) calib_transform = intrinsic @ calibration kep = calib_transform @ inf_point # Calculate center points and dimensions (rect.x/y are NOT used here — applied at draw time) w, h = self._content_rect.width, self._content_rect.height cx, cy = intrinsic[0, 2], intrinsic[1, 2] # Calculate max allowed offsets with margins margin = 5 max_x_offset = cx * zoom - w / 2 - margin max_y_offset = cy * zoom - h / 2 - margin # Calculate and clamp offsets to prevent out-of-bounds issues try: if abs(kep[2]) > 1e-6: x_offset = np.clip((kep[0] / kep[2] - cx) * zoom, -max_x_offset, max_x_offset) y_offset = np.clip((kep[1] / kep[2] - cy) * zoom + CAM_Y_OFFSET, -max_y_offset, max_y_offset) else: x_offset, y_offset = 0, 0 except (ZeroDivisionError, OverflowError): x_offset, y_offset = 0, 0 # Cache the computed transformation matrix to avoid recalculations self._matrix_cache_key = cache_key self._cached_matrix = np.array([ [zoom * 2 * cx / w, 0, -x_offset / w * 2], [0, zoom * 2 * cy / h, -y_offset / h * 2], [0, 0, 1.0] ]) # Built WITHOUT rect.x/y so the matrix (and the model_renderer projection it drives) stays # cache-stable while the view slides; ModelRenderer adds (rect.x, rect.y) as a draw-time offset. video_transform = np.array([ [zoom, 0.0, (w / 2 - x_offset) - (cx * zoom)], [0.0, zoom, (h / 2 - y_offset) - (cy * zoom)], [0.0, 0.0, 1.0] ]) self._model_renderer.set_transform(video_transform @ calib_transform) return self._cached_matrix def show_event(self): if gui_app.iqpilot_ui(): ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.RESUME) def hide_event(self): if gui_app.iqpilot_ui(): ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.PAUSE) if __name__ == "__main__": gui_app.init_window("OnRoad Camera View") 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): 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()