forked from IQ.Lvbs/IQ.Pilot
IQ.Pilot Release Commit @ 0798119
This commit is contained in:
IQ.Lvbs history cleanup
492
selfdrive/ui/mici/onroad/augmented_road_view.py
Normal file
492
selfdrive/ui/mici/onroad/augmented_road_view.py
Normal file
@@ -0,0 +1,492 @@
|
||||
"""
|
||||
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()
|
||||
Reference in New Issue
Block a user