mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-15 04:23:43 +08:00
Compare commits
4 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| a5f44653d7 | |||
| 63a2a3868e | |||
| 5484f7f4a7 | |||
| 347238b307 |
@@ -474,6 +474,7 @@ def main(demo=False):
|
||||
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, left_edge, right_edge)
|
||||
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
|
||||
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
|
||||
mdv2sp_send.valid = modelv2_send.valid
|
||||
mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
|
||||
|
||||
fill_driving_model_data(drivingdata_send, modelv2_send)
|
||||
|
||||
@@ -13,7 +13,7 @@ from openpilot.sunnypilot.models.helpers import ACTIVE_BUNDLE_KEYS, get_selected
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.ui.ui_state import device, ui_state
|
||||
from openpilot.selfdrive.ui.sunnypilot.model_info import (big_model_state, bundles_for_source, carrying_model, default_model_name,
|
||||
model_cache_size_mb, queued_name)
|
||||
model_cache_size_mb, queued_name, refresh_in_progress, refresh_model_list)
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.system.ui.widgets import DialogResult, Widget
|
||||
@@ -40,6 +40,8 @@ class ModelsLayout(Widget):
|
||||
self._downloading = False
|
||||
self._verifying = False
|
||||
self._clearing = False
|
||||
self._refreshing = False
|
||||
self._refresh_start: float | None = None
|
||||
self._last_note = None
|
||||
self.last_cache_calc_time = 0
|
||||
|
||||
@@ -67,10 +69,9 @@ class ModelsLayout(Widget):
|
||||
|
||||
self.download_item = download_status_item(lambda: tr("Download") if self._downloading else tr("Model Status"))
|
||||
|
||||
self.refresh_item = button_item(tr("Refresh Model List"), tr("REFRESH"), "",
|
||||
lambda: (ui_state.params.put("ModelManager_LastSyncTime", 0),
|
||||
ui_state.params.put("ModelManager_LastSyncTime_Chestnut", 0),
|
||||
gui_app.push_widget(alert_dialog(tr("Fetching Latest Models")))))
|
||||
self.refresh_item = button_item(tr("Refresh Model List"),
|
||||
lambda: tr("FETCHING...") if self._refreshing else tr("REFRESH"), "",
|
||||
self._refresh_models)
|
||||
|
||||
self.clear_cache_item = ListItemSP(
|
||||
title=tr("Clear Model Cache"),
|
||||
@@ -115,7 +116,7 @@ class ModelsLayout(Widget):
|
||||
if lagd_toggle:
|
||||
desc += f"<br>{tr('Live Steer Delay:')} {ui_state.sm['lateralDelay'].lateralDelay:.3f} s"
|
||||
elif ui_state.CP is not None:
|
||||
sw = float(ui_state.params.get("LagdToggleDelay", "0.2"))
|
||||
sw = float(ui_state.params.get("LagdToggleDelay", return_default=True))
|
||||
cp = ui_state.CP.steerActuatorDelay
|
||||
desc += f"<br>{tr('Actuator Delay:')} {cp:.2f} s + {tr('Software Delay:')} {sw:.2f} s = {tr('Total Delay:')} {cp + sw:.2f} s"
|
||||
self.lagd_toggle.set_description(desc)
|
||||
@@ -133,6 +134,10 @@ class ModelsLayout(Widget):
|
||||
tr("Clear Cache"), callback=_callback)
|
||||
gui_app.push_widget(dialog)
|
||||
|
||||
def _refresh_models(self):
|
||||
refresh_model_list()
|
||||
self._refresh_start = time.monotonic()
|
||||
|
||||
def _handle_bundle_download_progress(self):
|
||||
self.cancel_download_item.set_visible(False)
|
||||
self._downloading = False
|
||||
@@ -343,6 +348,10 @@ class ModelsLayout(Widget):
|
||||
# manager is offroad-only, so an onroad clear would never be serviced
|
||||
self.clear_cache_item.action_item.set_enabled(offroad and not self._downloading and not self._clearing)
|
||||
|
||||
# manager is offroad-only, so a refresh queued onroad would never be serviced
|
||||
self._refreshing = refresh_in_progress(self._refresh_start)
|
||||
self.refresh_item.action_item.set_enabled(offroad and not self._downloading and not self._refreshing)
|
||||
|
||||
def _render(self, rect):
|
||||
self._scroller.render(rect)
|
||||
|
||||
|
||||
@@ -14,7 +14,8 @@ from openpilot.sunnypilot.models.helpers import ACTIVE_BUNDLE_KEYS, get_selected
|
||||
from openpilot.selfdrive.ui.mici.widgets.button import BigButton
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, device
|
||||
from openpilot.selfdrive.ui.sunnypilot.model_info import (active_source, big_model_state, bundles_for_source, carrying_model,
|
||||
default_model_name, model_cache_size_mb, model_info, queued_name)
|
||||
default_model_name, model_cache_size_mb, model_info, queued_name,
|
||||
refresh_in_progress, refresh_model_list)
|
||||
from openpilot.system.ui.lib.application import FontWeight, gui_app
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
@@ -83,6 +84,10 @@ class ModelsLayoutMici(NavScroller):
|
||||
self.select_model_btn = BigButton(tr("select model"))
|
||||
self.select_model_btn.set_click_callback(self._show_folders)
|
||||
|
||||
self.refresh_btn = BigButton(tr("refresh models"))
|
||||
self.refresh_btn.set_click_callback(self._refresh_models)
|
||||
self._refresh_start: float | None = None
|
||||
|
||||
self.cancel_download_btn = BigButton(tr("cancel download"))
|
||||
self.cancel_download_btn.set_click_callback(lambda: ui_state.params.remove("ModelManager_DownloadRef"))
|
||||
|
||||
@@ -90,7 +95,7 @@ class ModelsLayoutMici(NavScroller):
|
||||
self.clear_cache_btn.set_click_callback(self._confirm_clear_cache)
|
||||
self._cache_size_time = 0.0
|
||||
|
||||
self.main_items = [self.current_model_info, self.select_model_btn, self.cancel_download_btn, self.clear_cache_btn]
|
||||
self.main_items = [self.current_model_info, self.select_model_btn, self.cancel_download_btn, self.refresh_btn, self.clear_cache_btn]
|
||||
self._scroller.add_widgets(self.main_items)
|
||||
|
||||
@property
|
||||
@@ -173,6 +178,10 @@ class ModelsLayoutMici(NavScroller):
|
||||
gui_app.push_widget(BigConfirmationDialog(f"{tr('slide to')}\n{tr('clear cache')}", icon,
|
||||
lambda: ui_state.params.put_bool("ModelManager_ClearCache", True), red=True))
|
||||
|
||||
def _refresh_models(self):
|
||||
refresh_model_list()
|
||||
self._refresh_start = time.monotonic()
|
||||
|
||||
def _select_folder(self, folder_name):
|
||||
source = self._selection_source
|
||||
if source is None: # folders are only reachable after picking a hardware
|
||||
@@ -226,6 +235,11 @@ class ModelsLayoutMici(NavScroller):
|
||||
self._cache_size_time = now
|
||||
self.clear_cache_btn.set_value(f"{model_cache_size_mb():.1f} MB")
|
||||
|
||||
# manager is offroad-only, so a refresh queued onroad would never be serviced
|
||||
refreshing = refresh_in_progress(self._refresh_start)
|
||||
self.refresh_btn.set_enabled(ui_state.is_offroad() and not is_downloading and not refreshing)
|
||||
self.refresh_btn.set_value(tr("fetching...") if refreshing else "")
|
||||
|
||||
self.current_model_info.current_model_header.set_text(tr("active model"))
|
||||
active_text, info_header, info_text = _model_info()
|
||||
self.current_model_info.current_model_text.set_text(active_text)
|
||||
|
||||
@@ -6,6 +6,7 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import contextlib
|
||||
import os
|
||||
import time
|
||||
|
||||
from openpilot.common.hardware.hw import Paths
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, ChestnutState
|
||||
@@ -98,3 +99,22 @@ def model_info() -> tuple[str, str, str]:
|
||||
active_name = active_bundle.displayName if active_bundle else default_model_name(source)
|
||||
other_name = other_bundle.displayName if other_bundle else default_model_name(other)
|
||||
return source, active_name, other_name
|
||||
|
||||
|
||||
# mirrors the manager's ModelCache keys; the manager restamps them on a successful fetch
|
||||
MODEL_SYNC_KEYS = ("ModelManager_LastSyncTime", "ModelManager_LastSyncTime_Chestnut")
|
||||
MODEL_SYNC_TIMEOUT = 20.0
|
||||
|
||||
|
||||
def refresh_model_list() -> None:
|
||||
# zeroing the sync keys makes the manager refetch each manifest on its next tick
|
||||
for key in MODEL_SYNC_KEYS:
|
||||
ui_state.params.put(key, 0)
|
||||
|
||||
|
||||
def refresh_in_progress(started_at: float | None) -> bool:
|
||||
"""Whether a user refresh is still outstanding. A failed fetch never restamps the
|
||||
sync keys, so the spinner is bounded by MODEL_SYNC_TIMEOUT rather than sticking."""
|
||||
if started_at is None or time.monotonic() - started_at > MODEL_SYNC_TIMEOUT:
|
||||
return False
|
||||
return not all(ui_state.params.get(key) for key in MODEL_SYNC_KEYS)
|
||||
|
||||
@@ -15,11 +15,19 @@ class CameraOffsetHelper:
|
||||
self.actual_camera_offset = 0.0
|
||||
|
||||
@staticmethod
|
||||
def apply_camera_offset(model_transform, intrinsics, height, offset_param):
|
||||
def get_v_horizon(intrinsics, rpy_calib):
|
||||
cy = intrinsics[1, 2]
|
||||
if len(rpy_calib) == 3 and np.isfinite(rpy_calib).all():
|
||||
fy = intrinsics[1, 1]
|
||||
pitch = rpy_calib[1]
|
||||
return float(cy - fy * np.tan(pitch))
|
||||
return float(cy)
|
||||
|
||||
@staticmethod
|
||||
def apply_camera_offset(model_transform, height, offset_param, v_horizon):
|
||||
shear = np.eye(3, dtype=np.float32)
|
||||
shear[0, 1] = offset_param / height
|
||||
shear[0, 2] = -offset_param / height * cy
|
||||
shear[0, 2] = -offset_param / height * v_horizon
|
||||
model_transform = (shear @ model_transform).astype(np.float32)
|
||||
return model_transform
|
||||
|
||||
@@ -30,10 +38,13 @@ class CameraOffsetHelper:
|
||||
self.actual_camera_offset = (0.9 * self.actual_camera_offset) + (0.1 * self.camera_offset)
|
||||
dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))]
|
||||
height = sm["extrinsicsCalibration"].height[0] if sm['extrinsicsCalibration'].height else 1.22
|
||||
rpy_calib = sm['extrinsicsCalibration'].rpyCalib
|
||||
|
||||
intrinsics_main = dc.wide_road.intrinsics if main_wide_camera else dc.narrow_road.intrinsics
|
||||
model_transform_main = self.apply_camera_offset(model_transform_main, intrinsics_main, height, self.actual_camera_offset)
|
||||
v_horizon_main = self.get_v_horizon(intrinsics_main, rpy_calib)
|
||||
model_transform_main = self.apply_camera_offset(model_transform_main, height, self.actual_camera_offset, v_horizon_main)
|
||||
|
||||
intrinsics_extra = dc.wide_road.intrinsics
|
||||
model_transform_extra = self.apply_camera_offset(model_transform_extra, intrinsics_extra, height, self.actual_camera_offset)
|
||||
v_horizon_extra = self.get_v_horizon(intrinsics_extra, rpy_calib)
|
||||
model_transform_extra = self.apply_camera_offset(model_transform_extra, height, self.actual_camera_offset, v_horizon_extra)
|
||||
return model_transform_main, model_transform_extra
|
||||
|
||||
@@ -552,6 +552,7 @@ def main(demo=False):
|
||||
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, left_edge, right_edge)
|
||||
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
|
||||
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
|
||||
mdv2sp_send.valid = modelv2_send.valid
|
||||
mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction
|
||||
drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
|
||||
drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
|
||||
|
||||
@@ -6,8 +6,9 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
from openpilot.common.transformations.camera import DEVICE_CAMERAS, view_frame_from_device_frame
|
||||
from openpilot.common.transformations.model import get_warp_matrix
|
||||
from openpilot.common.transformations.orientation import rot_from_euler
|
||||
from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
|
||||
@@ -46,29 +47,50 @@ class TestCameraOffset(OpenpilotTestCase):
|
||||
self.camera_offset.update(main_transform, extra_transform, sm, False)
|
||||
np.testing.assert_almost_equal(self.camera_offset.actual_camera_offset, 0.038)
|
||||
|
||||
def test_camera_offset_(self):
|
||||
def test_apply_camera_offset(self):
|
||||
intrinsics = self.dc.narrow_road.intrinsics
|
||||
v_horizon = CameraOffsetHelper.get_v_horizon(intrinsics, []) # pitch = 0 fallback: v_horizon == cy
|
||||
transform = np.eye(3, dtype=np.float32)
|
||||
height = 1.22
|
||||
offset = 0.1
|
||||
|
||||
cy = intrinsics[1, 2]
|
||||
expected_shear = np.eye(3, dtype=np.float32)
|
||||
expected_shear[0, 1] = offset / height
|
||||
expected_shear[0, 2] = -offset / height * cy
|
||||
expected_shear[0, 2] = -offset / height * v_horizon
|
||||
|
||||
result = CameraOffsetHelper.apply_camera_offset(transform, intrinsics, height, offset)
|
||||
result = CameraOffsetHelper.apply_camera_offset(transform, height, offset, v_horizon)
|
||||
np.testing.assert_array_almost_equal(result, expected_shear)
|
||||
|
||||
def test_v_horizon_empty_rpy(self):
|
||||
intrinsics = self.dc.narrow_road.intrinsics
|
||||
v_horizon = CameraOffsetHelper.get_v_horizon(intrinsics, [])
|
||||
np.testing.assert_almost_equal(v_horizon, intrinsics[1, 2])
|
||||
|
||||
def test_v_horizon_projection(self):
|
||||
intrinsics = self.dc.narrow_road.intrinsics
|
||||
f, cy = intrinsics[1, 1], intrinsics[1, 2]
|
||||
|
||||
for pitch_deg in [6.0, -6.0, 0.0]:
|
||||
rpy = [0.0, np.radians(pitch_deg), 0.0]
|
||||
d_dev = rot_from_euler(rpy) @ np.array([1.0, 0.0, 0.0])
|
||||
view = view_frame_from_device_frame @ d_dev
|
||||
expected = cy + f * view[1] / view[2]
|
||||
|
||||
v_horizon = CameraOffsetHelper.get_v_horizon(intrinsics, rpy)
|
||||
np.testing.assert_almost_equal(v_horizon, expected, decimal=4)
|
||||
|
||||
def test_update(self):
|
||||
height = 1.2
|
||||
pitch = np.radians(-8.0)
|
||||
|
||||
sm = MockStruct(
|
||||
deviceState=MockStruct(deviceType='mici'),
|
||||
narrowRoadCameraState=MockStruct(sensor='os04c10'),
|
||||
extrinsicsCalibration=MockStruct(rpyCalib=[0.0, 0.0, 0.0], height=[1.22])
|
||||
extrinsicsCalibration=MockStruct(rpyCalib=[0.0, pitch, 0.0], height=[height])
|
||||
)
|
||||
intrinsics_main = self.dc.narrow_road.intrinsics
|
||||
intrinsics_extra = self.dc.wide_road.intrinsics
|
||||
device_from_calib_euler = np.array([0.0, 0.0, 0.0], dtype=np.float32)
|
||||
device_from_calib_euler = np.array(sm['extrinsicsCalibration'].rpyCalib, dtype=np.float32)
|
||||
main_transform = get_warp_matrix(device_from_calib_euler, intrinsics_main, False).astype(np.float32)
|
||||
extra_transform = get_warp_matrix(device_from_calib_euler, intrinsics_extra, True).astype(np.float32)
|
||||
|
||||
@@ -81,5 +103,13 @@ class TestCameraOffset(OpenpilotTestCase):
|
||||
main_out, extra_out = self.camera_offset.update(main_transform, extra_transform, sm, False)
|
||||
assert not np.array_equal(main_out, main_transform)
|
||||
assert not np.array_equal(extra_out, extra_transform)
|
||||
assert main_out[0, 1] != 0.0
|
||||
assert main_out[0, 2] != 0.0
|
||||
|
||||
# settle the low-pass filter
|
||||
for _ in range(100):
|
||||
main_out, extra_out = self.camera_offset.update(main_transform, extra_transform, sm, False)
|
||||
|
||||
# undo main_transform dot product to get shear matrix
|
||||
shear = main_out @ np.linalg.inv(main_transform)
|
||||
expected_v_horizon = intrinsics_main[1, 2] - intrinsics_main[1, 1] * np.tan(pitch)
|
||||
np.testing.assert_almost_equal(shear[0, 1], self.camera_offset.actual_camera_offset / height, decimal=4)
|
||||
np.testing.assert_almost_equal(shear[0, 2], -self.camera_offset.actual_camera_offset / height * expected_v_horizon, decimal=4)
|
||||
|
||||
Reference in New Issue
Block a user