diff --git a/common/logging_extra.py b/common/logging_extra.py index e921b140c2..ceaf083cd5 100644 --- a/common/logging_extra.py +++ b/common/logging_extra.py @@ -199,7 +199,6 @@ class SwagLogger(logging.Logger): co = f.f_code filename = os.path.normcase(co.co_filename) - # TODO: is this pylint exception correct? if filename == _srcfile: f = f.f_back continue diff --git a/opendbc_repo b/opendbc_repo index 3d1a10424b..f66c6e6667 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 3d1a10424b17a4d74c597db39b03e34f72bc9402 +Subproject commit f66c6e66672196b3edfb82f3ad1306de17000b21 diff --git a/panda b/panda index 5f4742c39e..86cf5dc583 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 5f4742c39e9fc3690c4318ebf2ee8f3b83bfc8a9 +Subproject commit 86cf5dc5838628f9af5b5dac66b5f39b37e2b499 diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index 4e207b4882..e070c16b1c 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -157,7 +157,8 @@ class LateralLagEstimator: block_count: int = BLOCK_NUM, min_valid_block_count: int = BLOCK_NUM_NEEDED, block_size: int = BLOCK_SIZE, window_sec: float = MOVING_WINDOW_SEC, okay_window_sec: float = MIN_OKAY_WINDOW_SEC, min_recovery_buffer_sec: float = MIN_RECOVERY_BUFFER_SEC, min_vego: float = MIN_VEGO, min_yr: float = MIN_ABS_YAW_RATE, min_ncc: float = MIN_NCC, - max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE): + max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE, + enabled: bool = True): self.dt = dt self.window_sec = window_sec self.okay_window_sec = okay_window_sec @@ -172,6 +173,7 @@ class LateralLagEstimator: self.min_confidence = min_confidence self.max_lat_accel = max_lat_accel self.max_lat_accel_diff = max_lat_accel_diff + self.enabled = enabled self.t = 0.0 self.lat_active = False @@ -206,7 +208,7 @@ class LateralLagEstimator: liveDelay = msg.liveDelay valid_mean_lag, valid_std, current_mean_lag, current_std = self.block_avg.get() - if self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std): + if self.enabled and self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std): if valid_std > MAX_LAG_STD: liveDelay.status = log.LiveDelayData.Status.invalid else: @@ -303,7 +305,8 @@ class LateralLagEstimator: self.block_avg.update(delay) self.last_estimate_t = self.t - def actuator_delay(self, expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray, dt: float, max_lag: float) -> tuple[float, float, float]: + @staticmethod + def actuator_delay(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray, dt: float, max_lag: float) -> tuple[float, float, float]: assert len(expected_sig) == len(actual_sig) max_lag_samples = int(max_lag / dt) padded_size = fft_next_good_size(len(expected_sig) + max_lag_samples) @@ -366,7 +369,10 @@ def main(): params = Params() CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) - lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency) + # TODO: remove me, lagd is in shadow mode on release + is_release = params.get_bool("IsReleaseBranch") + + lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency, enabled=not is_release) if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None: lag, valid_blocks = initial_lag_params lag_learner.reset(lag, valid_blocks) diff --git a/selfdrive/locationd/test/test_lagd.py b/selfdrive/locationd/test/test_lagd.py index b805f1759d..a7f9c75ab4 100644 --- a/selfdrive/locationd/test/test_lagd.py +++ b/selfdrive/locationd/test/test_lagd.py @@ -3,7 +3,7 @@ import numpy as np import time import pytest -from cereal import messaging, log +from cereal import messaging, log, car from openpilot.selfdrive.locationd.lagd import LateralLagEstimator, retrieve_initial_lag, masked_normalized_cross_correlation, \ BLOCK_NUM_NEEDED, BLOCK_SIZE, MIN_OKAY_WINDOW_SEC from openpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams @@ -16,11 +16,7 @@ MAX_ERR_FRAMES = 1 DT = 0.05 -def process_messages(mocker, estimator, lag_frames, n_frames, vego=20.0, rejection_threshold=0.0): - class ZeroMock(mocker.Mock): - def __getattr__(self, *args): - return 0 - +def process_messages(estimator, lag_frames, n_frames, vego=20.0, rejection_threshold=0.0): for i in range(n_frames): t = i * estimator.dt desired_la = np.cos(10 * t) * 0.1 @@ -31,19 +27,15 @@ def process_messages(mocker, estimator, lag_frames, n_frames, vego=20.0, rejecti if rejected: actual_la = desired_la - desired_cuvature = desired_la / (vego ** 2) - actual_yr = actual_la / vego + desired_cuvature = float(desired_la / (vego ** 2)) + actual_yr = float(actual_la / vego) msgs = [ - (t, "carControl", mocker.Mock(latActive=not rejected)), - (t, "carState", mocker.Mock(vEgo=vego, steeringPressed=False)), - (t, "controlsState", mocker.Mock(desiredCurvature=desired_cuvature, - lateralControlState=mocker.Mock(which=mocker.Mock(return_value='debugControlState'), debugControlState=ZeroMock()))), - (t, "livePose", mocker.Mock(orientationNED=ZeroMock(), - velocityDevice=ZeroMock(), - accelerationDevice=ZeroMock(), - angularVelocityDevice=ZeroMock(z=actual_yr, valid=True), - posenetOK=True, inputsOK=True)), - (t, "liveCalibration", mocker.Mock(rpyCalib=[0, 0, 0], calStatus=log.LiveCalibrationData.Status.calibrated)), + (t, "carControl", car.CarControl(latActive=not rejected)), + (t, "carState", car.CarState(vEgo=vego, steeringPressed=False)), + (t, "controlsState", log.ControlsState(desiredCurvature=desired_cuvature)), + (t, "livePose", log.LivePose(angularVelocityDevice=log.LivePose.XYZMeasurement(z=actual_yr, valid=True), + posenetOK=True, inputsOK=True)), + (t, "liveCalibration", log.LiveCalibrationData(rpyCalib=[0, 0, 0], calStatus=log.LiveCalibrationData.Status.calibrated)), ] for t, w, m in msgs: estimator.handle_log(t, w, m) @@ -94,8 +86,8 @@ class TestLagd: corr = masked_normalized_cross_correlation(desired_sig, actual_sig, mask, 200)[len(desired_sig) - 1:len(desired_sig) + 20] assert np.argmax(corr) in range(lag_frames - MAX_ERR_FRAMES, lag_frames + MAX_ERR_FRAMES + 1) - def test_empty_estimator(self, mocker): - mocked_CP = mocker.Mock(steerActuatorDelay=0.8) + def test_empty_estimator(self): + mocked_CP = car.CarParams(steerActuatorDelay=0.8) estimator = LateralLagEstimator(mocked_CP, DT) msg = estimator.get_msg(True) assert msg.liveDelay.status == 'unestimated' @@ -103,12 +95,12 @@ class TestLagd: assert np.allclose(msg.liveDelay.lateralDelayEstimate, estimator.initial_lag) assert msg.liveDelay.validBlocks == 0 - def test_estimator_basics(self, mocker, subtests): + def test_estimator_basics(self, subtests): for lag_frames in range(5): with subtests.test(msg=f"lag_frames={lag_frames}"): - mocked_CP = mocker.Mock(steerActuatorDelay=0.8) + mocked_CP = car.CarParams(steerActuatorDelay=0.8) estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0) - process_messages(mocker, estimator, lag_frames, int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_NUM_NEEDED * BLOCK_SIZE) + process_messages(estimator, lag_frames, int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_NUM_NEEDED * BLOCK_SIZE) msg = estimator.get_msg(True) assert msg.liveDelay.status == 'estimated' assert np.allclose(msg.liveDelay.lateralDelay, lag_frames * DT, atol=0.01) @@ -116,18 +108,30 @@ class TestLagd: assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01) assert msg.liveDelay.validBlocks == BLOCK_NUM_NEEDED - def test_estimator_masking(self, mocker): - mocked_CP, lag_frames = mocker.Mock(steerActuatorDelay=0.8), random.randint(1, 19) + def test_disabled_estimator(self): + mocked_CP = car.CarParams(steerActuatorDelay=0.8) + estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0, enabled=False) + lag_frames = 5 + process_messages(estimator, lag_frames, int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_NUM_NEEDED * BLOCK_SIZE) + msg = estimator.get_msg(True) + assert msg.liveDelay.status == 'unestimated' + assert np.allclose(msg.liveDelay.lateralDelay, 1.0, atol=0.01) + assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01) + assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01) + assert msg.liveDelay.validBlocks == BLOCK_NUM_NEEDED + + def test_estimator_masking(self): + mocked_CP, lag_frames = car.CarParams(steerActuatorDelay=0.8), random.randint(1, 19) estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0, min_valid_block_count=1) - process_messages(mocker, estimator, lag_frames, (int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_SIZE) * 2, rejection_threshold=0.4) + process_messages(estimator, lag_frames, (int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_SIZE) * 2, rejection_threshold=0.4) msg = estimator.get_msg(True) assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01) assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01) @pytest.mark.skipif(PC, reason="only on device") @pytest.mark.timeout(60) - def test_estimator_performance(self, mocker): - mocked_CP = mocker.Mock(steerActuatorDelay=0.8) + def test_estimator_performance(self): + mocked_CP = car.CarParams(steerActuatorDelay=0.8) estimator = LateralLagEstimator(mocked_CP, DT) ds = [] diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 0578a61588..201baf6b0e 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -15,15 +15,15 @@ from openpilot.system.hardware import PC from openpilot.tools.lib.openpilotci import get_url from openpilot.selfdrive.test.process_replay.compare_logs import compare_logs, format_diff from openpilot.selfdrive.test.process_replay.process_replay import get_process_config, replay_process -from openpilot.tools.lib.framereader import FrameReader, NumpyFrameReader +from openpilot.tools.lib.framereader import FrameReader from openpilot.tools.lib.logreader import LogReader, save_log from openpilot.tools.lib.github_utils import GithubUtils TEST_ROUTE = "8494c69d3c710e81|000001d4--2648a9a404" SEGMENT = 4 -MAX_FRAMES = 100 if PC else 400 +START_FRAME = 0 +END_FRAME = 60 -NO_MODEL = "NO_MODEL" in os.environ SEND_EXTRA_INPUTS = bool(int(os.getenv("SEND_EXTRA_INPUTS", "0"))) DATA_TOKEN = os.getenv("CI_ARTIFACTS_TOKEN","") @@ -125,16 +125,15 @@ def comment_replay_report(proposed, master, full_logs): comment = f"ref for commit {commit}: {link}/{log_name}" + diff_plots + all_plots GITHUB.comment_on_pr(comment, PR_BRANCH, "commaci-public", True) -def trim_logs_to_max_frames(logs, max_frames, frs_types, include_all_types): +def trim_logs(logs, start_frame, end_frame, frs_types, include_all_types): all_msgs = [] cam_state_counts = defaultdict(int) - # keep adding messages until cam states are equal to MAX_FRAMES for msg in sorted(logs, key=lambda m: m.logMonoTime): - all_msgs.append(msg) if msg.which() in frs_types: cam_state_counts[msg.which()] += 1 - - if all(cam_state_counts[state] == max_frames for state in frs_types): + if any(cam_state_counts[state] >= start_frame for state in frs_types): + all_msgs.append(msg) + if all(cam_state_counts[state] == end_frame for state in frs_types): break if len(include_all_types) != 0: @@ -146,9 +145,9 @@ def trim_logs_to_max_frames(logs, max_frames, frs_types, include_all_types): def model_replay(lr, frs): # modeld is using frame pairs - modeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"roadCameraState", "wideRoadCameraState"}, - {"roadEncodeIdx", "wideRoadEncodeIdx", "carParams", "carState", "carControl"}) - dmodeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"driverCameraState"}, {"driverEncodeIdx", "carParams"}) + modeld_logs = trim_logs(lr, START_FRAME, END_FRAME, {"roadCameraState", "wideRoadCameraState"}, + {"roadEncodeIdx", "wideRoadEncodeIdx", "carParams", "carState", "carControl", "can"}) + dmodeld_logs = trim_logs(lr, START_FRAME, END_FRAME, {"driverCameraState"}, {"driverEncodeIdx", "carParams", "can"}) if not SEND_EXTRA_INPUTS: modeld_logs = [msg for msg in modeld_logs if msg.which() != 'liveCalibration'] @@ -165,9 +164,6 @@ def model_replay(lr, frs): dmonitoringmodeld = get_process_config("dmonitoringmodeld") modeld_msgs = replay_process(modeld, modeld_logs, frs) - if isinstance(frs['roadCameraState'], NumpyFrameReader): - del frs['roadCameraState'].frames - del frs['wideRoadCameraState'].frames dmonitoringmodeld_msgs = replay_process(dmonitoringmodeld, dmodeld_logs, frs) msgs = modeld_msgs + dmonitoringmodeld_msgs @@ -198,42 +194,21 @@ def model_replay(lr, frs): return msgs -def get_frames(): - regen_cache = "--regen-cache" in sys.argv - cache = "--cache" in sys.argv or not PC or regen_cache - videos = ('fcamera.hevc', 'dcamera.hevc', 'ecamera.hevc') - cams = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState') - - if cache: - frames_cache = '/tmp/model_replay_cache' if PC else '/data/model_replay_cache' - os.makedirs(frames_cache, exist_ok=True) - - cache_size = 200 - for v in videos: - if not all(os.path.isfile(f'{frames_cache}/{TEST_ROUTE}_{v}_{i}.npy') for i in range(MAX_FRAMES//cache_size)) or regen_cache: - f = FrameReader(get_url(TEST_ROUTE, SEGMENT, v)).get(0, MAX_FRAMES + 1, pix_fmt="nv12") - print(f'Caching {v}...') - for i in range(MAX_FRAMES//cache_size): - np.save(f'{frames_cache}/{TEST_ROUTE}_{v}_{i}', f[(i * cache_size) + 1:((i + 1) * cache_size) + 1]) - del f - - return {c : NumpyFrameReader(f"{frames_cache}/{TEST_ROUTE}_{v}", 1928, 1208, cache_size) for c,v in zip(cams, videos, strict=True)} - else: - return {c : FrameReader(get_url(TEST_ROUTE, SEGMENT, v), readahead=True) for c,v in zip(cams, videos, strict=True)} - - if __name__ == "__main__": update = "--update" in sys.argv or (os.getenv("GIT_BRANCH", "") == 'master') replay_dir = os.path.dirname(os.path.abspath(__file__)) # load logs lr = list(LogReader(get_url(TEST_ROUTE, SEGMENT, "rlog.zst"))) - frs = get_frames() + frs = { + 'roadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, "fcamera.hevc"), readahead=True), + 'driverCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, "dcamera.hevc"), readahead=True), + 'wideRoadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, "ecamera.hevc"), readahead=True) + } log_msgs = [] # run replays - if not NO_MODEL: - log_msgs += model_replay(lr, frs) + log_msgs += model_replay(lr, frs) # get diff failed = False @@ -242,13 +217,10 @@ if __name__ == "__main__": try: all_logs = list(LogReader(GITHUB.get_file_url(MODEL_REPLAY_BUCKET, log_fn))) cmp_log = [] - - # logs are ordered based on type: modelV2, drivingModelData, driverStateV2 - if not NO_MODEL: - model_start_index = next(i for i, m in enumerate(all_logs) if m.which() in ("modelV2", "drivingModelData", "cameraOdometry")) - cmp_log += all_logs[model_start_index:model_start_index + MAX_FRAMES*3] - dmon_start_index = next(i for i, m in enumerate(all_logs) if m.which() == "driverStateV2") - cmp_log += all_logs[dmon_start_index:dmon_start_index + MAX_FRAMES] + model_start_index = next(i for i, m in enumerate(all_logs) if m.which() in ("modelV2", "drivingModelData", "cameraOdometry")) + cmp_log += all_logs[model_start_index+START_FRAME*3:model_start_index + END_FRAME*3] + dmon_start_index = next(i for i, m in enumerate(all_logs) if m.which() == "driverStateV2") + cmp_log += all_logs[dmon_start_index+START_FRAME:dmon_start_index + END_FRAME] ignore = [ 'logMonoTime', diff --git a/selfdrive/ui/layouts/__init__.py b/selfdrive/ui/layouts/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/ui/layouts/home.py b/selfdrive/ui/layouts/home.py new file mode 100644 index 0000000000..56df253b52 --- /dev/null +++ b/selfdrive/ui/layouts/home.py @@ -0,0 +1,17 @@ +import pyray as rl +from openpilot.system.ui.lib.label import gui_text_box + + +class HomeLayout: + def __init__(self): + pass + + def render(self, rect: rl.Rectangle): + gui_text_box( + rect, + "Demo Home Layout", + font_size=170, + color=rl.WHITE, + alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER, + alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE, + ) diff --git a/selfdrive/ui/layouts/main.py b/selfdrive/ui/layouts/main.py new file mode 100644 index 0000000000..2096480f22 --- /dev/null +++ b/selfdrive/ui/layouts/main.py @@ -0,0 +1,92 @@ +import pyray as rl +from enum import IntEnum +from openpilot.selfdrive.ui.layouts.sidebar import Sidebar, SIDEBAR_WIDTH +from openpilot.selfdrive.ui.layouts.home import HomeLayout +from openpilot.selfdrive.ui.layouts.settings.settings import SettingsLayout +from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.selfdrive.ui.onroad.augmented_road_view import AugmentedRoadView + + +class MainState(IntEnum): + HOME = 0 + SETTINGS = 1 + ONROAD = 2 + + +class MainLayout: + def __init__(self): + self._sidebar = Sidebar() + self._sidebar_visible = True + self._current_mode = MainState.HOME + self._prev_onroad = False + self._window_rect = None + self._current_callback: callable | None = None + + # Initialize layouts + self._layouts = {MainState.HOME: HomeLayout(), MainState.SETTINGS: SettingsLayout(), MainState.ONROAD: AugmentedRoadView()} + + self._sidebar_rect = rl.Rectangle(0, 0, 0, 0) + self._content_rect = rl.Rectangle(0, 0, 0, 0) + + # Set callbacks + self._setup_callbacks() + + def render(self, rect): + self._current_callback = None + + self._update_layout_rects(rect) + self._render_main_content() + self._handle_input() + + if self._current_callback: + self._current_callback() + + def _setup_callbacks(self): + self._sidebar.set_callbacks( + on_settings=lambda: setattr(self, '_current_callback', self._on_settings_clicked), + on_flag=lambda: setattr(self, '_current_callback', self._on_flag_clicked), + ) + self._layouts[MainState.SETTINGS].set_callbacks( + on_close=lambda: setattr(self, '_current_callback', self._on_settings_closed) + ) + + def _update_layout_rects(self, rect): + self._window_rect = rect + self._sidebar_rect = rl.Rectangle(rect.x, rect.y, SIDEBAR_WIDTH, rect.height) + + x_offset = SIDEBAR_WIDTH if self._sidebar_visible else 0 + self._content_rect = rl.Rectangle(rect.y + x_offset, rect.y, rect.width - x_offset, rect.height) + + def _on_settings_clicked(self): + self._current_mode = MainState.SETTINGS + self._sidebar_visible = False + + def _on_settings_closed(self): + self._current_mode = MainState.HOME if not ui_state.started else MainState.ONROAD + self._sidebar_visible = True + + def _on_flag_clicked(self): + pass + + def _render_main_content(self): + # Render sidebar + if self._sidebar_visible: + self._sidebar.render(self._sidebar_rect) + + if ui_state.started != self._prev_onroad: + self._prev_onroad = ui_state.started + if ui_state.started: + self._current_mode = MainState.ONROAD + else: + self._current_mode = MainState.HOME + + content_rect = self._content_rect if self._sidebar_visible else self._window_rect + self._layouts[self._current_mode].render(content_rect) + + def _handle_input(self): + if self._current_mode != MainState.ONROAD or not rl.is_mouse_button_pressed(rl.MouseButton.MOUSE_BUTTON_LEFT): + return + + mouse_pos = rl.get_mouse_position() + if rl.check_collision_point_rec(mouse_pos, self._content_rect): + self._sidebar_visible = not self._sidebar_visible diff --git a/selfdrive/ui/layouts/settings/settings.py b/selfdrive/ui/layouts/settings/settings.py new file mode 100644 index 0000000000..6a9a50b855 --- /dev/null +++ b/selfdrive/ui/layouts/settings/settings.py @@ -0,0 +1,174 @@ +import pyray as rl +from dataclasses import dataclass +from enum import IntEnum +from collections.abc import Callable +from openpilot.common.params import Params +from openpilot.system.ui.lib.application import gui_app, FontWeight +from openpilot.system.ui.lib.label import gui_text_box + +# Import individual panels + +SETTINGS_CLOSE_TEXT = "X" +# Constants +SIDEBAR_WIDTH = 500 +CLOSE_BTN_SIZE = 200 +NAV_BTN_HEIGHT = 80 +PANEL_MARGIN = 50 +SCROLL_SPEED = 30 + +# Colors +SIDEBAR_COLOR = rl.BLACK +PANEL_COLOR = rl.Color(41, 41, 41, 255) +CLOSE_BTN_COLOR = rl.Color(41, 41, 41, 255) +CLOSE_BTN_PRESSED = rl.Color(59, 59, 59, 255) +TEXT_NORMAL = rl.Color(128, 128, 128, 255) +TEXT_SELECTED = rl.Color(255, 255, 255, 255) +TEXT_PRESSED = rl.Color(173, 173, 173, 255) + + +class PanelType(IntEnum): + DEVICE = 0 + NETWORK = 1 + TOGGLES = 2 + SOFTWARE = 3 + FIREHOSE = 4 + DEVELOPER = 5 + + +@dataclass +class PanelInfo: + name: str + instance: object + button_rect: rl.Rectangle + + +class SettingsLayout: + def __init__(self): + self._params = Params() + self._current_panel = PanelType.DEVICE + self._close_btn_pressed = False + self._scroll_offset = 0.0 + self._max_scroll = 0.0 + + # Panel configuration + self._panels = { + PanelType.DEVICE: PanelInfo("Device", None, rl.Rectangle(0, 0, 0, 0)), + PanelType.TOGGLES: PanelInfo("Toggles", None, rl.Rectangle(0, 0, 0, 0)), + PanelType.SOFTWARE: PanelInfo("Software", None, rl.Rectangle(0, 0, 0, 0)), + PanelType.FIREHOSE: PanelInfo("Firehose", None, rl.Rectangle(0, 0, 0, 0)), + PanelType.NETWORK: PanelInfo("Network", None, rl.Rectangle(0, 0, 0, 0)), + PanelType.DEVELOPER: PanelInfo("Developer", None, rl.Rectangle(0, 0, 0, 0)), + } + + self._font_medium = gui_app.font(FontWeight.MEDIUM) + self._font_bold = gui_app.font(FontWeight.SEMI_BOLD) + + # Callbacks + self._close_callback: Callable | None = None + + def set_callbacks(self, on_close: Callable): + self._close_callback = on_close + + def render(self, rect: rl.Rectangle): + # Calculate layout + sidebar_rect = rl.Rectangle(rect.x, rect.y, SIDEBAR_WIDTH, rect.height) + panel_rect = rl.Rectangle(rect.x + SIDEBAR_WIDTH, rect.y, rect.width - SIDEBAR_WIDTH, rect.height) + + # Draw components + self._draw_sidebar(sidebar_rect) + self._draw_current_panel(panel_rect) + + if rl.is_mouse_button_released(rl.MouseButton.MOUSE_BUTTON_LEFT): + self.handle_mouse_release(rl.get_mouse_position()) + + def _draw_sidebar(self, rect: rl.Rectangle): + rl.draw_rectangle_rec(rect, SIDEBAR_COLOR) + + # Close button + close_btn_rect = rl.Rectangle( + rect.x + (rect.width - CLOSE_BTN_SIZE) / 2, rect.y + 45, CLOSE_BTN_SIZE, CLOSE_BTN_SIZE + ) + + close_color = CLOSE_BTN_PRESSED if self._close_btn_pressed else CLOSE_BTN_COLOR + rl.draw_rectangle_rounded(close_btn_rect, 0.5, 20, close_color) + close_text_size = rl.measure_text_ex(self._font_bold, SETTINGS_CLOSE_TEXT, 140, 0) + close_text_pos = rl.Vector2( + close_btn_rect.x + (close_btn_rect.width - close_text_size.x) / 2, + close_btn_rect.y + (close_btn_rect.height - close_text_size.y) / 2 - 20, + ) + rl.draw_text_ex(self._font_bold, SETTINGS_CLOSE_TEXT, close_text_pos, 140, 0, TEXT_SELECTED) + + # Store close button rect for click detection + self._close_btn_rect = close_btn_rect + + # Navigation buttons + nav_start_y = rect.y + 300 + button_spacing = 20 + + i = 0 + for panel_type, panel_info in self._panels.items(): + button_rect = rl.Rectangle( + rect.x + 50, + nav_start_y + i * (NAV_BTN_HEIGHT + button_spacing), + rect.width - 150, # Right-aligned with margin + NAV_BTN_HEIGHT, + ) + + # Button styling + is_selected = panel_type == self._current_panel + text_color = TEXT_SELECTED if is_selected else TEXT_NORMAL + + # Draw button text (right-aligned) + text_size = rl.measure_text_ex(self._font_medium, panel_info.name, 65, 0) + text_pos = rl.Vector2( + button_rect.x + button_rect.width - text_size.x, button_rect.y + (button_rect.height - text_size.y) / 2 + ) + rl.draw_text_ex(self._font_medium, panel_info.name, text_pos, 65, 0, text_color) + + # Store button rect for click detection + panel_info.button_rect = button_rect + i += 1 + + def _draw_current_panel(self, rect: rl.Rectangle): + content_rect = rl.Rectangle(rect.x + PANEL_MARGIN, rect.y + 25, rect.width - (PANEL_MARGIN * 2), rect.height - 50) + rl.draw_rectangle_rounded(content_rect, 0.03, 30, PANEL_COLOR) + gui_text_box( + content_rect, + f"Demo {self._panels[self._current_panel].name} Panel", + font_size=170, + color=rl.WHITE, + alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER, + alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE, + ) + + def handle_mouse_release(self, mouse_pos: rl.Vector2) -> bool: + # Check close button + if rl.check_collision_point_rec(mouse_pos, self._close_btn_rect): + self._close_btn_pressed = True + if self._close_callback: + self._close_callback() + return True + + # Check navigation buttons + for panel_type, panel_info in self._panels.items(): + if rl.check_collision_point_rec(mouse_pos, panel_info.button_rect): + self._switch_to_panel(panel_type) + return True + + return False + + def _switch_to_panel(self, panel_type: PanelType): + if panel_type != self._current_panel: + self._current_panel = panel_type + self._scroll_offset = 0.0 # Reset scroll when switching panels + self._transition_progress = 0.0 + self._transitioning = True + + def set_current_panel(self, index: int, param: str = ""): + panel_types = list(self._panels.keys()) + if 0 <= index < len(panel_types): + self._switch_to_panel(panel_types[index]) + + def close_settings(self): + if self._close_callback: + self._close_callback() diff --git a/selfdrive/ui/layouts/sidebar.py b/selfdrive/ui/layouts/sidebar.py new file mode 100644 index 0000000000..eeb073e67b --- /dev/null +++ b/selfdrive/ui/layouts/sidebar.py @@ -0,0 +1,207 @@ +import pyray as rl +import time +from dataclasses import dataclass +from collections.abc import Callable +from cereal import log +from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.system.ui.lib.application import gui_app, FontWeight + +SIDEBAR_WIDTH = 300 +METRIC_HEIGHT = 126 +METRIC_WIDTH = 240 +METRIC_MARGIN = 30 + +SETTINGS_BTN = rl.Rectangle(50, 35, 200, 117) +HOME_BTN = rl.Rectangle(60, 860, 180, 180) + +ThermalStatus = log.DeviceState.ThermalStatus +NetworkType = log.DeviceState.NetworkType + +# Color scheme +class Colors: + SIDEBAR_BG = rl.Color(57, 57, 57, 255) + WHITE = rl.Color(255, 255, 255, 255) + WHITE_DIM = rl.Color(255, 255, 255, 85) + GRAY = rl.Color(84, 84, 84, 255) + + # Status colors + GOOD = rl.Color(255, 255, 255, 255) + WARNING = rl.Color(218, 202, 37, 255) + DANGER = rl.Color(201, 34, 49, 255) + + # UI elements + METRIC_BORDER = rl.Color(255, 255, 255, 85) + BUTTON_NORMAL = rl.Color(255, 255, 255, 255) + BUTTON_PRESSED = rl.Color(255, 255, 255, 166) + +NETWORK_TYPES = { + NetworkType.none: "Offline", + NetworkType.wifi: "WiFi", + NetworkType.cell2G: "2G", + NetworkType.cell3G: "3G", + NetworkType.cell4G: "LTE", + NetworkType.cell5G: "5G", + NetworkType.ethernet: "Ethernet", +} + + +@dataclass(slots=True) +class MetricData: + label: str + value: str + color: rl.Color + + def update(self, label: str, value: str, color: rl.Color): + self.label = label + self.value = value + self.color = color + +class Sidebar: + def __init__(self): + self._net_type = NETWORK_TYPES.get(NetworkType.none) + self._net_strength = 0 + + self._temp_status = MetricData("TEMP", "GOOD", Colors.GOOD) + self._panda_status = MetricData("VEHICLE", "ONLINE", Colors.GOOD) + self._connect_status = MetricData("CONNECT", "OFFLINE", Colors.WARNING) + + self._home_img = gui_app.texture("images/button_home.png", HOME_BTN.width, HOME_BTN.height) + self._flag_img = gui_app.texture("images/button_flag.png", HOME_BTN.width, HOME_BTN.height) + self._settings_img = gui_app.texture("images/button_settings.png", SETTINGS_BTN.width, SETTINGS_BTN.height) + self._font_regular = gui_app.font(FontWeight.NORMAL) + self._font_bold = gui_app.font(FontWeight.SEMI_BOLD) + + # Callbacks + self._on_settings_click: Callable | None = None + self._on_flag_click: Callable | None = None + + def set_callbacks(self, on_settings: Callable | None = None, on_flag: Callable | None = None): + self._on_settings_click = on_settings + self._on_flag_click = on_flag + + def render(self, rect: rl.Rectangle): + self.update_state() + + # Background + rl.draw_rectangle_rec(rect, Colors.SIDEBAR_BG) + + self._draw_buttons(rect) + self._draw_network_indicator(rect) + self._draw_metrics(rect) + + self._handle_mouse_release() + + def update_state(self): + sm = ui_state.sm + if not sm.updated['deviceState']: + return + + device_state = sm['deviceState'] + + self._update_network_status(device_state) + self._update_temperature_status(device_state) + self._update_connection_status(device_state) + self._update_panda_status() + + def _update_network_status(self, device_state): + self._net_type = NETWORK_TYPES.get(device_state.networkType.raw, "Unknown") + strength = device_state.networkStrength + self._net_strength = max(0, min(5, strength.raw + 1)) if strength > 0 else 0 + + def _update_temperature_status(self, device_state): + thermal_status = device_state.thermalStatus + + if thermal_status == ThermalStatus.green: + self._temp_status.update("TEMP", "GOOD", Colors.GOOD) + elif thermal_status == ThermalStatus.yellow: + self._temp_status.update("TEMP", "OK", Colors.WARNING) + else: + self._temp_status.update("TEMP", "HIGH", Colors.DANGER) + + def _update_connection_status(self, device_state): + last_ping = device_state.lastAthenaPingTime + if last_ping == 0: + self._connect_status.update("CONNECT", "OFFLINE", Colors.WARNING) + elif time.monotonic_ns() - last_ping < 80_000_000_000: # 80 seconds in nanoseconds + self._connect_status.update("CONNECT", "ONLINE", Colors.GOOD) + else: + self._connect_status.update("CONNECT", "ERROR", Colors.DANGER) + + def _update_panda_status(self): + if ui_state.panda_type == log.PandaState.PandaType.unknown: + self._panda_status.update("NO", "PANDA", Colors.DANGER) + else: + self._panda_status.update("VEHICLE", "ONLINE", Colors.GOOD) + + def _handle_mouse_release(self): + if not rl.is_mouse_button_released(rl.MouseButton.MOUSE_BUTTON_LEFT): + return + + mouse_pos = rl.get_mouse_position() + if rl.check_collision_point_rec(mouse_pos, SETTINGS_BTN): + if self._on_settings_click: + self._on_settings_click() + elif rl.check_collision_point_rec(mouse_pos, HOME_BTN) and ui_state.started: + if self._on_flag_click: + self._on_flag_click() + + def _draw_buttons(self, rect: rl.Rectangle): + mouse_pos = rl.get_mouse_position() + mouse_down = rl.is_mouse_button_down(rl.MouseButton.MOUSE_BUTTON_LEFT) + + + # Settings button + settings_down = mouse_down and rl.check_collision_point_rec(mouse_pos, SETTINGS_BTN) + tint = Colors.BUTTON_PRESSED if settings_down else Colors.BUTTON_NORMAL + rl.draw_texture(self._settings_img, int(SETTINGS_BTN.x), int(SETTINGS_BTN.y), tint) + + # Home/Flag button + flag_pressed = mouse_down and rl.check_collision_point_rec(mouse_pos, HOME_BTN) + button_img = self._flag_img if ui_state.started else self._home_img + + tint = Colors.BUTTON_PRESSED if (ui_state.started and flag_pressed) else Colors.BUTTON_NORMAL + rl.draw_texture(button_img, int(HOME_BTN.x), int(HOME_BTN.y), tint) + + def _draw_network_indicator(self, rect: rl.Rectangle): + # Signal strength dots + x_start = rect.x + 58 + y_pos = rect.y + 196 + dot_size = 27 + dot_spacing = 37 + + for i in range(5): + color = Colors.WHITE if i < self._net_strength else Colors.GRAY + x = int(x_start + i * dot_spacing + dot_size // 2) + y = int(y_pos + dot_size // 2) + rl.draw_circle(x, y, dot_size // 2, color) + + # Network type text + text_y = rect.y + 247 + text_pos = rl.Vector2(rect.x + 58, text_y) + rl.draw_text_ex(self._font_regular, self._net_type, text_pos, 35, 0, Colors.WHITE) + + def _draw_metrics(self, rect: rl.Rectangle): + metrics = [(self._temp_status, 338), (self._panda_status, 496), (self._connect_status, 654)] + + for metric, y_offset in metrics: + self._draw_metric(rect, metric, rect.y + y_offset) + + def _draw_metric(self, rect: rl.Rectangle, metric: MetricData, y: float): + metric_rect = rl.Rectangle(rect.x + METRIC_MARGIN, y, METRIC_WIDTH, METRIC_HEIGHT) + # Draw colored left edge (clipped rounded rectangle) + edge_rect = rl.Rectangle(metric_rect.x + 4, metric_rect.y + 4, 100, 118) + rl.begin_scissor_mode(int(metric_rect.x + 4), int(metric_rect.y), 18, int(metric_rect.height)) + rl.draw_rectangle_rounded(edge_rect, 0.18, 10, metric.color) + rl.end_scissor_mode() + + # Draw border + rl.draw_rectangle_rounded_lines_ex(metric_rect, 0.15, 10, 2, Colors.METRIC_BORDER) + + # Draw text + text = f"{metric.label}\n{metric.value}" + text_size = rl.measure_text_ex(self._font_bold, text, 35, 0) + text_pos = rl.Vector2( + metric_rect.x + 22 + (metric_rect.width - 22 - text_size.x) / 2, + metric_rect.y + (metric_rect.height - text_size.y) / 2 + ) + rl.draw_text_ex(self._font_bold, text, text_pos, 35, 0, Colors.WHITE) diff --git a/selfdrive/ui/onroad/__init__.py b/selfdrive/ui/onroad/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/ui/onroad/alert_renderer.py b/selfdrive/ui/onroad/alert_renderer.py new file mode 100644 index 0000000000..7b0128271e --- /dev/null +++ b/selfdrive/ui/onroad/alert_renderer.py @@ -0,0 +1,159 @@ +import time +import pyray as rl +from dataclasses import dataclass +from cereal import messaging, log +from openpilot.system.hardware import TICI +from openpilot.system.ui.lib.application import gui_app, FontWeight, DEFAULT_FPS +from openpilot.system.ui.lib.label import gui_text_box +from openpilot.system.ui.lib.text_measure import measure_text_cached +from openpilot.selfdrive.ui.ui_state import ui_state + + +ALERT_MARGIN = 40 +ALERT_PADDING = 60 +ALERT_LINE_SPACING = 45 +ALERT_BORDER_RADIUS = 30 + +ALERT_FONT_SMALL = 66 +ALERT_FONT_MEDIUM = 74 +ALERT_FONT_BIG = 88 + +SELFDRIVE_STATE_TIMEOUT = 5 # Seconds +SELFDRIVE_UNRESPONSIVE_TIMEOUT = 10 # Seconds + + +# Constants +ALERT_COLORS = { + log.SelfdriveState.AlertStatus.normal: rl.Color(0, 0, 0, 235), # Black + log.SelfdriveState.AlertStatus.userPrompt: rl.Color(0xFE, 0x8C, 0x34, 235), # Orange + log.SelfdriveState.AlertStatus.critical: rl.Color(0xC9, 0x22, 0x31, 235), # Red +} + + +@dataclass +class Alert: + text1: str = "" + text2: str = "" + size: int = 0 + status: int = 0 + + +# Pre-defined alert instances +ALERT_STARTUP_PENDING = Alert( + text1="openpilot Unavailable", + text2="Waiting to start", + size=log.SelfdriveState.AlertSize.mid, + status=log.SelfdriveState.AlertStatus.normal, +) + +ALERT_CRITICAL_TIMEOUT = Alert( + text1="TAKE CONTROL IMMEDIATELY", + text2="System Unresponsive", + size=log.SelfdriveState.AlertSize.full, + status=log.SelfdriveState.AlertStatus.critical, +) + +ALERT_CRITICAL_REBOOT = Alert( + text1="System Unresponsive", + text2="Reboot Device", + size=log.SelfdriveState.AlertSize.full, + status=log.SelfdriveState.AlertStatus.critical, +) + + +class AlertRenderer: + def __init__(self): + self.font_regular: rl.Font = gui_app.font(FontWeight.NORMAL) + self.font_bold: rl.Font = gui_app.font(FontWeight.BOLD) + + def get_alert(self, sm: messaging.SubMaster) -> Alert | None: + """Generate the current alert based on selfdrive state.""" + ss = sm['selfdriveState'] + + # Check if selfdriveState messages have stopped arriving + if not sm.updated['selfdriveState']: + recv_frame = sm.recv_frame['selfdriveState'] + if (sm.frame - recv_frame) > 5 * DEFAULT_FPS: + # Check if waiting to start + if recv_frame < ui_state.started_frame: + return ALERT_STARTUP_PENDING + + # Handle selfdrive timeout + if TICI: + ss_missing = time.monotonic() - sm.recv_time['selfdriveState'] + if ss_missing > SELFDRIVE_STATE_TIMEOUT: + if ss.enabled and (ss_missing - SELFDRIVE_STATE_TIMEOUT) < SELFDRIVE_UNRESPONSIVE_TIMEOUT: + return ALERT_CRITICAL_TIMEOUT + return ALERT_CRITICAL_REBOOT + + # No alert if size is none + if ss.alertSize == 0: + return None + + # Return current alert + return Alert(text1=ss.alertText1, text2=ss.alertText2, size=ss.alertSize, status=ss.alertStatus) + + def draw(self, rect: rl.Rectangle, sm: messaging.SubMaster) -> None: + alert = self.get_alert(sm) + if not alert: + return + + alert_rect = self._get_alert_rect(rect, alert.size) + self._draw_background(alert_rect, alert) + + text_rect = rl.Rectangle( + alert_rect.x + ALERT_PADDING, + alert_rect.y + ALERT_PADDING, + alert_rect.width - 2 * ALERT_PADDING, + alert_rect.height - 2 * ALERT_PADDING + ) + self._draw_text(text_rect, alert) + + def _get_alert_rect(self, rect: rl.Rectangle, size: int) -> rl.Rectangle: + if size == log.SelfdriveState.AlertSize.full: + return rect + + height = (ALERT_FONT_MEDIUM + 2 * ALERT_PADDING if size == log.SelfdriveState.AlertSize.small else + ALERT_FONT_BIG + ALERT_LINE_SPACING + ALERT_FONT_SMALL + 2 * ALERT_PADDING) + + return rl.Rectangle( + rect.x + ALERT_MARGIN, + rect.y + rect.height - ALERT_MARGIN - height, + rect.width - 2 * ALERT_MARGIN, + height + ) + + def _draw_background(self, rect: rl.Rectangle, alert: Alert) -> None: + color = ALERT_COLORS.get(alert.status, ALERT_COLORS[log.SelfdriveState.AlertStatus.normal]) + + if alert.size != log.SelfdriveState.AlertSize.full: + roundness = ALERT_BORDER_RADIUS / (min(rect.width, rect.height) / 2) + rl.draw_rectangle_rounded(rect, roundness, 10, color) + else: + rl.draw_rectangle_rec(rect, color) + + def _draw_text(self, rect: rl.Rectangle, alert: Alert) -> None: + if alert.size == log.SelfdriveState.AlertSize.small: + self._draw_centered(alert.text1, rect, self.font_bold, ALERT_FONT_MEDIUM) + + elif alert.size == log.SelfdriveState.AlertSize.mid: + self._draw_centered(alert.text1, rect, self.font_bold, ALERT_FONT_BIG, center_y=False) + rect.y += ALERT_FONT_BIG + ALERT_LINE_SPACING + self._draw_centered(alert.text2, rect, self.font_regular, ALERT_FONT_SMALL, center_y=False) + + else: + is_long = len(alert.text1) > 15 + font_size1 = 132 if is_long else 177 + align_ment = rl.GuiTextAlignment.TEXT_ALIGN_CENTER + vertical_align = rl.GuiTextAlignmentVertical.TEXT_ALIGN_MIDDLE + text_rect = rl.Rectangle(rect.x, rect.y, rect.width, rect.height // 2) + + gui_text_box(text_rect, alert.text1, font_size1, alignment=align_ment, alignment_vertical=vertical_align, font_weight=FontWeight.BOLD) + text_rect.y = rect.y + rect.height // 2 + gui_text_box(text_rect, alert.text2, ALERT_FONT_BIG, alignment=align_ment) + + def _draw_centered(self, text, rect, font, font_size, center_y=True, color=rl.WHITE) -> None: + text_size = measure_text_cached(font, text, font_size) + x = rect.x + (rect.width - text_size.x) / 2 + y = rect.y + ((rect.height - text_size.y) / 2 if center_y else 0) + rl.draw_text_ex(font, text, rl.Vector2(x, y), font_size, 0, color) diff --git a/system/ui/onroad/augmented_road_view.py b/selfdrive/ui/onroad/augmented_road_view.py similarity index 71% rename from system/ui/onroad/augmented_road_view.py rename to selfdrive/ui/onroad/augmented_road_view.py index 18f94325f4..fa7469d2dd 100644 --- a/system/ui/onroad/augmented_road_view.py +++ b/selfdrive/ui/onroad/augmented_road_view.py @@ -1,14 +1,14 @@ import numpy as np import pyray as rl -from enum import Enum -from cereal import messaging, log +from cereal import log from msgq.visionipc import VisionStreamType -from openpilot.system.ui.onroad.alert_renderer import AlertRenderer -from openpilot.system.ui.onroad.driver_state import DriverStateRenderer -from openpilot.system.ui.onroad.hud_renderer import HudRenderer -from openpilot.system.ui.onroad.model_renderer import ModelRenderer -from openpilot.system.ui.widgets.cameraview import CameraView +from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus, UI_BORDER_SIZE +from openpilot.selfdrive.ui.onroad.alert_renderer import AlertRenderer +from openpilot.selfdrive.ui.onroad.driver_state import DriverStateRenderer +from openpilot.selfdrive.ui.onroad.hud_renderer import HudRenderer +from openpilot.selfdrive.ui.onroad.model_renderer import ModelRenderer +from openpilot.selfdrive.ui.onroad.cameraview import CameraView from openpilot.system.ui.lib.application import gui_app from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCameraConfig, view_frame_from_device_frame from openpilot.common.transformations.orientation import rot_from_euler @@ -17,22 +17,18 @@ from openpilot.common.transformations.orientation import rot_from_euler OpState = log.SelfdriveState.OpenpilotState CALIBRATED = log.LiveCalibrationData.Status.calibrated DEFAULT_DEVICE_CAMERA = DEVICE_CAMERAS["tici", "ar0231"] -UI_BORDER_SIZE = 30 -class BorderStatus(Enum): - DISENGAGED = rl.Color(0x17, 0x33, 0x49, 0xc8) # Blue for disengaged state - OVERRIDE = rl.Color(0x91, 0x9b, 0x95, 0xf1) # Gray for override state - ENGAGED = rl.Color(0x17, 0x86, 0x44, 0xf1) # Green for engaged state +BORDER_COLORS = { + UIStatus.DISENGAGED: rl.Color(0x17, 0x33, 0x49, 0xC8), # Blue for disengaged state + UIStatus.OVERRIDE: rl.Color(0x91, 0x9B, 0x95, 0xF1), # Gray for override state + UIStatus.ENGAGED: rl.Color(0x17, 0x86, 0x44, 0xF1), # Green for engaged state +} class AugmentedRoadView(CameraView): - def __init__(self, sm: messaging.SubMaster, stream_type: VisionStreamType): + def __init__(self, stream_type: VisionStreamType = VisionStreamType.VISION_STREAM_ROAD): super().__init__("camerad", stream_type) - self.sm = sm - self.stream_type = stream_type - self.is_wide_camera = stream_type == VisionStreamType.VISION_STREAM_WIDE_ROAD - 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() @@ -48,6 +44,10 @@ class AugmentedRoadView(CameraView): self.driver_state_renderer = DriverStateRenderer() def render(self, rect): + # Only render when system is started to avoid invalid data access + if not ui_state.started: + return + # Update calibration before rendering self._update_calibration() @@ -75,10 +75,10 @@ class AugmentedRoadView(CameraView): super().render(rect) # Draw all UI overlays - self.model_renderer.draw(self._content_rect, self.sm) - self._hud_renderer.draw(self._content_rect, self.sm) - self.alert_renderer.draw(self._content_rect, self.sm) - self.driver_state_renderer.draw(self._content_rect, self.sm) + self.model_renderer.draw(self._content_rect, ui_state.sm) + self._hud_renderer.draw(self._content_rect, ui_state.sm) + self.alert_renderer.draw(self._content_rect, ui_state.sm) + self.driver_state_renderer.draw(self._content_rect, ui_state.sm) # Custom UI extension point - add custom overlays here # Use self._content_rect for positioning within camera bounds @@ -87,18 +87,12 @@ class AugmentedRoadView(CameraView): rl.end_scissor_mode() def _draw_border(self, rect: rl.Rectangle): - state = self.sm["selfdriveState"] - if state.state in (OpState.preEnabled, OpState.overriding): - status = BorderStatus.OVERRIDE - elif state.enabled: - status = BorderStatus.ENGAGED - else: - status = BorderStatus.DISENGAGED - - rl.draw_rectangle_lines_ex(rect, UI_BORDER_SIZE, status.value) + border_color = BORDER_COLORS.get(ui_state.status, BORDER_COLORS[UIStatus.DISENGAGED]) + rl.draw_rectangle_lines_ex(rect, UI_BORDER_SIZE, border_color) 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))] @@ -106,7 +100,7 @@ class AugmentedRoadView(CameraView): if not (sm.updated["liveCalibration"] and sm.valid['liveCalibration']): return - calib = self.sm['liveCalibration'] + calib = sm['liveCalibration'] if len(calib.rpyCalib) != 3 or calib.calStatus != CALIBRATED: return @@ -121,7 +115,7 @@ class AugmentedRoadView(CameraView): def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: # Check if we can use cached matrix - calib_time = self.sm.recv_frame['liveCalibration'] + calib_time = ui_state.sm.recv_frame['liveCalibration'] current_dims = (self._content_rect.width, self._content_rect.height) if (self._last_calib_time == calib_time and self._last_rect_dims == current_dims and @@ -130,9 +124,10 @@ class AugmentedRoadView(CameraView): # Get camera configuration device_camera = self.device_camera or DEFAULT_DEVICE_CAMERA - intrinsic = device_camera.ecam.intrinsics if self.is_wide_camera else device_camera.fcam.intrinsics - calibration = self.view_from_wide_calib if self.is_wide_camera else self.view_from_calib - zoom = 2.0 if self.is_wide_camera else 1.1 + is_wide_camera = self.stream_type == VisionStreamType.VISION_STREAM_WIDE_ROAD + 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 + zoom = 2.0 if is_wide_camera else 1.1 # Calculate transforms for vanishing point inf_point = np.array([1000.0, 0.0, 0.0]) @@ -180,13 +175,14 @@ class AugmentedRoadView(CameraView): if __name__ == "__main__": gui_app.init_window("OnRoad Camera View") - sm = messaging.SubMaster(["modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", - "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2", - "roadCameraState", "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan"]) - road_camera_view = AugmentedRoadView(sm, VisionStreamType.VISION_STREAM_ROAD) + road_camera_view = AugmentedRoadView(VisionStreamType.VISION_STREAM_ROAD) + print("***press space to switch camera view***") try: for _ in gui_app.render(): - sm.update(0) + ui_state.update() + if rl.is_key_released(rl.KeyboardKey.KEY_SPACE): + is_wide = road_camera_view.stream_type == VisionStreamType.VISION_STREAM_WIDE_ROAD + road_camera_view.switch_stream(VisionStreamType.VISION_STREAM_ROAD if is_wide else VisionStreamType.VISION_STREAM_WIDE_ROAD) road_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) finally: road_camera_view.close() diff --git a/system/ui/widgets/cameraview.py b/selfdrive/ui/onroad/cameraview.py similarity index 91% rename from system/ui/widgets/cameraview.py rename to selfdrive/ui/onroad/cameraview.py index 49832444f8..d59e7b0a48 100644 --- a/system/ui/widgets/cameraview.py +++ b/selfdrive/ui/onroad/cameraview.py @@ -3,6 +3,7 @@ import pyray as rl from openpilot.system.hardware import TICI from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf +from openpilot.common.swaglog import cloudlog from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.lib.egl import init_egl, create_egl_image, destroy_egl_image, bind_egl_image_to_texture, EGLImage @@ -56,7 +57,10 @@ else: class CameraView: def __init__(self, name: str, stream_type: VisionStreamType): - self.client = VisionIpcClient(name, stream_type, False) + self.client = VisionIpcClient(name, stream_type, conflate=True) + self._name = name + self._stream_type = stream_type + self._texture_needs_update = True self.last_connection_attempt: float = 0.0 self.shader = rl.load_shader_from_memory(VERTEX_SHADER, FRAME_FRAGMENT_SHADER) @@ -80,6 +84,18 @@ class CameraView: self.egl_texture = rl.load_texture_from_image(temp_image) rl.unload_image(temp_image) + def switch_stream(self, stream_type: VisionStreamType) -> None: + if self._stream_type != stream_type: + cloudlog.debug(f'switching stream from {self._stream_type} to {stream_type}') + self._clear_textures() + self.frame = None + self._stream_type = stream_type + self.client = VisionIpcClient(self._name, stream_type, conflate=True) + + @property + def stream_type(self) -> VisionStreamType: + return self._stream_type + def close(self) -> None: self._clear_textures() @@ -92,6 +108,8 @@ class CameraView: if self.shader and self.shader.id: rl.unload_shader(self.shader) + self.client = None + def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: if not self.frame: return np.eye(3) @@ -201,12 +219,12 @@ class CameraView: current_time = rl.get_time() if current_time - self.last_connection_attempt < CONNECTION_RETRY_INTERVAL: return False - self.last_connection_attempt = current_time if not self.client.connect(False) or not self.client.num_buffers: return False + cloudlog.debug(f"Connected to {self._name} stream: {self._stream_type}, buffers: {self.client.num_buffers}") self._clear_textures() if not TICI: diff --git a/selfdrive/ui/onroad/driver_camera_view.py b/selfdrive/ui/onroad/driver_camera_view.py new file mode 100644 index 0000000000..2f9ff2ca7a --- /dev/null +++ b/selfdrive/ui/onroad/driver_camera_view.py @@ -0,0 +1,94 @@ +import numpy as np +import pyray as rl +from cereal import messaging +from msgq.visionipc import VisionStreamType +from openpilot.selfdrive.ui.onroad.cameraview import CameraView +from openpilot.selfdrive.ui.onroad.driver_state import DriverStateRenderer +from openpilot.system.ui.lib.application import gui_app, FontWeight +from openpilot.system.ui.lib.label import gui_label + + +class DriverCameraView(CameraView): + def __init__(self, stream_type: VisionStreamType): + super().__init__("camerad", stream_type) + self.driver_state_renderer = DriverStateRenderer() + + def render(self, rect, sm): + super().render(rect) + + if not self.frame: + gui_label( + rect, + "camera starting", + font_size=100, + font_weight=FontWeight.BOLD, + alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER, + ) + return + + self._draw_face_detection(rect, sm) + self.driver_state_renderer.draw(rect, sm) + + def _draw_face_detection(self, rect: rl.Rectangle, sm) -> None: + driver_state = sm["driverStateV2"] + is_rhd = driver_state.wheelOnRightProb > 0.5 + driver_data = driver_state.rightDriverData if is_rhd else driver_state.leftDriverData + face_detect = driver_data.faceProb > 0.7 + if not face_detect: + return + + # Get face position and orientation + face_x, face_y = driver_data.facePosition + face_std = max(driver_data.faceOrientationStd[0], driver_data.faceOrientationStd[1]) + alpha = 0.7 + if face_std > 0.15: + alpha = max(0.7 - (face_std - 0.15) * 3.5, 0.0) + + # use approx instead of distort_points + # TODO: replace with distort_points + fbox_x = int(1080.0 - 1714.0 * face_x) + fbox_y = int(-135.0 + (504.0 + abs(face_x) * 112.0) + (1205.0 - abs(face_x) * 724.0) * face_y) + box_size = 220 + + line_color = rl.Color(255, 255, 255, int(alpha * 255)) + rl.draw_rectangle_rounded_lines_ex( + rl.Rectangle(fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size), + 35.0 / box_size / 2, + 10, + 10, + line_color, + ) + + def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: + driver_view_ratio = 2.0 + + # Get stream dimensions + if self.frame: + stream_width = self.frame.width + stream_height = self.frame.height + else: + # Default values if frame not available + stream_width = 1928 + stream_height = 1208 + + yscale = stream_height * driver_view_ratio / stream_width + xscale = yscale * rect.height / rect.width * stream_width / stream_height + + return np.array([ + [xscale, 0.0, 0.0], + [0.0, yscale, 0.0], + [0.0, 0.0, 1.0] + ]) + + +if __name__ == "__main__": + gui_app.init_window("Driver Camera View") + sm = messaging.SubMaster(["selfdriveState", "driverStateV2", "driverMonitoringState"]) + + driver_camera_view = DriverCameraView(VisionStreamType.VISION_STREAM_DRIVER) + try: + for _ in gui_app.render(): + sm.update() + driver_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height), sm) + finally: + driver_camera_view.close() diff --git a/system/ui/onroad/driver_state.py b/selfdrive/ui/onroad/driver_state.py similarity index 94% rename from system/ui/onroad/driver_state.py rename to selfdrive/ui/onroad/driver_state.py index b998936903..8e950f514e 100644 --- a/system/ui/onroad/driver_state.py +++ b/selfdrive/ui/onroad/driver_state.py @@ -1,8 +1,10 @@ import numpy as np import pyray as rl from dataclasses import dataclass +from openpilot.selfdrive.ui.ui_state import ui_state, UI_BORDER_SIZE from openpilot.system.ui.lib.application import gui_app + # Default 3D coordinates for face keypoints as a NumPy array DEFAULT_FACE_KPTS_3D = np.array([ [-5.98, -51.20, 8.00], [-17.64, -49.14, 8.00], [-23.81, -46.40, 8.00], [-29.98, -40.91, 8.00], @@ -17,7 +19,6 @@ DEFAULT_FACE_KPTS_3D = np.array([ ], dtype=np.float32) # UI constants -UI_BORDER_SIZE = 30 BTN_SIZE = 192 IMG_SIZE = 144 ARC_LENGTH = 133 @@ -27,6 +28,9 @@ ARC_THICKNESS_EXTEND = 12.0 SCALES_POS = np.array([0.9, 0.4, 0.4], dtype=np.float32) SCALES_NEG = np.array([0.7, 0.4, 0.4], dtype=np.float32) +ARC_POINT_COUNT = 37 # Number of points in the arc +ARC_ANGLES = np.linspace(0.0, np.pi, ARC_POINT_COUNT, dtype=np.float32) + @dataclass class ArcData: """Data structure for arc rendering parameters.""" @@ -57,8 +61,8 @@ class DriverStateRenderer: # Pre-allocate drawing arrays self.face_lines = [rl.Vector2(0, 0) for _ in range(len(DEFAULT_FACE_KPTS_3D))] - self.h_arc_lines = [rl.Vector2(0, 0) for _ in range(37)] # 37 points for horizontal arc - self.v_arc_lines = [rl.Vector2(0, 0) for _ in range(37)] # 37 points for vertical arc + self.h_arc_lines = [rl.Vector2(0, 0) for _ in range(ARC_POINT_COUNT)] + self.v_arc_lines = [rl.Vector2(0, 0) for _ in range(ARC_POINT_COUNT)] # Load the driver face icon self.dm_img = gui_app.texture("icons/driver_face.png", IMG_SIZE, IMG_SIZE) @@ -92,8 +96,7 @@ class DriverStateRenderer: rl.draw_spline_linear(self.face_lines, len(self.face_lines), 5.2, self.white_color) # Set arc color based on engaged state - engaged = True - self.arc_color = self.engaged_color if engaged else self.disengaged_color + self.arc_color = self.engaged_color if ui_state.engaged else self.disengaged_color self.arc_color.a = int(0.4 * 255 * (1.0 - self.dm_fade_state)) # Fade out when inactive # Draw arcs @@ -104,7 +107,7 @@ class DriverStateRenderer: def _is_visible(self, sm): """Check if the visualization should be rendered.""" - return (sm.seen['driverStateV2'] and + return (sm.recv_frame['driverStateV2'] > ui_state.started_frame and sm.seen['driverMonitoringState'] and sm['selfdriveState'].alertSize == 0) @@ -218,9 +221,7 @@ class DriverStateRenderer: ) # Pre-calculate arc points - start_rad = np.deg2rad(start_angle) - end_rad = np.deg2rad(start_angle + 180) - angles = np.linspace(start_rad, end_rad, 37) + angles = ARC_ANGLES + np.deg2rad(start_angle) center_x = x + arc_data.width / 2 center_y = y + arc_data.height / 2 diff --git a/system/ui/onroad/hud_renderer.py b/selfdrive/ui/onroad/hud_renderer.py similarity index 76% rename from system/ui/onroad/hud_renderer.py rename to selfdrive/ui/onroad/hud_renderer.py index b63b50f26d..0e893acfe9 100644 --- a/system/ui/onroad/hud_renderer.py +++ b/selfdrive/ui/onroad/hud_renderer.py @@ -1,13 +1,15 @@ import pyray as rl from dataclasses import dataclass from cereal.messaging import SubMaster +from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus from openpilot.system.ui.lib.application import gui_app, FontWeight +from openpilot.system.ui.lib.text_measure import measure_text_cached from openpilot.common.conversions import Conversions as CV -from enum import IntEnum # Constants SET_SPEED_NA = 255 KM_TO_MILE = 0.621371 +CRUISE_DISABLED_CHAR = '–' @dataclass(frozen=True) @@ -52,23 +54,14 @@ FONT_SIZES = FontSizes() COLORS = Colors() -class HudStatus(IntEnum): - DISENGAGED = 0 - OVERRIDE = 1 - ENGAGED = 2 - - class HudRenderer: def __init__(self): """Initialize the HUD renderer.""" - self.is_metric: bool = False - self.status: HudStatus = HudStatus.DISENGAGED self.is_cruise_set: bool = False self.is_cruise_available: bool = False self.set_speed: float = SET_SPEED_NA self.speed: float = 0.0 self.v_ego_cluster_seen: bool = False - self.font_metrics_cache: dict[[str, int, str], rl.Vector2] = {} self._wheel_texture: rl.Texture = gui_app.texture('icons/chffr_wheel.png', UI_CONFIG.wheel_icon_size, UI_CONFIG.wheel_icon_size) self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD) self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD) @@ -76,10 +69,7 @@ class HudRenderer: def _update_state(self, sm: SubMaster) -> None: """Update HUD state based on car state and controls state.""" - self.is_metric = True - self.status = HudStatus.DISENGAGED - - if not sm.valid['carState']: + if sm.recv_frame["carState"] < ui_state.started_frame: self.is_cruise_set = False self.set_speed = SET_SPEED_NA self.speed = 0.0 @@ -95,13 +85,13 @@ class HudRenderer: self.is_cruise_set = 0 < self.set_speed < SET_SPEED_NA self.is_cruise_available = self.set_speed != -1 - if self.is_cruise_set and not self.is_metric: + if self.is_cruise_set and not ui_state.is_metric: self.set_speed *= KM_TO_MILE v_ego_cluster = car_state.vEgoCluster self.v_ego_cluster_seen = self.v_ego_cluster_seen or v_ego_cluster != 0.0 v_ego = v_ego_cluster if self.v_ego_cluster_seen else car_state.vEgo - speed_conversion = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH + speed_conversion = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH self.speed = max(0.0, v_ego * speed_conversion) def draw(self, rect: rl.Rectangle, sm: SubMaster) -> None: @@ -124,7 +114,7 @@ class HudRenderer: def _draw_set_speed(self, rect: rl.Rectangle) -> None: """Draw the MAX speed indicator box.""" - set_speed_width = UI_CONFIG.set_speed_width_metric if self.is_metric else UI_CONFIG.set_speed_width_imperial + set_speed_width = UI_CONFIG.set_speed_width_metric if ui_state.is_metric else UI_CONFIG.set_speed_width_imperial x = rect.x + 60 + (UI_CONFIG.set_speed_width_imperial - set_speed_width) // 2 y = rect.y + 45 @@ -136,14 +126,15 @@ class HudRenderer: set_speed_color = COLORS.dark_grey if self.is_cruise_set: set_speed_color = COLORS.white - max_color = { - HudStatus.DISENGAGED: COLORS.disengaged, - HudStatus.OVERRIDE: COLORS.override, - HudStatus.ENGAGED: COLORS.engaged, - }.get(self.status, COLORS.grey) + if ui_state.status == UIStatus.ENGAGED: + max_color = COLORS.engaged + elif ui_state.status == UIStatus.DISENGAGED: + max_color = COLORS.disengaged + elif ui_state.status == UIStatus.OVERRIDE: + max_color = COLORS.override max_text = "MAX" - max_text_width = self._measure_text(max_text, self._font_semi_bold, FONT_SIZES.max_speed, 'semi_bold').x + max_text_width = measure_text_cached(self._font_semi_bold, max_text, FONT_SIZES.max_speed).x rl.draw_text_ex( self._font_semi_bold, max_text, @@ -153,8 +144,8 @@ class HudRenderer: max_color, ) - set_speed_text = "–" if not self.is_cruise_set else str(round(self.set_speed)) - speed_text_width = self._measure_text(set_speed_text, self._font_bold, FONT_SIZES.set_speed, 'bold').x + set_speed_text = CRUISE_DISABLED_CHAR if not self.is_cruise_set else str(round(self.set_speed)) + speed_text_width = measure_text_cached(self._font_bold, set_speed_text, FONT_SIZES.set_speed).x rl.draw_text_ex( self._font_bold, set_speed_text, @@ -167,12 +158,12 @@ class HudRenderer: def _draw_current_speed(self, rect: rl.Rectangle) -> None: """Draw the current vehicle speed and unit.""" speed_text = str(round(self.speed)) - speed_text_size = self._measure_text(speed_text, self._font_bold, FONT_SIZES.current_speed, 'bold') + speed_text_size = measure_text_cached(self._font_bold, speed_text, FONT_SIZES.current_speed) speed_pos = rl.Vector2(rect.x + rect.width / 2 - speed_text_size.x / 2, 180 - speed_text_size.y / 2) rl.draw_text_ex(self._font_bold, speed_text, speed_pos, FONT_SIZES.current_speed, 0, COLORS.white) - unit_text = "km/h" if self.is_metric else "mph" - unit_text_size = self._measure_text(unit_text, self._font_medium, FONT_SIZES.speed_unit, 'medium') + unit_text = "km/h" if ui_state.is_metric else "mph" + unit_text_size = measure_text_cached(self._font_medium, unit_text, FONT_SIZES.speed_unit) unit_pos = rl.Vector2(rect.x + rect.width / 2 - unit_text_size.x / 2, 290 - unit_text_size.y / 2) rl.draw_text_ex(self._font_medium, unit_text, unit_pos, FONT_SIZES.speed_unit, 0, COLORS.white_translucent) @@ -182,13 +173,6 @@ class HudRenderer: center_y = int(rect.y + UI_CONFIG.border_size + UI_CONFIG.button_size / 2) rl.draw_circle(center_x, center_y, UI_CONFIG.button_size / 2, COLORS.black_translucent) - opacity = 0.7 if self.status == HudStatus.DISENGAGED else 1.0 + opacity = 0.7 if ui_state.status == UIStatus.DISENGAGED else 1.0 img_pos = rl.Vector2(center_x - self._wheel_texture.width / 2, center_y - self._wheel_texture.height / 2) rl.draw_texture_v(self._wheel_texture, img_pos, rl.Color(255, 255, 255, int(255 * opacity))) - - def _measure_text(self, text: str, font: rl.Font, font_size: int, font_type: str) -> rl.Vector2: - """Measure text dimensions with caching.""" - key = (text, font_size, font_type) - if key not in self.font_metrics_cache: - self.font_metrics_cache[key] = rl.measure_text_ex(font, text, font_size, 0) - return self.font_metrics_cache[key] diff --git a/system/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py similarity index 89% rename from system/ui/onroad/model_renderer.py rename to selfdrive/ui/onroad/model_renderer.py index ef7e567cda..38951fb504 100644 --- a/system/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -4,6 +4,7 @@ import pyray as rl from cereal import messaging, car from dataclasses import dataclass, field from openpilot.common.params import Params +from openpilot.selfdrive.ui.ui_state import ui_state from openpilot.system.ui.lib.application import DEFAULT_FPS from openpilot.system.ui.lib.shader_polygon import draw_polygon @@ -14,6 +15,8 @@ MAX_DRAW_DISTANCE = 100.0 PATH_COLOR_TRANSITION_DURATION = 0.5 # Seconds for color transition animation PATH_BLEND_INCREMENT = 1.0 / (PATH_COLOR_TRANSITION_DURATION * DEFAULT_FPS) +MAX_POINTS = 200 + THROTTLE_COLORS = [ rl.Color(13, 248, 122, 102), # HSLF(148/360, 0.94, 0.51, 0.4) rl.Color(114, 255, 92, 89), # HSLF(112/360, 1.0, 0.68, 0.35) @@ -61,6 +64,11 @@ class ModelRenderer: self._transform_dirty = True self._clip_region = None self._rect = None + + # Pre-allocated arrays for polygon conversion + self._temp_points_3d = np.empty((MAX_POINTS * 2, 3), dtype=np.float32) + self._temp_proj = np.empty((3, MAX_POINTS * 2), dtype=np.float32) + self._exp_gradient = { 'start': (0.0, 1.0), # Bottom of path 'end': (0.0, 0.0), # Top of path @@ -79,7 +87,8 @@ class ModelRenderer: def draw(self, rect: rl.Rectangle, sm: messaging.SubMaster): # Check if data is up-to-date - if not sm.valid['modelV2'] or not sm.valid['liveCalibration']: + if (sm.recv_frame["liveCalibration"] < ui_state.started_frame or + sm.recv_frame["modelV2"] < ui_state.started_frame): return # Set up clipping region @@ -105,13 +114,13 @@ class ModelRenderer: if model_updated: self._update_raw_points(model) - pos_x_array = self._path.raw_points[:, 0] - if pos_x_array.size == 0: + path_x_array = self._path.raw_points[:, 0] + if path_x_array.size == 0: return - self._update_model(lead_one, pos_x_array) + self._update_model(lead_one, path_x_array) if render_lead_indicator: - self._update_leads(radar_state, pos_x_array) + self._update_leads(radar_state, path_x_array) self._transform_dirty = False @@ -136,23 +145,26 @@ class ModelRenderer: self._road_edge_stds = np.array(model.roadEdgeStds, dtype=np.float32) self._acceleration_x = np.array(model.acceleration.x, dtype=np.float32) - def _update_leads(self, radar_state, pos_x_array): + def _update_leads(self, radar_state, path_x_array): """Update positions of lead vehicles""" self._lead_vehicles = [LeadVehicle(), LeadVehicle()] leads = [radar_state.leadOne, radar_state.leadTwo] + for i, lead_data in enumerate(leads): if lead_data and lead_data.status: d_rel, y_rel, v_rel = lead_data.dRel, lead_data.yRel, lead_data.vRel - idx = self._get_path_length_idx(pos_x_array, d_rel) + idx = self._get_path_length_idx(path_x_array, d_rel) + + # Get z-coordinate from path at the lead vehicle position z = self._path.raw_points[idx, 2] if idx < len(self._path.raw_points) else 0.0 point = self._map_to_screen(d_rel, -y_rel, z + self._path_offset_z) if point: self._lead_vehicles[i] = self._update_lead_vehicle(d_rel, v_rel, point, self._rect) - def _update_model(self, lead, pos_x_array): + def _update_model(self, lead, path_x_array): """Update model visualization data based on model message""" - max_distance = np.clip(pos_x_array[-1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE) - max_idx = self._get_path_length_idx(pos_x_array, max_distance) + max_distance = np.clip(path_x_array[-1], MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE) + max_idx = self._get_path_length_idx(self._lane_lines[0].raw_points[:, 0], max_distance) # Update lane lines using raw points for i, lane_line in enumerate(self._lane_lines): @@ -168,8 +180,8 @@ class ModelRenderer: if lead and lead.status: lead_d = lead.dRel * 2.0 max_distance = np.clip(lead_d - min(lead_d * 0.35, 10.0), 0.0, max_distance) - max_idx = self._get_path_length_idx(pos_x_array, max_distance) + max_idx = self._get_path_length_idx(path_x_array, max_distance) self._path.projected_points = self._map_line_to_polygon( self._path.raw_points, 0.9, self._path_offset_z, max_idx, allow_invert=False ) @@ -309,8 +321,10 @@ class ModelRenderer: @staticmethod def _get_path_length_idx(pos_x_array: np.ndarray, path_height: float) -> int: """Get the index corresponding to the given path height""" - idx = np.searchsorted(pos_x_array, path_height, side='right') - return int(np.clip(idx - 1, 0, len(pos_x_array) - 1)) + if len(pos_x_array) == 0: + return 0 + indices = np.where(pos_x_array <= path_height)[0] + return indices[-1] if indices.size > 0 else 0 def _map_to_screen(self, in_x, in_y, in_z): """Project a point in car space to screen space""" @@ -334,20 +348,22 @@ class ModelRenderer: return np.empty((0, 2), dtype=np.float32) # Slice points and filter non-negative x-coordinates - points = line[:max_idx + 1][line[:max_idx + 1, 0] >= 0] + points = line[:max_idx + 1] + points = points[points[:, 0] >= 0] if points.shape[0] == 0: return np.empty((0, 2), dtype=np.float32) # Create left and right 3D points in one array n_points = points.shape[0] - points_3d = np.empty((n_points * 2, 3), dtype=np.float32) + points_3d = self._temp_points_3d[:n_points * 2] points_3d[:n_points, 0] = points_3d[n_points:, 0] = points[:, 0] points_3d[:n_points, 1] = points[:, 1] - y_off points_3d[n_points:, 1] = points[:, 1] + y_off points_3d[:n_points, 2] = points_3d[n_points:, 2] = points[:, 2] + z_off # Single matrix multiplication for projections - proj = self._car_space_transform @ points_3d.T + proj = np.ascontiguousarray(self._temp_proj[:, :n_points * 2]) # Slice the pre-allocated array + np.dot(self._car_space_transform, points_3d.T, out=proj) valid_z = np.abs(proj[2]) > 1e-6 if not np.any(valid_z): return np.empty((0, 2), dtype=np.float32) @@ -388,15 +404,20 @@ class ModelRenderer: @staticmethod def _map_val(x, x0, x1, y0, y1): - x = max(x0, min(x, x1)) + x = np.clip(x, x0, x1) ra = x1 - x0 rb = y1 - y0 return (x - x0) * rb / ra + y0 if ra != 0 else y0 @staticmethod def _hsla_to_color(h, s, l, a): - r, g, b = [max(0, min(255, int(v * 255))) for v in colorsys.hls_to_rgb(h, l, s)] - return rl.Color(r, g, b, max(0, min(255, int(a * 255)))) + rgb = colorsys.hls_to_rgb(h, l, s) + return rl.Color( + int(rgb[0] * 255), + int(rgb[1] * 255), + int(rgb[2] * 255), + int(a * 255) + ) @staticmethod def _blend_colors(begin_colors, end_colors, t): diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 219b39546f..e3ca837907 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -9,7 +9,9 @@ #ifdef SUNNYPILOT #include "selfdrive/ui/sunnypilot/qt/onroad/buttons.h" #include "selfdrive/ui/sunnypilot/qt/onroad/hud.h" +#include "selfdrive/ui/sunnypilot/qt/onroad/model.h" #define ExperimentalButton ExperimentalButtonSP +#define ModelRenderer ModelRendererSP #else #include "selfdrive/ui/qt/onroad/buttons.h" #include "selfdrive/ui/qt/onroad/hud.h" diff --git a/selfdrive/ui/qt/onroad/model.cc b/selfdrive/ui/qt/onroad/model.cc index 52902abdc8..dc801e591f 100644 --- a/selfdrive/ui/qt/onroad/model.cc +++ b/selfdrive/ui/qt/onroad/model.cc @@ -1,18 +1,5 @@ #include "selfdrive/ui/qt/onroad/model.h" -constexpr int CLIP_MARGIN = 500; -constexpr float MIN_DRAW_DISTANCE = 10.0; -constexpr float MAX_DRAW_DISTANCE = 100.0; - -static int get_path_length_idx(const cereal::XYZTData::Reader &line, const float path_height) { - const auto &line_x = line.getX(); - int max_idx = 0; - for (int i = 1; i < line_x.size() && line_x[i] <= path_height; ++i) { - max_idx = i; - } - return max_idx; -} - void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { auto *s = uiState(); auto &sm = *(s->sm); @@ -35,7 +22,7 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) { update_model(model, lead_one); drawLaneLines(painter); - drawPath(painter, model, surface_rect.height()); + drawPath(painter, model, surface_rect); if (longitudinal_control && sm.alive("radarState")) { update_leads(radar_state, model.getPosition()); diff --git a/selfdrive/ui/qt/onroad/model.h b/selfdrive/ui/qt/onroad/model.h index 7e1b43acb4..85eb236e76 100644 --- a/selfdrive/ui/qt/onroad/model.h +++ b/selfdrive/ui/qt/onroad/model.h @@ -9,21 +9,39 @@ #include "selfdrive/ui/ui.h" #endif +constexpr int CLIP_MARGIN = 500; +constexpr float MIN_DRAW_DISTANCE = 10.0; +constexpr float MAX_DRAW_DISTANCE = 100.0; + +inline int get_path_length_idx(const cereal::XYZTData::Reader &line, const float path_height) { + const auto &line_x = line.getX(); + int max_idx = 0; + for (int i = 1; i < line_x.size() && line_x[i] <= path_height; ++i) { + max_idx = i; + } + return max_idx; +} + class ModelRenderer { public: + virtual ~ModelRenderer() = default; + ModelRenderer() {} void setTransform(const Eigen::Matrix3f &transform) { car_space_transform = transform; } void draw(QPainter &painter, const QRect &surface_rect); -private: +protected: bool mapToScreen(float in_x, float in_y, float in_z, QPointF *out); void mapLineToPolygon(const cereal::XYZTData::Reader &line, float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert = true); void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, const QRect &surface_rect); void update_leads(const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line); - void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead); + virtual void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead); void drawLaneLines(QPainter &painter); void drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height); + virtual void drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, const QRect &surface_rect) {; + drawPath(painter, model, surface_rect.height()); + } void updatePathGradient(QLinearGradient &bg); QColor blendColors(const QColor &start, const QColor &end, float t); diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript index a8a70f98b0..8c40998a6f 100644 --- a/selfdrive/ui/sunnypilot/SConscript +++ b/selfdrive/ui/sunnypilot/SConscript @@ -32,6 +32,7 @@ qt_src = [ "sunnypilot/qt/offroad/settings/sunnylink/sponsor_widget.cc", "sunnypilot/qt/offroad/settings/trips_panel.cc", "sunnypilot/qt/offroad/settings/vehicle_panel.cc", + "sunnypilot/qt/offroad/settings/visuals_panel.cc", "sunnypilot/qt/onroad/annotated_camera.cc", "sunnypilot/qt/onroad/buttons.cc", "sunnypilot/qt/onroad/hud.cc", diff --git a/selfdrive/ui/sunnypilot/qt/common/json_fetcher.h b/selfdrive/ui/sunnypilot/qt/common/json_fetcher.h index 2002e72aa0..5691f600a6 100644 --- a/selfdrive/ui/sunnypilot/qt/common/json_fetcher.h +++ b/selfdrive/ui/sunnypilot/qt/common/json_fetcher.h @@ -1,28 +1,9 @@ /** -The MIT License - -Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. - -Last updated: July 29, 2024 -***/ + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ #pragma once diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/osm/locations_fetcher.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/osm/locations_fetcher.h index a8d94fa536..32d169ad15 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/osm/locations_fetcher.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/osm/locations_fetcher.h @@ -18,13 +18,14 @@ #include "selfdrive/ui/sunnypilot/qt/common/json_fetcher.h" static const std::tuple defaultLocation = std::make_tuple("== None ==", ""); + // New class LocationsFetcher that handles web requests and JSON parsing class LocationsFetcher { public: - inline std::vector> + inline std::vector > getLocationsFromURL(const QUrl &url, const std::tuple &customLocation = defaultLocation) const { // Initialize an empty vector to hold the locations - std::vector> locations; + std::vector > locations; JsonFetcher fetcher; QJsonObject json = fetcher.getJsonFromURL(url.toString()); @@ -38,25 +39,25 @@ public: } // Sort locations by full name std::sort(locations.begin(), locations.end(), [](const auto &lhs, const auto &rhs) { - return std::get < 0 > (lhs) < std::get < 0 > (rhs); // Compare full names + return std::get<0>(lhs) < std::get<0>(rhs); // Compare full names }); // Optionally, you can now add defaultName entry at the beginning locations.insert(locations.begin(), std::tuple_cat(customLocation, std::make_tuple("", ""))); return locations; } - inline std::vector> + inline std::vector > getLocationsFromURL(const QString &url, const std::tuple &customLocation = defaultLocation) const { return getLocationsFromURL(QUrl(url), customLocation); } - inline std::vector> + inline std::vector > getOsmLocations(const std::tuple &customLocation = defaultLocation) const { - return getLocationsFromURL("https://raw.githubusercontent.com/pfeiferj/openpilot-mapd/main/nation_bounding_boxes.json", customLocation); + return getLocationsFromURL( "https://raw.githubusercontent.com/pfeiferj/openpilot-mapd/main/nation_bounding_boxes.json", customLocation); } - inline std::vector> + inline std::vector > getUsStatesLocations(const std::tuple &customLocation = defaultLocation) const { - return getLocationsFromURL("https://raw.githubusercontent.com/pfeiferj/openpilot-mapd/main/us_states_bounding_boxes.json", customLocation); + return getLocationsFromURL( "https://raw.githubusercontent.com/pfeiferj/openpilot-mapd/main/us_states_bounding_boxes.json", customLocation); } }; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/osm/models_fetcher.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/osm/models_fetcher.cc index 1bdd597b9c..b67e41e09d 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/osm/models_fetcher.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/osm/models_fetcher.cc @@ -9,11 +9,11 @@ #include -ModelsFetcher::ModelsFetcher(QObject* parent) : QObject(parent) { +ModelsFetcher::ModelsFetcher(QObject *parent) : QObject(parent) { manager = new QNetworkAccessManager(this); } -QByteArray ModelsFetcher::verifyFileHash(const QString& filePath, const QString& expectedHash, bool& hashMatches) { +QByteArray ModelsFetcher::verifyFileHash(const QString &filePath, const QString &expectedHash, bool &hashMatches) { hashMatches = false; // Default to false QByteArray fileData; @@ -38,7 +38,7 @@ QByteArray ModelsFetcher::verifyFileHash(const QString& filePath, const QString& } -void ModelsFetcher::download(const DownloadInfo& downloadInfo, const QString& filename, const QString& destinationPath) { +void ModelsFetcher::download(const DownloadInfo &downloadInfo, const QString &filename, const QString &destinationPath) { QString fullPath = destinationPath + "/" + filename; QFileInfo fileInfo(fullPath); bool hashMatches = false; @@ -54,14 +54,14 @@ void ModelsFetcher::download(const DownloadInfo& downloadInfo, const QString& fi // Proceed with download if file does not exist or hash verification failed QNetworkRequest request(downloadInfo.url); - QNetworkReply* reply = manager->get(request); + QNetworkReply *reply = manager->get(request); connect(reply, &QNetworkReply::downloadProgress, this, &ModelsFetcher::onDownloadProgress); connect(reply, &QNetworkReply::finished, this, [this, reply, destinationPath, filename, downloadInfo]() { onFinished(reply, destinationPath, filename, downloadInfo.sha256); }); } -QString extractFileName(const QString& contentDisposition) { +QString extractFileName(const QString &contentDisposition) { const QString filenameTag = "filename="; const int idx = contentDisposition.indexOf(filenameTag); if (idx < 0) { @@ -76,7 +76,7 @@ QString extractFileName(const QString& contentDisposition) { return filename; } -void ModelsFetcher::onFinished(QNetworkReply* reply, const QString& destinationPath, const QString& filename, const QString& expectedHash) { +void ModelsFetcher::onFinished(QNetworkReply *reply, const QString &destinationPath, const QString &filename, const QString &expectedHash) { // Handle download error if (reply->error()) { return; // Possibly emit a signal or log an error as per your error handling policy @@ -92,25 +92,22 @@ void ModelsFetcher::onFinished(QNetworkReply* reply, const QString& destinationP QString finalPath = QDir(destinationPath).filePath(finalFilename); // Save the downloaded file - - QFile file(finalPath); //ensure if the path exists and if not create it - if (!QDir().mkpath(destinationPath)) - { + if (!QDir().mkpath(destinationPath)) { LOGE("Unable to create directory: %s", destinationPath.toStdString().c_str()); emit downloadFailed(filename); return; // Stop further processing } - + //Retry the file open and write 3 times with a little delay between each retry for (int i = 0; i < 3; i++) { if (file.isOpen()) break; - + file.open(QIODevice::WriteOnly); if (!file.isOpen()) QThread::msleep(100); } - + // If the file is still not open, log an error and emit a failure signal if (!file.isOpen()) { LOGE("Unable to open file for writing: %s", finalPath.toStdString().c_str()); @@ -133,7 +130,6 @@ void ModelsFetcher::onFinished(QNetworkReply* reply, const QString& destinationP emit downloadFailed(filename); return; // Stop further processing } - emit downloadComplete(data, false); // Emit your success signal } @@ -143,7 +139,7 @@ void ModelsFetcher::onDownloadProgress(qint64 bytesReceived, qint64 bytesTotal) emit downloadProgress(progress); } -std::vector ModelsFetcher::getModelsFromURL(const QUrl&url) { +std::vector ModelsFetcher::getModelsFromURL(const QUrl &url) { std::vector models; JsonFetcher fetcher; QJsonObject json = fetcher.getJsonFromURL(url.toString()); @@ -153,7 +149,7 @@ std::vector ModelsFetcher::getModelsFromURL(const QUrl&url) { return models; } -std::vector ModelsFetcher::getModelsFromURL(const QString&url) { +std::vector ModelsFetcher::getModelsFromURL(const QString &url) { return getModelsFromURL(QUrl(url)); } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/osm/models_fetcher.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/osm/models_fetcher.h index d9a1060849..73890e9d5a 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/osm/models_fetcher.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/osm/models_fetcher.h @@ -121,22 +121,23 @@ class ModelsFetcher : public QObject { Q_OBJECT public: - explicit ModelsFetcher(QObject* parent = nullptr); - void download(const DownloadInfo&url, const QString& filename = "", const QString&destinationPath = MODELS_PATH); - static std::vector getModelsFromURL(const QUrl&url); - static std::vector getModelsFromURL(const QString&url); + explicit ModelsFetcher(QObject *parent = nullptr); + void download(const DownloadInfo &url, const QString &filename = "", const QString &destinationPath = MODELS_PATH); + static std::vector getModelsFromURL(const QUrl &url); + static std::vector getModelsFromURL(const QString &url); static std::vector getModelsFromURL(); signals: void downloadProgress(double percentage); - void downloadComplete(const QByteArray&data, bool fromCache = false); + void downloadComplete(const QByteArray &data, bool fromCache = false); void downloadFailed(const QString &filename); private: -// static bool verifyFileHash(const QString& filePath, const QString& expectedHash); - static QByteArray verifyFileHash(const QString& filePath, const QString& expectedHash, bool& hashMatches); + // static bool verifyFileHash(const QString& filePath, const QString& expectedHash); + static QByteArray verifyFileHash(const QString &filePath, const QString &expectedHash, bool &hashMatches); void onDownloadProgress(qint64 bytesReceived, qint64 bytesTotal); - void onFinished(QNetworkReply* reply, const QString&destinationPath, const QString&filename, const QString& expectedHash); + void onFinished(QNetworkReply *reply, const QString &destinationPath, const QString &filename, + const QString &expectedHash); - QNetworkAccessManager* manager; + QNetworkAccessManager *manager; }; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.cc index 651dc966f4..332fd71a18 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.cc @@ -32,7 +32,7 @@ OsmPanel::OsmPanel(QWidget *parent) : QFrame(parent) { timer = new QTimer(this); connect(timer, &QTimer::timeout, this, QOverload<>::of(&OsmPanel::updateLabels)); - timer->start(FAST_REFRESH_INTERVAL); // Time specified in milliseconds. + timer->start(FAST_REFRESH_INTERVAL); // Time specified in milliseconds. updateLabels(); osmScreen = new QWidget(this); @@ -43,7 +43,7 @@ OsmPanel::OsmPanel(QWidget *parent) : QFrame(parent) { } ButtonControlSP *OsmPanel::setupOsmDeleteMapsButton(QWidget *parent) { - osmDeleteMapsBtn = new ButtonControlSP(tr("Downloaded Maps"), tr("DELETE")); // Updated on updateLabels() + osmDeleteMapsBtn = new ButtonControlSP(tr("Downloaded Maps"), tr("DELETE")); // Updated on updateLabels() connect(osmDeleteMapsBtn, &ButtonControlSP::clicked, [=]() { if (showConfirmationDialog(parent, tr("This will delete ALL downloaded maps\n\nAre you sure you want to delete all the maps?"), tr("Yes, delete all the maps."))) { QtConcurrent::run([=]() { @@ -62,7 +62,7 @@ ButtonControlSP *OsmPanel::setupOsmDeleteMapsButton(QWidget *parent) { } ButtonControlSP *OsmPanel::setupOsmUpdateButton(QWidget *parent) { - osmUpdateBtn = new ButtonControlSP(tr("Database Update"), tr("CHECK")); // Updated on updateLabels() + osmUpdateBtn = new ButtonControlSP(tr("Database Update"), tr("CHECK")); // Updated on updateLabels() connect(osmUpdateBtn, &ButtonControlSP::clicked, [=]() { if (osm_download_in_progress && !download_failed_state) { updateLabels(); @@ -80,14 +80,14 @@ ButtonControlSP *OsmPanel::setupOsmDownloadButton(QWidget *parent) { connect(osmDownloadBtn, &ButtonControlSP::clicked, [=]() { osmDownloadBtn->setEnabled(false); osmDownloadBtn->setValue(tr("Fetching Country list...")); - const std::vector> locations = getOsmLocations(); + const std::vector > locations = getOsmLocations(); osmDownloadBtn->setEnabled(true); osmDownloadBtn->setValue(""); const QString initTitle = QString::fromStdString(params.get("OsmLocationTitle")); const QString currentTitle = ((initTitle == "== None ==") || (initTitle.length() == 0)) ? "== None ==" : initTitle; QStringList locationTitles; - for (auto &loc : locations) { + for (auto &loc: locations) { locationTitles.push_back(std::get<0>(loc)); } @@ -95,7 +95,7 @@ ButtonControlSP *OsmPanel::setupOsmDownloadButton(QWidget *parent) { if (!selection.isEmpty()) { params.put("OsmLocal", "1"); params.put("OsmLocationTitle", selection.toStdString()); - for (auto &loc : locations) { + for (auto &loc: locations) { if (std::get<0>(loc) == selection) { params.put("OsmLocationName", std::get<1>(loc).toStdString()); break; @@ -123,21 +123,22 @@ ButtonControlSP *OsmPanel::setupUsStatesButton(QWidget *parent) { const std::tuple allStatesOption = std::make_tuple("All States (~4.8 GB)", "All"); usStatesBtn->setEnabled(false); usStatesBtn->setValue(tr("Fetching State list...")); - const std::vector> locations = getUsStatesLocations(allStatesOption); + const std::vector > locations = + getUsStatesLocations(allStatesOption); usStatesBtn->setEnabled(true); usStatesBtn->setValue(""); const QString initTitle = QString::fromStdString(params.get("OsmStateTitle")); const QString currentTitle = ((initTitle == std::get<0>(allStatesOption)) || (initTitle.length() == 0)) ? tr("All") : initTitle; QStringList locationTitles; - for (auto &loc : locations) { + for (auto &loc: locations) { locationTitles.push_back(std::get<0>(loc)); } const QString selection = MultiOptionDialog::getSelection(tr("State"), locationTitles, currentTitle, this); if (!selection.isEmpty()) { params.put("OsmStateTitle", selection.toStdString()); - for (auto &loc : locations) { + for (auto &loc: locations) { if (std::get<0>(loc) == selection) { params.put("OsmStateName", std::get<1>(loc).toStdString()); break; @@ -152,12 +153,12 @@ ButtonControlSP *OsmPanel::setupUsStatesButton(QWidget *parent) { } updateLabels(); }); - usStatesBtn->setVisible(false); // initially hidden + usStatesBtn->setVisible(false); // initially hidden return usStatesBtn; } void OsmPanel::showEvent(QShowEvent *event) { - updateLabels(); // For snappier feeling + updateLabels(); // For snappier feeling if (!timer->isActive()) { timer->start(FAST_REFRESH_INTERVAL); } @@ -185,7 +186,7 @@ void OsmPanel::updateLabels() { LOGT("Timer Interval %d", timer->interval()); const std::string osmLastDownloadTimeStr = params.get("OsmDownloadedDate"); - if (!lastDownloadedTimePoint.has_value() && !osmLastDownloadTimeStr.empty()) { + if (!lastDownloadedTimePoint.has_value() && !osmLastDownloadTimeStr.empty()) { const double osmLastDownloadTime = std::stod(osmLastDownloadTimeStr); lastDownloadedTimePoint = std::chrono::system_clock::from_time_t(static_cast(osmLastDownloadTime)); } @@ -244,11 +245,11 @@ void OsmPanel::updateDownloadProgress() { osmDeleteMapsBtn->setValue(formatSize(mapsDirSize)); } -int OsmPanel::extractIntFromJson(const QJsonObject& json, const QString& key) { +int OsmPanel::extractIntFromJson(const QJsonObject &json, const QString &key) { return (json.contains(key)) ? json[key].toInt() : 0; } -QString OsmPanel::processUpdateStatus(bool pending_update, int total_files, int downloaded_files, const QJsonObject& json, bool failed_state) { +QString OsmPanel::processUpdateStatus(bool pending_update, int total_files, int downloaded_files, const QJsonObject &json, bool failed_state) { if (pending_update && !osm_download_in_progress && !total_files) { lastDownloadedTimePoint.reset(); return tr("Download starting..."); diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.h index 9ffa2956cb..171129a11c 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.h @@ -25,10 +25,11 @@ #include "selfdrive/ui/sunnypilot/ui.h" #include "system/hardware/hw.h" -constexpr int FAST_REFRESH_INTERVAL = 1000; // ms -constexpr int SLOW_REFRESH_INTERVAL = 5000; // ms +constexpr int FAST_REFRESH_INTERVAL = 1000; // ms +constexpr int SLOW_REFRESH_INTERVAL = 5000; // ms static const QString MAP_PATH = Hardware::PC() ? QDir::homePath() + "/.comma/media/0/osm/offline/" : "/data/media/0/osm/offline/"; + class OsmPanel : public QFrame { Q_OBJECT @@ -36,19 +37,20 @@ public: explicit OsmPanel(QWidget *parent = nullptr); private: - QStackedLayout* main_layout = nullptr; - QWidget* osmScreen = nullptr; + QStackedLayout *main_layout = nullptr; + QWidget *osmScreen = nullptr; Params params; - Params mem_params{ Hardware::PC() ? "": "/dev/shm/params"}; - std::map toggles; - std::optional> mapSizeFuture; + Params mem_params{Hardware::PC() ? "" : "/dev/shm/params"}; + std::map toggles; + std::optional > mapSizeFuture; const SubMaster &sm = *uiStateSP()->sm; bool is_onroad = false; std::string mapd_version; - bool isWifi() const {return sm["deviceState"].getDeviceState().getNetworkType() == cereal::DeviceState::NetworkType::WIFI; } - bool isMetered() const {return sm["deviceState"].getDeviceState().getNetworkMetered(); } + + bool isWifi() const { return sm["deviceState"].getDeviceState().getNetworkType() == cereal::DeviceState::NetworkType::WIFI; } + bool isMetered() const { return sm["deviceState"].getDeviceState().getNetworkMetered(); } bool osm_download_in_progress = false; bool download_failed_state = false; quint64 mapsDirSize = 0; @@ -58,30 +60,32 @@ private: ButtonControlSP *osmUpdateBtn; ButtonControlSP *usStatesBtn; ButtonControlSP *osmDeleteMapsBtn; + ButtonControlSP *setupOsmDeleteMapsButton(QWidget *parent);; - ButtonControlSP* setupOsmUpdateButton(QWidget *parent); - ButtonControlSP* setupOsmDownloadButton(QWidget *parent); - ButtonControlSP* setupUsStatesButton(QWidget *parent); + ButtonControlSP *setupOsmUpdateButton(QWidget *parent); + ButtonControlSP *setupOsmDownloadButton(QWidget *parent); + ButtonControlSP *setupUsStatesButton(QWidget *parent); + QTimer *timer; std::string osm_download_locations; -// void updateButtonControlSP(ButtonControlSP *btnControl, QWidget *parent, const QString &initTitle, const QString &allStatesOption); + // void updateButtonControlSP(ButtonControlSP *btnControl, QWidget *parent, const QString &initTitle, const QString &allStatesOption); void showEvent(QShowEvent *event) override; - void hideEvent(QHideEvent* event) override; + void hideEvent(QHideEvent *event) override; void updateLabels(); void updateDownloadProgress(); - static int extractIntFromJson(const QJsonObject& json, const QString& key); - QString processUpdateStatus(bool pending_update_check, int total_files, int downloaded_files, const QJsonObject& json, bool failed_state); + static int extractIntFromJson(const QJsonObject &json, const QString &key); + QString processUpdateStatus(bool pending_update_check, int total_files, int downloaded_files, const QJsonObject &json, bool failed_state); - ConfirmationDialog* confirmationDialog; + ConfirmationDialog *confirmationDialog; LabelControlSP *mapdVersion; LabelControlSP *offlineMapsStatus; LabelControlSP *offlineMapsETA; LabelControlSP *offlineMapsElapsed; std::optional lastDownloadedTimePoint; LocationsFetcher locationsFetcher; - void updateMapSize(); + void updateMapSize(); bool showConfirmationDialog(QWidget *parent, const QString &message = QString(), @@ -89,7 +93,7 @@ private: const auto _is_metered = isMetered(); const QString warning_message = _is_metered ? tr("\n\nWarning: You are on a metered connection!") : QString(); QString final_message = message.isEmpty() ? tr("This will start the download process and it might take a while to complete.") : message; - final_message += warning_message; // Append the warning message if the connection is metered + final_message += warning_message; // Append the warning message if the connection is metered const QString final_buttonText = confirmButtonText.isEmpty() ? (_is_metered ? tr("Continue on Metered") : tr("Start Download")) : confirmButtonText; @@ -97,10 +101,11 @@ private: } // Refactored methods - std::vector> getOsmLocations(const std::tuple& customLocation = defaultLocation) const { + std::vector > getOsmLocations(const std::tuple &customLocation = defaultLocation) const { return locationsFetcher.getOsmLocations(customLocation); } - std::vector> getUsStatesLocations(const std::tuple& customLocation = defaultLocation) const { + + std::vector > getUsStatesLocations(const std::tuple &customLocation = defaultLocation) const { return locationsFetcher.getUsStatesLocations(customLocation); } @@ -181,7 +186,6 @@ private: } static QString formatDownloadStatus(const QJsonObject &json) { - if (!json.contains("total_files") || !json.contains("downloaded_files")) return ""; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc index 342ae0051d..12d7bd74f7 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/settings.cc @@ -21,6 +21,7 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/osm_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/trips_panel.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle_panel.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h" TogglesPanelSP::TogglesPanelSP(SettingsWindowSP *parent) : TogglesPanel(parent) { QObject::connect(uiStateSP(), &UIStateSP::uiUpdate, this, &TogglesPanelSP::updateState); @@ -84,6 +85,7 @@ SettingsWindowSP::SettingsWindowSP(QWidget *parent) : SettingsWindow(parent) { PanelInfo(" " + tr("Models"), new ModelsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_models.png"), PanelInfo(" " + tr("Steering"), new LateralPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_lateral.png"), PanelInfo(" " + tr("Cruise"), new LongitudinalPanel(this), "../assets/icons/speed_limit.png"), + PanelInfo(" " + tr("Visuals"), new VisualsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_visuals.png"), PanelInfo(" " + tr("OSM"), new OsmPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_map.png"), PanelInfo(" " + tr("Trips"), new TripsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_trips.png"), PanelInfo(" " + tr("Vehicle"), new VehiclePanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_vehicle.png"), diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc new file mode 100644 index 0000000000..cf9729be18 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc @@ -0,0 +1,12 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h" + +VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) { + +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h new file mode 100644 index 0000000000..9f58104c4e --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h @@ -0,0 +1,18 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#pragma once + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" + +class VisualsPanel : public QWidget { + Q_OBJECT + +public: + explicit VisualsPanel(QWidget *parent = nullptr); + +}; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/model.h b/selfdrive/ui/sunnypilot/qt/onroad/model.h index e8594629cb..8569e58f66 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/model.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/model.h @@ -11,5 +11,5 @@ class ModelRendererSP : public ModelRenderer { public: - ModelRendererSP() {} + ModelRendererSP() = default; }; diff --git a/selfdrive/ui/sunnypilot/ui.h b/selfdrive/ui/sunnypilot/ui.h index 08e1d37b0f..a94dfca3b3 100644 --- a/selfdrive/ui/sunnypilot/ui.h +++ b/selfdrive/ui/sunnypilot/ui.h @@ -20,6 +20,11 @@ class UIStateSP : public UIState { public: UIStateSP(QObject *parent = 0); void updateStatus() override; + inline bool engaged() const override { + return scene.started && ( + (*sm)["selfdriveState"].getSelfdriveState().getEnabled() || (*sm)["selfdriveStateSP"].getSelfdriveStateSP().getMads().getEnabled() + ); + } void setSunnylinkRoles(const std::vector &roles); void setSunnylinkDeviceUsers(const std::vector &users); diff --git a/selfdrive/ui/tests/test_ui/run.py b/selfdrive/ui/tests/test_ui/run.py index f8c64de769..536917a1a6 100755 --- a/selfdrive/ui/tests/test_ui/run.py +++ b/selfdrive/ui/tests/test_ui/run.py @@ -26,7 +26,7 @@ from openpilot.tools.lib.framereader import FrameReader from openpilot.tools.lib.route import Route from openpilot.tools.lib.cache import DEFAULT_CACHE_DIR -UI_DELAY = 0.1 # may be slower on CI? +UI_DELAY = 0.5 # may be slower on CI? TEST_ROUTE = "a2a0ccea32023010|2023-07-27--13-01-19" STREAMS: list[tuple[VisionStreamType, CameraConfig, bytes]] = [] @@ -252,6 +252,12 @@ def setup_settings_driving(click, pm: PubMaster, scroll=None): click(278, 962) time.sleep(UI_DELAY) +def setup_settings_visuals(click, pm: PubMaster, scroll=None): + setup_settings_device(click, pm) + scroll(-400, 278, 962) + click(278, 560) + time.sleep(UI_DELAY) + def setup_settings_trips(click, pm: PubMaster, scroll=None): setup_settings_device(click, pm) scroll(-400, 278, 962) @@ -307,6 +313,7 @@ CASES.update({ "settings_steering_mads": setup_settings_steering_mads, "settings_steering_alc": setup_settings_steering_alc, "settings_driving": setup_settings_driving, + "settings_visuals": setup_settings_visuals, "settings_trips": setup_settings_trips, "settings_vehicle": setup_settings_vehicle, }) diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts index 2aba1e4e9f..de24097d8f 100644 --- a/selfdrive/ui/translations/main_ar.ts +++ b/selfdrive/ui/translations/main_ar.ts @@ -1397,6 +1397,10 @@ This may take up to a minute. Cruise + + Visuals + + Setup diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index e5d90381e1..6229920121 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -1377,6 +1377,10 @@ Dies kann bis zu einer Minute dauern. Developer Entwickler + + Visuals + + Setup diff --git a/selfdrive/ui/translations/main_es.ts b/selfdrive/ui/translations/main_es.ts index edc2295bc7..3d9622166a 100644 --- a/selfdrive/ui/translations/main_es.ts +++ b/selfdrive/ui/translations/main_es.ts @@ -1377,6 +1377,10 @@ Esto puede tardar un minuto. Cruise + + Visuals + + Setup diff --git a/selfdrive/ui/translations/main_fr.ts b/selfdrive/ui/translations/main_fr.ts index 5f7c7502cf..741cc606a7 100644 --- a/selfdrive/ui/translations/main_fr.ts +++ b/selfdrive/ui/translations/main_fr.ts @@ -1377,6 +1377,10 @@ Cela peut prendre jusqu'à une minute. Cruise + + Visuals + + Setup diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index d4b3f3e742..f85ee37689 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -1372,6 +1372,10 @@ This may take up to a minute. Cruise + + Visuals + + Setup diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index e5d139e83e..89f30ea545 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -1372,6 +1372,10 @@ This may take up to a minute. Cruise + + Visuals + + Setup diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index ef4b4c1ee3..bc9e50ee0a 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -1377,6 +1377,10 @@ Isso pode levar até um minuto. Cruise + + Visuals + + Setup diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index 3f6a199a83..2c3d0bdb6c 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -1372,6 +1372,10 @@ This may take up to a minute. Cruise + + Visuals + + Setup diff --git a/selfdrive/ui/translations/main_tr.ts b/selfdrive/ui/translations/main_tr.ts index b20e38fbf7..fd06e81ef1 100644 --- a/selfdrive/ui/translations/main_tr.ts +++ b/selfdrive/ui/translations/main_tr.ts @@ -1370,6 +1370,10 @@ This may take up to a minute. Cruise + + Visuals + + Setup diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index 0c4029c7fe..06c6c67152 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -1372,6 +1372,10 @@ This may take up to a minute. Cruise + + Visuals + + Setup diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 116e7b0725..c46157e78e 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -1372,6 +1372,10 @@ This may take up to a minute. Cruise + + Visuals + + Setup diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 6ef4f8e6d9..4ead8aecb9 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -72,7 +72,7 @@ class UIState : public QObject { public: UIState(QObject* parent = 0); virtual void updateStatus(); - inline bool engaged() const { + virtual inline bool engaged() const { return scene.started && (*sm)["selfdriveState"].getSelfdriveState().getEnabled(); } diff --git a/selfdrive/ui/ui.py b/selfdrive/ui/ui.py new file mode 100755 index 0000000000..8a3e34466f --- /dev/null +++ b/selfdrive/ui/ui.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python3 +import pyray as rl +from openpilot.system.ui.lib.application import gui_app +from openpilot.selfdrive.ui.layouts.main import MainLayout +from openpilot.selfdrive.ui.ui_state import ui_state + + +def main(): + gui_app.init_window("UI") + main_layout = MainLayout() + for _ in gui_app.render(): + ui_state.update() + + #TODO handle brigntness and awake state here + + main_layout.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py new file mode 100644 index 0000000000..d9a8c40597 --- /dev/null +++ b/selfdrive/ui/ui_state.py @@ -0,0 +1,129 @@ +import pyray as rl +from enum import Enum +from cereal import messaging, log +from openpilot.common.params import Params, UnknownKeyName + + +UI_BORDER_SIZE = 30 + + +class UIStatus(Enum): + DISENGAGED = "disengaged" + ENGAGED = "engaged" + OVERRIDE = "override" + + +class UIState: + _instance: 'UIState | None' = None + + def __new__(cls): + if cls._instance is None: + cls._instance = super().__new__(cls) + cls._instance._initialize() + return cls._instance + + def _initialize(self): + self.params = Params() + self.sm = messaging.SubMaster( + [ + "modelV2", + "controlsState", + "liveCalibration", + "radarState", + "deviceState", + "pandaStates", + "carParams", + "driverMonitoringState", + "carState", + "driverStateV2", + "roadCameraState", + "wideRoadCameraState", + "managerState", + "selfdriveState", + "longitudinalPlan", + ] + ) + + # UI Status tracking + self.status: UIStatus = UIStatus.DISENGAGED + self.started_frame: int = 0 + self._engaged_prev: bool = False + self._started_prev: bool = False + + # Core state variables + self.is_metric: bool = self.params.get_bool("IsMetric") + self.started: bool = False + self.ignition: bool = False + self.panda_type: log.PandaState.PandaType = log.PandaState.PandaType.unknown + self.personality: log.LongitudinalPersonality = log.LongitudinalPersonality.standard + self.light_sensor: float = -1.0 + + self._update_params() + + @property + def engaged(self) -> bool: + return self.started and self.sm["selfdriveState"].enabled + + def update(self) -> None: + self.sm.update(0) + self._update_state() + self._update_status() + + def _update_state(self) -> None: + # Handle panda states updates + if self.sm.updated["pandaStates"]: + panda_states = self.sm["pandaStates"] + + if len(panda_states) > 0: + # Get panda type from first panda + self.panda_type = panda_states[0].pandaType + # Check ignition status across all pandas + if self.panda_type != log.PandaState.PandaType.unknown: + self.ignition = any(state.ignitionLine or state.ignitionCan for state in panda_states) + elif self.sm.frame - self.sm.recv_frame["pandaStates"] > 5 * rl.get_fps(): + self.panda_type = log.PandaState.PandaType.unknown + + # Handle wide road camera state updates + if self.sm.updated["wideRoadCameraState"]: + cam_state = self.sm["wideRoadCameraState"] + + # Scale factor based on sensor type + scale = 6.0 if cam_state.sensor == 'ar0231' else 1.0 + self.light_sensor = max(100.0 - scale * cam_state.exposureValPercent, 0.0) + elif not self.sm.alive["wideRoadCameraState"] or not self.sm.valid["wideRoadCameraState"]: + self.light_sensor = -1 + + # Update started state + self.started = self.sm["deviceState"].started and self.ignition + + def _update_status(self) -> None: + if self.started and self.sm.updated["selfdriveState"]: + ss = self.sm["selfdriveState"] + state = ss.state + + if state in (log.SelfdriveState.OpenpilotState.preEnabled, log.SelfdriveState.OpenpilotState.overriding): + self.status = UIStatus.OVERRIDE + else: + self.status = UIStatus.ENGAGED if ss.enabled else UIStatus.DISENGAGED + + # Check for engagement state changes + if self.engaged != self._engaged_prev: + self._engaged_prev = self.engaged + + # Handle onroad/offroad transition + if self.started != self._started_prev or self.sm.frame == 1: + if self.started: + self.status = UIStatus.DISENGAGED + self.started_frame = self.sm.frame + + self._started_prev = self.started + + def _update_params(self) -> None: + try: + self.is_metric = self.params.get_bool("IsMetric") + except UnknownKeyName: + self.is_metric = False + + +# Global instance +ui_state = UIState() diff --git a/sunnypilot/mapd/mapd_installer.py b/sunnypilot/mapd/mapd_installer.py index 32e1d3cde6..f91da376b3 100755 --- a/sunnypilot/mapd/mapd_installer.py +++ b/sunnypilot/mapd/mapd_installer.py @@ -84,7 +84,7 @@ class MapdInstallManager: def wait_for_internet_connection(self, return_on_failure=False): max_retries = 10 - for retries in range(max_retries+1): + for retries in range(max_retries + 1): self._spinner.update(f"Waiting for internet connection... [{retries}/{max_retries}]") time.sleep(2) try: diff --git a/sunnypilot/mapd/mapd_manager.py b/sunnypilot/mapd/mapd_manager.py index 5b1171e110..afd632cff7 100644 --- a/sunnypilot/mapd/mapd_manager.py +++ b/sunnypilot/mapd/mapd_manager.py @@ -147,6 +147,14 @@ def main_thread(sm=None, pm=None): rk = Ratekeeper(1, print_delay_threshold=None) live_map_sp = OsmMapData() + # Create folder needed for OSM + try: + os.mkdir(COMMON_DIR) + except FileExistsError: + pass + except PermissionError: + cloudlog.exception(f"mapd: failed to make {COMMON_DIR}") + while True: show_alert = get_files_for_cleanup() and params.get_bool("OsmLocal") set_offroad_alert("Offroad_OSMUpdateRequired", show_alert, "This alert will be cleared when new maps are downloaded.") diff --git a/sunnypilot/navd/helpers.py b/sunnypilot/navd/helpers.py index f3fb38723a..85894fb2ab 100644 --- a/sunnypilot/navd/helpers.py +++ b/sunnypilot/navd/helpers.py @@ -13,9 +13,9 @@ MODIFIABLE_DIRECTIONS = ('left', 'right') EARTH_MEAN_RADIUS = 6371007.2 SPEED_CONVERSIONS = { - 'km/h': Conversions.KPH_TO_MS, - 'mph': Conversions.MPH_TO_MS, - } + 'km/h': Conversions.KPH_TO_MS, + 'mph': Conversions.MPH_TO_MS, +} class Coordinate: @@ -65,9 +65,9 @@ class Coordinate: haversine_dlon *= haversine_dlon y = haversine_dlat \ - + math.cos(math.radians(self.latitude)) \ - * math.cos(math.radians(other.latitude)) \ - * haversine_dlon + + math.cos(math.radians(self.latitude)) \ + * math.cos(math.radians(other.latitude)) \ + * haversine_dlon x = 2 * math.asin(math.sqrt(y)) return x * EARTH_MEAN_RADIUS diff --git a/sunnypilot/selfdrive/assets/offroad/icon_visuals.png b/sunnypilot/selfdrive/assets/offroad/icon_visuals.png new file mode 100644 index 0000000000..3cc9c3b145 --- /dev/null +++ b/sunnypilot/selfdrive/assets/offroad/icon_visuals.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3ecaac6fb687fcab3300d1fe2dfea96fb49734d634a663e2c02ee0b26ebd773e +size 20632 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py index e184f6d20d..f2c704f606 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py @@ -194,7 +194,7 @@ class SpeedLimitController: """ Make state transition from inactive state """ if self._engage_type == Engage.user_confirm: if (((self._last_op_enabled_time + 7.) >= self._current_time >= (self._last_op_enabled_time + 2.)) or - self._speed_limit_changed): + self._speed_limit_changed): if self._speed_limit_changed: self._last_op_enabled_time = self._current_time - 2. # immediately prompt confirmation self.state = SpeedLimitControlState.preActive @@ -253,7 +253,7 @@ class SpeedLimitController: # In any case, if op is disabled, or speed limit control is disabled # or the reported speed limit is 0 or gas is pressed, deactivate. if not self._op_enabled or not self._is_enabled or self._speed_limit == 0 or \ - (self._gas_pressed and self._disengage_on_accelerator): + (self._gas_pressed and self._disengage_on_accelerator): self.state = SpeedLimitControlState.inactive return @@ -261,7 +261,7 @@ class SpeedLimitController: # Ignore if a minimum amount of time has not passed since activation. This is to prevent temp inactivations # due to controlsd logic changing cruise setpoint when going active. if self._engage_type == Engage.auto and self._v_cruise_setpoint_changed and \ - self._current_time > (self._last_op_enabled_time + TEMP_INACTIVE_GUARD_PERIOD): + self._current_time > (self._last_op_enabled_time + TEMP_INACTIVE_GUARD_PERIOD): self.state = SpeedLimitControlState.tempInactive return @@ -277,7 +277,7 @@ class SpeedLimitController: def get_adapting_state_target_acceleration(self) -> float: """ In adapting state, calculate target acceleration based on speed limit and current velocity """ if self.distance > 0: - return (self.speed_limit_offseted**2 - self._v_ego**2) / (2. * self.distance) + return (self.speed_limit_offseted ** 2 - self._v_ego ** 2) / (2. * self.distance) return self._v_offset / ModelConstants.T_IDXS[CONTROL_N] @@ -287,13 +287,13 @@ class SpeedLimitController: def _update_events(self, events_sp: EventsSP) -> None: if self._speed_limit > 0 and self._warning_type == 2 and \ - self._speed_limit_warning_offsetted_rounded < int(round(self._v_ego * self._ms_to_local)): + self._speed_limit_warning_offsetted_rounded < int(round(self._v_ego * self._ms_to_local)): events_sp.add(EventNameSP.speedLimitPreActive) if not self.is_active: if self._state == SpeedLimitControlState.preActive and self._state_prev != SpeedLimitControlState.preActive and \ - self._v_cruise_rounded != self._speed_limit_offsetted_rounded: - events_sp.add(EventNameSP.speedLimitPreActive) + self._v_cruise_rounded != self._speed_limit_offsetted_rounded: + events_sp.add(EventNameSP.speedLimitPreActive) else: if self._engage_type == Engage.user_confirm: if self._state_prev == SpeedLimitControlState.preActive: diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py index 1dec697d9e..d04b8ae923 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py @@ -85,7 +85,7 @@ class SpeedLimitResolver: if 0. < next_speed_limit < self._v_ego: adapt_time = (next_speed_limit - self._v_ego) / LIMIT_ADAPT_ACC - adapt_distance = self._v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time**2 + adapt_distance = self._v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2 if distance_to_speed_limit_ahead <= adapt_distance: self._limit_solutions[Source.map_data] = next_speed_limit diff --git a/system/manager/process.py b/system/manager/process.py index 6c233c21ab..7f8044461d 100644 --- a/system/manager/process.py +++ b/system/manager/process.py @@ -96,7 +96,6 @@ class ManagerProcess(ABC): try: fn = WATCHDOG_FN + str(self.proc.pid) with open(fn, "rb") as f: - # TODO: why can't pylint find struct.unpack? self.last_watchdog_time = struct.unpack('Q', f.read())[0] except Exception: pass diff --git a/system/ui/README.md b/system/ui/README.md index c71b77ab66..85d32bfd6c 100644 --- a/system/ui/README.md +++ b/system/ui/README.md @@ -3,7 +3,7 @@ The user interfaces here are built with [raylib](https://www.raylib.com/). Quick start: -* set `DEBUG_FPS=1` to show the FPS +* set `SHOW_FPS=1` to show the FPS * set `STRICT_MODE=1` to kill the app if it drops too much below 60fps * set `SCALE=1.5` to scale the entire UI by 1.5x * https://www.raylib.com/cheatsheet/cheatsheet.html diff --git a/system/ui/lib/application.py b/system/ui/lib/application.py index 9bb56fec8d..c2d6ac8e2c 100644 --- a/system/ui/lib/application.py +++ b/system/ui/lib/application.py @@ -13,7 +13,7 @@ FPS_DROP_THRESHOLD = 0.9 # FPS drop threshold for triggering a warning FPS_CRITICAL_THRESHOLD = 0.5 # Critical threshold for triggering strict actions ENABLE_VSYNC = os.getenv("ENABLE_VSYNC") == "1" -DEBUG_FPS = os.getenv("DEBUG_FPS") == '1' +SHOW_FPS = os.getenv("SHOW_FPS") == '1' STRICT_MODE = os.getenv("STRICT_MODE") == '1' SCALE = float(os.getenv("SCALE", "1.0")) @@ -158,7 +158,7 @@ class GuiApplication: dst_rect = rl.Rectangle(0, 0, float(self._scaled_width), float(self._scaled_height)) rl.draw_texture_pro(self._render_texture.texture, src_rect, dst_rect, rl.Vector2(0, 0), 0.0, rl.WHITE) - if DEBUG_FPS: + if SHOW_FPS: rl.draw_fps(10, 10) rl.end_drawing() @@ -192,10 +192,12 @@ class GuiApplication: # Create a character set from our keyboard layouts from openpilot.system.ui.widgets.keyboard import KEYBOARD_LAYOUTS + from openpilot.selfdrive.ui.onroad.hud_renderer import CRUISE_DISABLED_CHAR all_chars = set() for layout in KEYBOARD_LAYOUTS.values(): all_chars.update(key for row in layout for key in row) all_chars = "".join(all_chars) + all_chars += CRUISE_DISABLED_CHAR codepoint_count = rl.ffi.new("int *", 1) codepoints = rl.load_codepoints(all_chars, codepoint_count) diff --git a/system/ui/lib/label.py b/system/ui/lib/label.py index 5244c6baf2..00a4eda7c6 100644 --- a/system/ui/lib/label.py +++ b/system/ui/lib/label.py @@ -56,7 +56,8 @@ def gui_text_box( font_size: int = DEFAULT_TEXT_SIZE, color: rl.Color = DEFAULT_TEXT_COLOR, alignment: int = rl.GuiTextAlignment.TEXT_ALIGN_LEFT, - alignment_vertical: int = rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP + alignment_vertical: int = rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP, + font_weight: FontWeight = FontWeight.NORMAL, ): styles = [ (rl.GuiControl.DEFAULT, rl.GuiControlProperty.TEXT_COLOR_NORMAL, rl.color_to_int(color)), @@ -66,6 +67,12 @@ def gui_text_box( (rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.TEXT_ALIGNMENT_VERTICAL, alignment_vertical), (rl.GuiControl.DEFAULT, rl.GuiDefaultProperty.TEXT_WRAP_MODE, rl.GuiTextWrapMode.TEXT_WRAP_WORD) ] + if font_weight != FontWeight.NORMAL: + rl.gui_set_font(gui_app.font(font_weight)) with GuiStyleContext(styles): rl.gui_label(rect, text) + + if font_weight != FontWeight.NORMAL: + rl.gui_set_font(gui_app.font(FontWeight.NORMAL)) + diff --git a/system/ui/lib/shader_polygon.py b/system/ui/lib/shader_polygon.py index 9ecaa926bc..94190abc36 100644 --- a/system/ui/lib/shader_polygon.py +++ b/system/ui/lib/shader_polygon.py @@ -16,13 +16,12 @@ uniform int pointCount; uniform vec4 fillColor; uniform vec2 resolution; -uniform bool useGradient; +uniform int useGradient; uniform vec2 gradientStart; uniform vec2 gradientEnd; uniform vec4 gradientColors[15]; uniform float gradientStops[15]; uniform int gradientColorCount; -uniform vec2 visibleGradientRange; vec4 getGradientColor(vec2 pos) { vec2 gradientDir = gradientEnd - gradientStart; @@ -30,22 +29,9 @@ vec4 getGradientColor(vec2 pos) { if (gradientLength < 0.001) return gradientColors[0]; vec2 normalizedDir = gradientDir / gradientLength; - vec2 pointVec = pos - gradientStart; - float projection = dot(pointVec, normalizedDir); + float t = clamp(dot(pos - gradientStart, normalizedDir) / gradientLength, 0.0, 1.0); - float t = projection / gradientLength; - - // Gradient clipping: remap t to visible range - float visibleStart = visibleGradientRange.x; - float visibleEnd = visibleGradientRange.y; - float visibleRange = visibleEnd - visibleStart; - - // Remap t to visible range - if (visibleRange > 0.001) { - t = visibleStart + t * visibleRange; - } - - t = clamp(t, 0.0, 1.0); + if (gradientColorCount <= 1) return gradientColors[0]; for (int i = 0; i < gradientColorCount - 1; i++) { if (t >= gradientStops[i] && t <= gradientStops[i+1]) { float segmentT = (t - gradientStops[i]) / (gradientStops[i+1] - gradientStops[i]); @@ -58,16 +44,11 @@ vec4 getGradientColor(vec2 pos) { bool isPointInsidePolygon(vec2 p) { if (pointCount < 3) return false; - int crossings = 0; for (int i = 0, j = pointCount - 1; i < pointCount; j = i++) { vec2 pi = points[i]; vec2 pj = points[j]; - - // Skip degenerate edges if (distance(pi, pj) < 0.001) continue; - - // Ray-casting if (((pi.y > p.y) != (pj.y > p.y)) && (p.x < (pj.x - pi.x) * (p.y - pi.y) / (pj.y - pi.y + 0.001) + pi.x)) { crossings++; @@ -104,24 +85,24 @@ float distanceToEdge(vec2 p) { return minDist; } -float signedDistanceToPolygon(vec2 p) { - float dist = distanceToEdge(p); - bool inside = isPointInsidePolygon(p); - return inside ? dist : -dist; -} - void main() { vec2 pixel = fragTexCoord * resolution; - float signedDist = signedDistanceToPolygon(pixel); - + // Compute pixel size for anti-aliasing vec2 pixelGrad = vec2(dFdx(pixel.x), dFdy(pixel.y)); float pixelSize = length(pixelGrad); - float aaWidth = max(0.5, pixelSize * 0.5); // Sharper anti-aliasing + float aaWidth = max(0.5, pixelSize * 1.5); - float alpha = smoothstep(-aaWidth, aaWidth, signedDist); - if (alpha > 0.0) { - vec4 color = useGradient ? getGradientColor(fragTexCoord) : fillColor; + bool inside = isPointInsidePolygon(pixel); + if (inside) { + finalColor = useGradient == 1 ? getGradientColor(pixel) : fillColor; + return; + } + + float sd = -distanceToEdge(pixel); + float alpha = smoothstep(-aaWidth, aaWidth, sd); + if (alpha > 0.0){ + vec4 color = useGradient == 1 ? getGradientColor(pixel) : fillColor; finalColor = vec4(color.rgb, color.a * alpha); } else { finalColor = vec4(0.0); @@ -180,7 +161,6 @@ class ShaderState: 'gradientStops': None, 'gradientColorCount': None, 'mvp': None, - 'visibleGradientRange': None, } # Pre-allocated FFI objects @@ -191,7 +171,6 @@ class ShaderState: self.gradient_start_ptr = rl.ffi.new("float[]", [0.0, 0.0]) self.gradient_end_ptr = rl.ffi.new("float[]", [0.0, 0.0]) self.color_count_ptr = rl.ffi.new("int[]", [0]) - self.visible_gradient_range_ptr = rl.ffi.new("float[]", [0.0, 0.0]) self.gradient_colors_ptr = rl.ffi.new("float[]", MAX_GRADIENT_COLORS * 4) self.gradient_stops_ptr = rl.ffi.new("float[]", MAX_GRADIENT_COLORS) @@ -232,66 +211,40 @@ class ShaderState: self.initialized = False -def _configure_shader_color(state, color, gradient, rect, min_xy, max_xy): - """Configure shader uniforms for solid color or gradient rendering""" +def _configure_shader_color(state, color, gradient, clipped_rect, original_rect): use_gradient = 1 if gradient else 0 state.use_gradient_ptr[0] = use_gradient rl.set_shader_value(state.shader, state.locations['useGradient'], state.use_gradient_ptr, UNIFORM_INT) if use_gradient: - # Set gradient start/end - state.gradient_start_ptr[0:2] = gradient['start'] - state.gradient_end_ptr[0:2] = gradient['end'] + start = np.array(gradient['start']) * np.array([original_rect.width, original_rect.height]) + np.array([original_rect.x, original_rect.y]) + end = np.array(gradient['end']) * np.array([original_rect.width, original_rect.height]) + np.array([original_rect.x, original_rect.y]) + start = start - np.array([clipped_rect.x, clipped_rect.y]) + end = end - np.array([clipped_rect.x, clipped_rect.y]) + state.gradient_start_ptr[0:2] = start.astype(np.float32) + state.gradient_end_ptr[0:2] = end.astype(np.float32) rl.set_shader_value(state.shader, state.locations['gradientStart'], state.gradient_start_ptr, UNIFORM_VEC2) rl.set_shader_value(state.shader, state.locations['gradientEnd'], state.gradient_end_ptr, UNIFORM_VEC2) - # Calculate visible gradient range - width = max_xy[0] - min_xy[0] - height = max_xy[1] - min_xy[1] - - gradient_dir = (gradient['end'][0] - gradient['start'][0], gradient['end'][1] - gradient['start'][1]) - is_vertical = abs(gradient_dir[1]) > abs(gradient_dir[0]) - - visible_start = 0.0 - visible_end = 1.0 - - if is_vertical and height > 0: - visible_start = (rect.y - min_xy[1]) / height - visible_end = visible_start + rect.height / height - elif width > 0: - visible_start = (rect.x - min_xy[0]) / width - visible_end = visible_start + rect.width / width - - # Clamp visible range - visible_start = max(0.0, min(1.0, visible_start)) - visible_end = max(0.0, min(1.0, visible_end)) - - state.visible_gradient_range_ptr[0:2] = [visible_start, visible_end] - rl.set_shader_value(state.shader, state.locations['visibleGradientRange'], state.visible_gradient_range_ptr, UNIFORM_VEC2) - - # Set gradient colors colors = gradient['colors'] color_count = min(len(colors), MAX_GRADIENT_COLORS) + state.color_count_ptr[0] = color_count for i, c in enumerate(colors[:color_count]): base_idx = i * 4 state.gradient_colors_ptr[base_idx:base_idx+4] = [c.r / 255.0, c.g / 255.0, c.b / 255.0, c.a / 255.0] rl.set_shader_value_v(state.shader, state.locations['gradientColors'], state.gradient_colors_ptr, UNIFORM_VEC4, color_count) - # Set gradient stops - stops = gradient.get('stops', [i / (color_count - 1) for i in range(color_count)]) - state.gradient_stops_ptr[0:color_count] = stops[:color_count] + stops = gradient.get('stops', [i / max(1, color_count - 1) for i in range(color_count)]) + stops = np.clip(stops[:color_count], 0.0, 1.0) + state.gradient_stops_ptr[0:color_count] = stops rl.set_shader_value_v(state.shader, state.locations['gradientStops'], state.gradient_stops_ptr, UNIFORM_FLOAT, color_count) - - # Set color count - state.color_count_ptr[0] = color_count rl.set_shader_value(state.shader, state.locations['gradientColorCount'], state.color_count_ptr, UNIFORM_INT) else: - color = color or rl.WHITE # Default to white if no color provided + color = color or rl.WHITE state.fill_color_ptr[0:4] = [color.r / 255.0, color.g / 255.0, color.b / 255.0, color.a / 255.0] rl.set_shader_value(state.shader, state.locations['fillColor'], state.fill_color_ptr, UNIFORM_VEC4) - -def draw_polygon(rect: rl.Rectangle, points: np.ndarray, color=None, gradient=None): +def draw_polygon(origin_rect: rl.Rectangle, points: np.ndarray, color=None, gradient=None): """ Draw a complex polygon using shader-based even-odd fill rule @@ -317,21 +270,16 @@ def draw_polygon(rect: rl.Rectangle, points: np.ndarray, color=None, gradient=No # Find bounding box min_xy = np.min(points, axis=0) max_xy = np.max(points, axis=0) - - # Clip coordinates to rectangle - clip_x = max(rect.x, min_xy[0]) - clip_y = max(rect.y, min_xy[1]) - clip_right = min(rect.x + rect.width, max_xy[0]) - clip_bottom = min(rect.y + rect.height, max_xy[1]) + clip_x = max(origin_rect.x, min_xy[0]) + clip_y = max(origin_rect.y, min_xy[1]) + clip_right = min(origin_rect.x + origin_rect.width, max_xy[0]) + clip_bottom = min(origin_rect.y + origin_rect.height, max_xy[1]) # Check if polygon is completely off-screen if clip_x >= clip_right or clip_y >= clip_bottom: return - clipped_width = clip_right - clip_x - clipped_height = clip_bottom - clip_y - - clip_rect = rl.Rectangle(clip_x, clip_y, clipped_width, clipped_height) + clipped_rect = rl.Rectangle(clip_x, clip_y, clip_right - clip_x, clip_bottom - clip_y) # Transform points relative to the CLIPPED area transformed_points = points - np.array([clip_x, clip_y]) @@ -340,21 +288,21 @@ def draw_polygon(rect: rl.Rectangle, points: np.ndarray, color=None, gradient=No state.point_count_ptr[0] = len(transformed_points) rl.set_shader_value(state.shader, state.locations['pointCount'], state.point_count_ptr, UNIFORM_INT) - state.resolution_ptr[0:2] = [clipped_width, clipped_height] + state.resolution_ptr[0:2] = [clipped_rect.width, clipped_rect.height] rl.set_shader_value(state.shader, state.locations['resolution'], state.resolution_ptr, UNIFORM_VEC2) flat_points = np.ascontiguousarray(transformed_points.flatten().astype(np.float32)) points_ptr = rl.ffi.cast("float *", flat_points.ctypes.data) rl.set_shader_value_v(state.shader, state.locations['points'], points_ptr, UNIFORM_VEC2, len(transformed_points)) - _configure_shader_color(state, color, gradient, clip_rect, min_xy, max_xy) + _configure_shader_color(state, color, gradient, clipped_rect, origin_rect) # Render rl.begin_shader_mode(state.shader) rl.draw_texture_pro( state.white_texture, rl.Rectangle(0, 0, 2, 2), - clip_rect, + clipped_rect, rl.Vector2(0, 0), 0.0, rl.WHITE, diff --git a/system/ui/lib/text_measure.py b/system/ui/lib/text_measure.py new file mode 100644 index 0000000000..9cdd566409 --- /dev/null +++ b/system/ui/lib/text_measure.py @@ -0,0 +1,13 @@ +import pyray as rl + +_cache: dict[int, rl.Vector2] = {} + + +def measure_text_cached(font: rl.Font, text: str, font_size: int, spacing: int = 0) -> rl.Vector2: + key = hash((font.texture.id, text, font_size, spacing)) + if key in _cache: + return _cache[key] + + result = rl.measure_text_ex(font, text, font_size, spacing) + _cache[key] = result + return result diff --git a/system/ui/onroad/alert_renderer.py b/system/ui/onroad/alert_renderer.py deleted file mode 100644 index 600c721680..0000000000 --- a/system/ui/onroad/alert_renderer.py +++ /dev/null @@ -1,234 +0,0 @@ -import numpy as np -import pyray as rl -from dataclasses import dataclass -from cereal import messaging, log -from openpilot.system.ui.lib.application import gui_app, FontWeight - -# Constants -ALERT_COLORS = { - log.SelfdriveState.AlertStatus.normal: rl.Color(0, 0, 0, 150), # Black - log.SelfdriveState.AlertStatus.userPrompt: rl.Color(0xFE, 0x8C, 0x34, 100), # Orange - log.SelfdriveState.AlertStatus.critical: rl.Color(0xC9, 0x22, 0x31, 150), # Red -} - -ALERT_HEIGHTS = { - log.SelfdriveState.AlertSize.small: 271, - log.SelfdriveState.AlertSize.mid: 420, -} - -SELFDRIVE_STATE_TIMEOUT = 5 # Seconds -SELFDRIVE_UNRESPONSIVE_TIMEOUT = 10 # Seconds - - -@dataclass -class Alert: - text1: str = "" - text2: str = "" - alert_type: str = "" - size: log.SelfdriveState.AlertSize = log.SelfdriveState.AlertSize.none - status: log.SelfdriveState.AlertStatus = log.SelfdriveState.AlertStatus.normal - - def is_equal(self, other: 'Alert') -> bool: - """Check if two alerts are equal.""" - return ( - self.text1 == other.text1 - and self.text2 == other.text2 - and self.alert_type == other.alert_type - and self.size == other.size - and self.status == other.status - ) - - -class AlertRenderer: - def __init__(self): - """Initialize the alert renderer.""" - self.alert: Alert = Alert() - self.started_frame: int = 0 - self.font_regular: rl.Font = gui_app.font(FontWeight.NORMAL) - self.font_bold: rl.Font = gui_app.font(FontWeight.BOLD) - self.font_metrics_cache: dict[tuple[str, int, str], rl.Vector2] = {} - - def clear(self) -> None: - """Reset the alert to its default state.""" - self.alert = Alert() - - def update_state(self, sm: messaging.SubMaster, started_frame: int) -> None: - """Update alert state based on SubMaster data.""" - self.started_frame = started_frame - new_alert = self.get_alert(sm) - if not self.alert.is_equal(new_alert): - self.alert = new_alert - - def get_alert(self, sm: messaging.SubMaster) -> Alert: - """Generate the current alert based on selfdrive state.""" - if not sm.valid['selfdriveState']: - return Alert() - - ss = sm['selfdriveState'] - selfdrive_frame = sm.recv_frame['selfdriveState'] - alert_status = self._get_enum_value(ss.alertStatus, log.SelfdriveState.AlertStatus) - - # Return current alert if selfdrive state is recent - if selfdrive_frame >= self.started_frame: - return Alert( - text1=ss.alertText1, - text2=ss.alertText2, - alert_type=ss.alertType, - size=self._get_enum_value(ss.alertSize, log.SelfdriveState.AlertSize), - status=alert_status, - ) - - # Handle selfdrive timeout - ss_missing = (np.uint64(rl.get_time() * 1e9) - sm.recv_time['selfdriveState']) / 1e9 - if selfdrive_frame < self.started_frame: - return Alert( - text1="openpilot Unavailable", - text2="Waiting to start", - alert_type="selfdriveWaiting", - size=log.SelfdriveState.AlertSize.mid, - status=log.SelfdriveState.AlertStatus.normal, - ) - elif ss_missing > SELFDRIVE_STATE_TIMEOUT: - if ss.enabled and (ss_missing - SELFDRIVE_STATE_TIMEOUT) < SELFDRIVE_UNRESPONSIVE_TIMEOUT: - return Alert( - text1="TAKE CONTROL IMMEDIATELY", - text2="System Unresponsive", - alert_type="selfdriveUnresponsive", - size=log.SelfdriveState.AlertSize.full, - status=log.SelfdriveState.AlertStatus.critical, - ) - return Alert( - text1="System Unresponsive", - text2="Reboot Device", - alert_type="selfdriveUnresponsivePermanent", - size=log.SelfdriveState.AlertSize.mid, - status=log.SelfdriveState.AlertStatus.normal, - ) - - return Alert() - - def draw(self, rect: rl.Rectangle, sm: messaging.SubMaster) -> None: - """Render the alert within the specified rectangle.""" - self.update_state(sm, sm.recv_frame['selfdriveState']) - alert_size = self._get_enum_value(self.alert.size, log.SelfdriveState.AlertSize) - if alert_size == log.SelfdriveState.AlertSize.none: - return - - # Calculate alert rectangle - margin = 0 if alert_size == log.SelfdriveState.AlertSize.full else 40 - radius = 0 if alert_size == log.SelfdriveState.AlertSize.full else 30 - height = ALERT_HEIGHTS.get(alert_size, rect.height) - alert_rect = rl.Rectangle( - rect.x + margin, - rect.y + rect.height - height + margin, - rect.width - margin * 2, - height - margin * 2, - ) - - # Draw background - alert_status = self._get_enum_value(self.alert.status, log.SelfdriveState.AlertStatus) - color = ALERT_COLORS.get(alert_status, ALERT_COLORS[log.SelfdriveState.AlertStatus.normal]) - if alert_size != log.SelfdriveState.AlertSize.full: - roundness = radius / (min(alert_rect.width, alert_rect.height) / 2) - rl.draw_rectangle_rounded(alert_rect, roundness, 10, color) - else: - rl.draw_rectangle_rec(alert_rect, color) - - # Draw text - center_x = rect.x + rect.width / 2 - center_y = alert_rect.y + alert_rect.height / 2 - self._draw_text(alert_size, alert_rect, center_x, center_y) - - def _draw_text( - self, alert_size: log.SelfdriveState.AlertSize, alert_rect: rl.Rectangle, center_x: float, center_y: float - ) -> None: - """Draw text based on alert size.""" - if alert_size == log.SelfdriveState.AlertSize.small: - font_size = 74 - text_width = self._measure_text(self.font_bold, self.alert.text1, font_size, 'bold').x - rl.draw_text_ex( - self.font_bold, - self.alert.text1, - rl.Vector2(center_x - text_width / 2, center_y - font_size / 2), - font_size, - 0, - rl.WHITE, - ) - elif alert_size == log.SelfdriveState.AlertSize.mid: - font_size1 = 88 - text1_width = self._measure_text(self.font_bold, self.alert.text1, font_size1, 'bold').x - rl.draw_text_ex( - self.font_bold, - self.alert.text1, - rl.Vector2(center_x - text1_width / 2, center_y - 125), - font_size1, - 0, - rl.WHITE, - ) - font_size2 = 66 - text2_width = self._measure_text(self.font_regular, self.alert.text2, font_size2, 'regular').x - rl.draw_text_ex( - self.font_regular, - self.alert.text2, - rl.Vector2(center_x - text2_width / 2, center_y + 21), - font_size2, - 0, - rl.WHITE, - ) - elif alert_size == log.SelfdriveState.AlertSize.full: - is_long = len(self.alert.text1) > 15 - font_size1 = 132 if is_long else 177 - text1_y = alert_rect.y + (240 if is_long else 270) - wrapped_text1 = self._wrap_text(self.alert.text1, alert_rect.width - 100, font_size1, self.font_bold) - for i, line in enumerate(wrapped_text1): - line_width = self._measure_text(self.font_bold, line, font_size1, 'bold').x - rl.draw_text_ex( - self.font_bold, - line, - rl.Vector2(center_x - line_width / 2, text1_y + i * font_size1), - font_size1, - 0, - rl.WHITE, - ) - font_size2 = 88 - text2_y = alert_rect.y + alert_rect.height - (361 if is_long else 420) - wrapped_text2 = self._wrap_text(self.alert.text2, alert_rect.width - 100, font_size2, self.font_regular) - for i, line in enumerate(wrapped_text2): - line_width = self._measure_text(self.font_regular, line, font_size2, 'regular').x - rl.draw_text_ex( - self.font_regular, - line, - rl.Vector2(center_x - line_width / 2, text2_y + i * font_size2), - font_size2, - 0, - rl.WHITE, - ) - - def _wrap_text(self, text: str, max_width: float, font_size: int, font: rl.Font) -> list[str]: - """Wrap text to fit within max width.""" - words = text.split() - lines = [] - current_line = "" - for word in words: - test_line = f"{current_line} {word}" if current_line else word - if self._measure_text(font, test_line, font_size, 'bold' if font == self.font_bold else 'regular').x <= max_width: - current_line = test_line - else: - if current_line: - lines.append(current_line) - current_line = word - if current_line: - lines.append(current_line) - return lines - - def _measure_text(self, font: rl.Font, text: str, font_size: int, font_type: str) -> rl.Vector2: - """Measure text dimensions with caching.""" - key = (text, font_size, font_type) - if key not in self.font_metrics_cache: - self.font_metrics_cache[key] = rl.measure_text_ex(font, text, font_size, 0) - return self.font_metrics_cache[key] - - @staticmethod - def _get_enum_value(enum_value, enum_type: type): - """Safely convert capnp enum to Python enum value.""" - return enum_value.raw if hasattr(enum_value, 'raw') else enum_value diff --git a/system/ui/widgets/driver_camera_view.py b/system/ui/widgets/driver_camera_view.py deleted file mode 100644 index 174fac378d..0000000000 --- a/system/ui/widgets/driver_camera_view.py +++ /dev/null @@ -1,46 +0,0 @@ -import numpy as np -import pyray as rl -from openpilot.system.ui.widgets.cameraview import CameraView -from msgq.visionipc import VisionStreamType -from openpilot.system.ui.lib.application import gui_app - - -class DriverCameraView(CameraView): - def __init__(self, stream_type: VisionStreamType): - super().__init__("camerad", stream_type) - - def render(self, rect): - super().render(rect) - - # TODO: Add additional rendering logic - - def _calc_frame_matrix(self, rect: rl.Rectangle) -> np.ndarray: - driver_view_ratio = 2.0 - - # Get stream dimensions - if self.frame: - stream_width = self.frame.width - stream_height = self.frame.height - else: - # Default values if frame not available - stream_width = 1928 - stream_height = 1208 - - yscale = stream_height * driver_view_ratio / stream_width - xscale = yscale * rect.height / rect.width * stream_width / stream_height - - return np.array([ - [xscale, 0.0, 0.0], - [0.0, yscale, 0.0], - [0.0, 0.0, 1.0] - ]) - - -if __name__ == "__main__": - gui_app.init_window("Driver Camera View") - driver_camera_view = DriverCameraView(VisionStreamType.VISION_STREAM_DRIVER) - try: - for _ in gui_app.render(): - driver_camera_view.render(rl.Rectangle(0, 0, gui_app.width, gui_app.height)) - finally: - driver_camera_view.close() diff --git a/system/ui/widgets/network.py b/system/ui/widgets/network.py index 46274dbf7e..80f55ebf56 100644 --- a/system/ui/widgets/network.py +++ b/system/ui/widgets/network.py @@ -15,7 +15,7 @@ NM_DEVICE_STATE_NEED_AUTH = 60 MIN_PASSWORD_LENGTH = 8 MAX_PASSWORD_LENGTH = 64 ITEM_HEIGHT = 160 -ICON_SIZE = 49 +ICON_SIZE = 50 STRENGTH_ICONS = [ "icons/wifi_strength_low.png", diff --git a/tools/lib/framereader.py b/tools/lib/framereader.py index 06e2345abf..76fb6786ae 100644 --- a/tools/lib/framereader.py +++ b/tools/lib/framereader.py @@ -535,25 +535,3 @@ def FrameIterator(fn, pix_fmt, **kwargs): else: for i in range(fr.frame_count): yield fr.get(i, pix_fmt=pix_fmt)[0] - - -class NumpyFrameReader: - def __init__(self, name, w, h, cache_size): - self.name = name - self.pos = -1 - self.frames = None - self.w = w - self.h = h - self.cache_size = cache_size - - def close(self): - pass - - def get(self, num, count=1, pix_fmt="nv12"): - num -= 1 - q = num // self.cache_size - if q != self.pos: - del self.frames - self.pos = q - self.frames = np.load(f'{self.name}_{self.pos}.npy') - return [self.frames[num % self.cache_size]]