mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-15 02:13:43 +08:00
Compare commits
6 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| a5f44653d7 | |||
| 63a2a3868e | |||
| 5484f7f4a7 | |||
| 347238b307 | |||
| c57f9a7f4e | |||
| 3c24eeea25 |
@@ -1,43 +0,0 @@
|
||||
exclude-labels:
|
||||
- 'no-changelog'
|
||||
categories:
|
||||
- title: '🚀 Features'
|
||||
labels:
|
||||
- 'feature'
|
||||
- 'enhancement'
|
||||
- title: '🐛 Bug Fixes'
|
||||
collapse-after: 5
|
||||
labels:
|
||||
- 'fix'
|
||||
- 'bugfix'
|
||||
- 'bug'
|
||||
- title: '🧰 Maintenance'
|
||||
collapse-after: 5
|
||||
label: 'chore'
|
||||
change-template: '- $TITLE @$AUTHOR (#$NUMBER)'
|
||||
change-title-escapes: '\<*_&'
|
||||
replacers:
|
||||
- search: '/[Ss][Uu][Nn][Nn][Yy][Pp][Ii][Ll][Oo][Tt]/g'
|
||||
replace: 'sunnypilot'
|
||||
- search: '/\b[Ss][Pp]\b/g'
|
||||
replace: 'SP'
|
||||
version-resolver:
|
||||
major:
|
||||
labels:
|
||||
- 'major'
|
||||
minor:
|
||||
labels:
|
||||
- 'minor'
|
||||
patch:
|
||||
labels:
|
||||
- 'patch'
|
||||
default: patch
|
||||
name-template: 'v$RESOLVED_VERSION 🚀'
|
||||
tag-template: 'v$RESOLVED_VERSION'
|
||||
version-template: "0.$MAJOR.$MINOR.$PATCH" # The day OP becomes v1, we need to bump this
|
||||
tag-prefix: "v0." # The day OP becomes v1, we need to bump this
|
||||
prerelease-identifier: "staging"
|
||||
template: |
|
||||
## Changes
|
||||
|
||||
$CHANGES
|
||||
@@ -20,6 +20,11 @@ on:
|
||||
required: false
|
||||
type: string
|
||||
default: 'sunnypilot/sunnypilot_models_v1'
|
||||
docs_repo:
|
||||
description: 'GitHub repo holding the driving_models JSON on its gh-pages branch'
|
||||
required: false
|
||||
type: string
|
||||
default: 'sunnypilot/sunnypilot-models'
|
||||
|
||||
jobs:
|
||||
setup:
|
||||
@@ -34,7 +39,6 @@ jobs:
|
||||
- name: Checkout sunnypilot repo
|
||||
uses: actions/checkout@v4
|
||||
with:
|
||||
repository: sunnypilot/sunnypilot
|
||||
path: sunnypilot
|
||||
submodules: recursive
|
||||
|
||||
@@ -47,10 +51,10 @@ jobs:
|
||||
echo "tinygrad_ref=$ref" >> $GITHUB_OUTPUT
|
||||
echo "tinygrad_ref is $ref"
|
||||
|
||||
- name: Checkout docs repo (sunnypilot-models, gh-pages)
|
||||
- name: Checkout docs repo (gh-pages)
|
||||
uses: actions/checkout@v4
|
||||
with:
|
||||
repository: sunnypilot/sunnypilot-models
|
||||
repository: ${{ inputs.docs_repo }}
|
||||
ref: gh-pages
|
||||
path: docs
|
||||
ssh-key: ${{ secrets.CI_SUNNYPILOT_DOCS_PRIVATE_KEY }}
|
||||
@@ -118,6 +122,7 @@ jobs:
|
||||
json_version: ${{ needs.setup.outputs.json_version }}
|
||||
target_hardware: ${{ github.event.inputs.target_hardware }}
|
||||
hf_repo: ${{ github.event.inputs.hf_repo }}
|
||||
docs_repo: ${{ inputs.docs_repo }}
|
||||
set_min_version: ${{ github.event.inputs.set_min_version }}
|
||||
tinygrad_ref: ${{ needs.setup.outputs.tinygrad_ref }}
|
||||
secrets: inherit
|
||||
@@ -162,6 +167,7 @@ jobs:
|
||||
target_hardware: ${{ github.event.inputs.target_hardware }}
|
||||
artifact_suffix: -retry
|
||||
hf_repo: ${{ github.event.inputs.hf_repo }}
|
||||
docs_repo: ${{ inputs.docs_repo }}
|
||||
set_min_version: ${{ github.event.inputs.set_min_version }}
|
||||
tinygrad_ref: ${{ needs.setup.outputs.tinygrad_ref }}
|
||||
secrets: inherit
|
||||
|
||||
@@ -39,6 +39,11 @@ on:
|
||||
required: false
|
||||
type: string
|
||||
default: 'sunnypilot/sunnypilot_models_v1'
|
||||
docs_repo:
|
||||
description: 'GitHub repo holding the driving_models JSON on its gh-pages branch'
|
||||
required: false
|
||||
type: string
|
||||
default: 'sunnypilot/sunnypilot-models'
|
||||
set_min_version:
|
||||
description: 'Minimum selector version'
|
||||
required: false
|
||||
@@ -107,6 +112,11 @@ on:
|
||||
required: false
|
||||
type: string
|
||||
default: 'sunnypilot/sunnypilot_models_v1'
|
||||
docs_repo:
|
||||
description: 'GitHub repo holding the driving_models JSON on its gh-pages branch'
|
||||
required: false
|
||||
type: string
|
||||
default: 'sunnypilot/sunnypilot-models'
|
||||
env:
|
||||
RECOMPILED_DIR: recompiled${{ inputs.recompiled_dir }}
|
||||
JSON_FILE: docs/docs/driving_models_${{ inputs.target_hardware == 'chestnut' && 'chestnut_v' || 'v' }}${{ inputs.json_version }}.json
|
||||
@@ -136,7 +146,7 @@ jobs:
|
||||
- name: Checkout docs repo
|
||||
uses: actions/checkout@v4
|
||||
with:
|
||||
repository: sunnypilot/sunnypilot-models
|
||||
repository: ${{ inputs.docs_repo }}
|
||||
ref: gh-pages
|
||||
path: docs
|
||||
ssh-key: ${{ secrets.CI_SUNNYPILOT_DOCS_PRIVATE_KEY }}
|
||||
|
||||
@@ -1,28 +0,0 @@
|
||||
name: Release Drafter
|
||||
|
||||
on:
|
||||
push:
|
||||
branches:
|
||||
- master
|
||||
tags:
|
||||
- 'v*'
|
||||
pull_request_target:
|
||||
types: [opened, reopened, synchronize]
|
||||
workflow_dispatch:
|
||||
|
||||
permissions:
|
||||
contents: read
|
||||
|
||||
jobs:
|
||||
update_release_draft:
|
||||
permissions:
|
||||
contents: write
|
||||
pull-requests: write
|
||||
runs-on: ubuntu-latest
|
||||
steps:
|
||||
- uses: release-drafter/release-drafter@v6
|
||||
with:
|
||||
config-name: release-drafter.yml
|
||||
prerelease: ${{ !startsWith(github.ref, 'refs/tags/v') }}
|
||||
env:
|
||||
GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
|
||||
@@ -1,78 +0,0 @@
|
||||
name: Debug Discourse Posting
|
||||
|
||||
on:
|
||||
push:
|
||||
|
||||
jobs:
|
||||
test-discourse-post:
|
||||
runs-on: ubuntu-latest
|
||||
steps:
|
||||
- uses: actions/checkout@v4
|
||||
|
||||
- name: Post test message to Discourse
|
||||
uses: ./.github/workflows/post-to-discourse
|
||||
with:
|
||||
discourse-url: ${{ vars.DISCOURSE_URL }}
|
||||
api-key: ${{ secrets.DISCOURSE_API_KEY }}
|
||||
api-username: ${{ secrets.DISCOURSE_API_USERNAME }}
|
||||
topic-id: ${{ vars.DISCOURSE_UPDATES_TOPIC_ID }}
|
||||
message: |
|
||||
## 🧪 Test Post from GitHub Actions
|
||||
|
||||
**This is a test post to verify Discourse integration**
|
||||
|
||||
- **Workflow**: ${{ github.workflow }}
|
||||
- **Run Number**: #${{ github.run_number }}
|
||||
- **Branch**: `${{ github.ref_name }}`
|
||||
- **Commit**: ${{ github.sha }}
|
||||
- **Actor**: @${{ github.actor }}
|
||||
- **Timestamp**: ${{ github.event.head_commit.timestamp }}
|
||||
|
||||
---
|
||||
|
||||
### Fake Build Info (for testing)
|
||||
- **Version**: 0.9.8-test
|
||||
- **Build**: #42
|
||||
- **Branch**: release-test
|
||||
|
||||
[View workflow run](${{ github.server_url }}/${{ github.repository }}/actions/runs/${{ github.run_id }})
|
||||
|
||||
*This is an automated test message. Drive safe! 🚗💨*
|
||||
|
||||
|
||||
- name: Create topic on Discourse
|
||||
uses: ./.github/workflows/post-to-discourse
|
||||
with:
|
||||
discourse-url: ${{ vars.DISCOURSE_URL }}
|
||||
api-key: ${{ secrets.DISCOURSE_API_KEY }}
|
||||
api-username: ${{ secrets.DISCOURSE_API_USERNAME }}
|
||||
#topic-id: ${{ vars.DISCOURSE_UPDATES_TOPIC_ID }}
|
||||
category-id: 4
|
||||
title: "This is a test of a new topic instead of a reply"
|
||||
message: |
|
||||
## 🧪 Test Post from GitHub Actions
|
||||
|
||||
**This is a test post to verify Discourse integration**
|
||||
|
||||
- **Workflow**: ${{ github.workflow }}
|
||||
- **Run Number**: #${{ github.run_number }}
|
||||
- **Branch**: `${{ github.ref_name }}`
|
||||
- **Commit**: ${{ github.sha }}
|
||||
- **Actor**: @${{ github.actor }}
|
||||
- **Timestamp**: ${{ github.event.head_commit.timestamp }}
|
||||
|
||||
---
|
||||
|
||||
### Fake Build Info (for testing)
|
||||
- **Version**: 0.9.8-test
|
||||
- **Build**: #42
|
||||
- **Branch**: release-test
|
||||
|
||||
[View workflow run](${{ github.server_url }}/${{ github.repository }}/actions/runs/${{ github.run_id }})
|
||||
|
||||
*This is an automated test message. Drive safe! 🚗💨*
|
||||
- name: Display results
|
||||
if: always()
|
||||
run: |
|
||||
echo "::notice::Discourse post test completed"
|
||||
echo "Check your Discourse topic to verify the post appeared correctly"
|
||||
@@ -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