From db769226b859de253a5787290d1ac74661fb7572 Mon Sep 17 00:00:00 2001 From: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Date: Thu, 21 May 2026 04:37:41 -0400 Subject: [PATCH] BigUI WIP: The Trail We Blaze --- selfdrive/ui/layouts/main.py | 35 +- .../layouts/settings/starpilot/appearance.py | 47 +++ .../settings/starpilot/system_settings.py | 9 +- selfdrive/ui/onroad/alert_renderer.py | 3 + selfdrive/ui/onroad/driver_state.py | 7 +- selfdrive/ui/onroad/exp_button.py | 51 ++- selfdrive/ui/onroad/hud_renderer.py | 16 +- selfdrive/ui/onroad/model_renderer.py | 234 ++++++++++++- selfdrive/ui/onroad/starpilot/cem_status.py | 47 +++ selfdrive/ui/onroad/starpilot/compass.py | 74 ++++ .../ui/onroad/starpilot/csc_force_stop.py | 125 +++++++ .../ui/onroad/starpilot/developer_sidebar.py | 277 +++++++++++++++ selfdrive/ui/onroad/starpilot/path.py | 22 +- .../ui/onroad/starpilot/pause_indicators.py | 49 +++ selfdrive/ui/onroad/starpilot/pedal_icons.py | 49 +++ .../ui/onroad/starpilot/slc_speed_limit.py | 22 ++ .../onroad/starpilot/starpilot_onroad_view.py | 322 +++++++++++++++++- .../ui/onroad/starpilot/stopping_point.py | 64 ++++ selfdrive/ui/onroad/starpilot/weather_icon.py | 104 ++++++ selfdrive/ui/ui_state.py | 36 ++ 20 files changed, 1526 insertions(+), 67 deletions(-) create mode 100644 selfdrive/ui/onroad/starpilot/cem_status.py create mode 100644 selfdrive/ui/onroad/starpilot/compass.py create mode 100644 selfdrive/ui/onroad/starpilot/csc_force_stop.py create mode 100644 selfdrive/ui/onroad/starpilot/developer_sidebar.py create mode 100644 selfdrive/ui/onroad/starpilot/pause_indicators.py create mode 100644 selfdrive/ui/onroad/starpilot/pedal_icons.py create mode 100644 selfdrive/ui/onroad/starpilot/stopping_point.py create mode 100644 selfdrive/ui/onroad/starpilot/weather_icon.py diff --git a/selfdrive/ui/layouts/main.py b/selfdrive/ui/layouts/main.py index 95509e1b2..4762c8c1f 100644 --- a/selfdrive/ui/layouts/main.py +++ b/selfdrive/ui/layouts/main.py @@ -4,6 +4,7 @@ import cereal.messaging as messaging from openpilot.system.hardware import PC from openpilot.system.ui.lib.application import gui_app from openpilot.selfdrive.ui.layouts.sidebar import Sidebar, SIDEBAR_WIDTH +from openpilot.selfdrive.ui.onroad.starpilot.developer_sidebar import DeveloperSidebar from openpilot.selfdrive.ui.layouts.home import HomeLayout from openpilot.selfdrive.ui.layouts.settings.settings import SettingsLayout, PanelType from openpilot.selfdrive.ui.onroad.starpilot.starpilot_onroad_view import StarPilotOnroadView @@ -25,21 +26,20 @@ class MainLayout(Widget): self._pm = messaging.PubMaster(['bookmarkButton']) self._sidebar = Sidebar() + self._dev_sidebar = DeveloperSidebar() self._current_mode = MainState.HOME self._prev_onroad = False - # Initialize layouts self._layouts = {MainState.HOME: HomeLayout(), MainState.SETTINGS: SettingsLayout(), MainState.ONROAD: StarPilotOnroadView()} self._sidebar_rect = rl.Rectangle(0, 0, 0, 0) + self._dev_sidebar_rect = rl.Rectangle(0, 0, 0, 0) self._content_rect = rl.Rectangle(0, 0, 0, 0) - # Set callbacks self._setup_callbacks() gui_app.push_widget(self) - # Skip onboarding on desktop; keep normal flow on device. self._onboarding_window = None if not PC: self._onboarding_window = OnboardingWindow() @@ -61,20 +61,27 @@ class MainLayout(Widget): device.add_interactive_timeout_callback(self._set_mode_for_state) def _update_layout_rects(self): - self._sidebar_rect = rl.Rectangle(self._rect.x, self._rect.y, SIDEBAR_WIDTH, self._rect.height) + left_w = SIDEBAR_WIDTH if self._sidebar.is_visible else 0 + right_w = SIDEBAR_WIDTH if (self._current_mode == MainState.ONROAD and self._dev_sidebar.visible) else 0 - x_offset = SIDEBAR_WIDTH if self._sidebar.is_visible else 0 - self._content_rect = rl.Rectangle(self._rect.y + x_offset, self._rect.y, self._rect.width - x_offset, self._rect.height) + self._sidebar_rect = rl.Rectangle(self._rect.x, self._rect.y, SIDEBAR_WIDTH, self._rect.height) + self._dev_sidebar_rect = rl.Rectangle( + self._rect.x + self._rect.width - SIDEBAR_WIDTH, self._rect.y, + SIDEBAR_WIDTH, self._rect.height + ) + + self._content_rect = rl.Rectangle( + self._rect.x + left_w, self._rect.y, + self._rect.width - left_w - right_w, self._rect.height + ) def _handle_onroad_transition(self): if ui_state.started != self._prev_onroad: self._prev_onroad = ui_state.started - self._set_mode_for_state() def _set_mode_for_state(self): if ui_state.started: - # Don't hide sidebar from interactive timeout if self._current_mode != MainState.ONROAD: self._sidebar.set_visible(False) self._set_current_layout(MainState.ONROAD) @@ -105,9 +112,17 @@ class MainLayout(Widget): self._sidebar.set_visible(not self._sidebar.is_visible) def _render_main_content(self): - # Render sidebar + if self._current_mode == MainState.ONROAD: + self._dev_sidebar.update() + + self._update_layout_rects() + if self._sidebar.is_visible: self._sidebar.render(self._sidebar_rect) - content_rect = self._content_rect if self._sidebar.is_visible else self._rect + has_dev = self._current_mode == MainState.ONROAD and self._dev_sidebar.visible + content_rect = self._content_rect if (self._sidebar.is_visible or has_dev) else self._rect self._layouts[self._current_mode].render(content_rect) + + if has_dev: + self._dev_sidebar.render(self._dev_sidebar_rect) diff --git a/selfdrive/ui/layouts/settings/starpilot/appearance.py b/selfdrive/ui/layouts/settings/starpilot/appearance.py index 236d92e0a..368889336 100644 --- a/selfdrive/ui/layouts/settings/starpilot/appearance.py +++ b/selfdrive/ui/layouts/settings/starpilot/appearance.py @@ -297,6 +297,53 @@ class StarPilotAppearanceLayout(_SettingsPage): set_state=lambda s: self._params.put_bool("RotatingWheel", s)), ], tab_key="widgets", column_pair="widgets"), + SettingSection(tr_noop("Screen Borders"), [ + SettingRow("ShowSteering", "toggle", tr_noop("Steering Torque Indicator"), + subtitle="", + get_state=lambda: self._params.get_bool("ShowSteering"), + set_state=lambda s: self._params.put_bool("ShowSteering", s)), + SettingRow("SignalMetrics", "toggle", tr_noop("Turn Signal Borders"), + subtitle="", + get_state=lambda: self._params.get_bool("SignalMetrics"), + set_state=lambda s: self._params.put_bool("SignalMetrics", s)), + SettingRow("BlindSpotMetrics", "toggle", tr_noop("Blind Spot Borders"), + subtitle="", + get_state=lambda: self._params.get_bool("BlindSpotMetrics"), + set_state=lambda s: self._params.put_bool("BlindSpotMetrics", s), + visible=bsm), + ], tab_key="widgets", column_pair="widgets_extra"), + + SettingSection(tr_noop("Developer Metrics"), [ + SettingRow("RadarTracksUI", "toggle", tr_noop("Radar Point Display"), + subtitle="", + get_state=lambda: self._params.get_bool("RadarTracksUI"), + set_state=lambda s: self._params.put_bool("RadarTracksUI", s)), + SettingRow("LeadInfo", "toggle", tr_noop("Lead Vehicle Metrics"), + subtitle="", + get_state=lambda: self._params.get_bool("LeadInfo"), + set_state=lambda s: self._params.put_bool("LeadInfo", s), + visible=ol), + SettingRow("LeadDetectionProbability", "value", tr_noop("Lead Detection Threshold"), + subtitle="", + get_value=lambda: f"{self._params.get_int('LeadDetectionThreshold')}%", + on_click=lambda: self._show_int_selector("LeadDetectionProbability", 25, 100, "%"), + visible=ol), + SettingRow("ShowStoppingPoint", "toggle", tr_noop("Show Stop Sign"), + subtitle="", + get_state=lambda: self._params.get_bool("ShowStoppingPoint"), + set_state=lambda s: self._params.put_bool("ShowStoppingPoint", s), + visible=ol), + SettingRow("ShowStoppingPointMetrics", "toggle", tr_noop("Stop Distance"), + subtitle="", + get_state=lambda: self._params.get_bool("ShowStoppingPointMetrics"), + set_state=lambda s: self._params.put_bool("ShowStoppingPointMetrics", s), + visible=lambda: self._params.get_bool("ShowStoppingPoint") and ol()), + SettingRow("DeveloperSidebar", "toggle", tr_noop("Developer Sidebar"), + subtitle=tr_noop("Driving metrics panel on the right"), + get_state=lambda: self._params.get_bool("DeveloperSidebar"), + set_state=lambda s: self._params.put_bool("DeveloperSidebar", s)), + ], tab_key="widgets", column_pair="widgets_extra"), + # ═══ Tab 3: Convenience — QOL + Navigation ═══ SettingSection(tr_noop("Quality of Life"), [ SettingRow("QOLVisuals", "toggle", tr_noop("Quality of Life"), diff --git a/selfdrive/ui/layouts/settings/starpilot/system_settings.py b/selfdrive/ui/layouts/settings/starpilot/system_settings.py index 44bead83d..52c4498a6 100644 --- a/selfdrive/ui/layouts/settings/starpilot/system_settings.py +++ b/selfdrive/ui/layouts/settings/starpilot/system_settings.py @@ -252,6 +252,13 @@ class SystemSettingsManagerView(AetherInteractiveMixin, Widget): "get": lambda: self._controller._params.get_bool("DebugMode"), "set": lambda v: self._controller._params.put_bool("DebugMode", v), }, + { + "id": "ShowFPS", + "title": tr("Show FPS"), + "subtitle": tr("Display screen refresh rate and system performance metrics onroad."), + "get": lambda: self._controller._params.get_bool("ShowFPS"), + "set": lambda v: self._controller._params.put_bool("ShowFPS", v), + }, { "id": "NoUploads", "title": tr("Disable Uploads"), @@ -330,7 +337,7 @@ class SystemSettingsManagerView(AetherInteractiveMixin, Widget): { "id": "device_controls", "title": tr("Device Controls"), - "toggle_ids": ["StandbyMode", "IncreaseThermalLimits", "UseKonikServer", "DebugMode"], + "toggle_ids": ["StandbyMode", "IncreaseThermalLimits", "UseKonikServer", "DebugMode", "ShowFPS"], }, { "id": "uploads_logging", diff --git a/selfdrive/ui/onroad/alert_renderer.py b/selfdrive/ui/onroad/alert_renderer.py index a81fbfc44..2a297f7af 100644 --- a/selfdrive/ui/onroad/alert_renderer.py +++ b/selfdrive/ui/onroad/alert_renderer.py @@ -107,6 +107,9 @@ class AlertRenderer(Widget): if ss.alertSize == 0: return None + if ss.alertStatus.raw == AlertStatus.normal and ui_state.starpilot_toggles.get("hide_alerts", False): + return None + # Don't get old alert if recv_frame < ui_state.started_frame: return None diff --git a/selfdrive/ui/onroad/driver_state.py b/selfdrive/ui/onroad/driver_state.py index 7b3181d1a..a42082b48 100644 --- a/selfdrive/ui/onroad/driver_state.py +++ b/selfdrive/ui/onroad/driver_state.py @@ -64,6 +64,8 @@ class DriverStateRenderer(Widget): self.h_arc_data = None self.v_arc_data = None + self.x_shift: float = 0.0 + # 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(ARC_POINT_COUNT)] @@ -79,7 +81,8 @@ class DriverStateRenderer(Widget): self.disengaged_color = rl.Color(139, 139, 139, 255) self.set_visible(lambda: (ui_state.sm["selfdriveState"].alertSize == AlertSize.none and - ui_state.sm.recv_frame["driverStateV2"] > ui_state.started_frame)) + ui_state.sm.recv_frame["driverStateV2"] > ui_state.started_frame and + not ui_state.starpilot_toggles.get("hide_dm_icon", False))) def _render(self, rect): # Set opacity based on active state @@ -165,7 +168,7 @@ class DriverStateRenderer(Widget): # Calculate icon position (bottom-left or bottom-right) width, height = self._rect.width, self._rect.height offset = UI_BORDER_SIZE + BTN_SIZE // 2 - self.position_x = self._rect.x + (width - offset if self.is_rhd else offset) + self.position_x = self._rect.x + (width - offset if self.is_rhd else offset) + self.x_shift self.position_y = self._rect.y + height - offset # Pre-calculate the face lines positions diff --git a/selfdrive/ui/onroad/exp_button.py b/selfdrive/ui/onroad/exp_button.py index a3047e280..d2476c2b3 100644 --- a/selfdrive/ui/onroad/exp_button.py +++ b/selfdrive/ui/onroad/exp_button.py @@ -4,6 +4,7 @@ from openpilot.common.params import Params from openpilot.selfdrive.ui.ui_state import ui_state from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.widgets import Widget +from openpilot.common.filter_simple import FirstOrderFilter from openpilot.starpilot.common.experimental_state import ( CEStatus, next_manual_ce_status, @@ -29,13 +30,49 @@ class ExpButton(Widget): self._txt_exp: rl.Texture = gui_app.texture('icons/experimental.png', icon_size, icon_size) self._rect = rl.Rectangle(0, 0, button_size, button_size) + self._steer_angle_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) + self._bg_colors = { + "disengaged": rl.Color(0, 0, 0, 166), + "switchback": rl.Color(0x8b, 0x6c, 0xc5, 255), + "aol": rl.Color(0x0a, 0xba, 0xb5, 255), + "cem_disabled": rl.Color(0xff, 0xff, 0x00, 255), + "experimental": rl.Color(0xda, 0x6f, 0x25, 255), + "traffic": rl.Color(0xc9, 0x22, 0x31, 255), + } + self._bg_color = self._black_bg + + # Visibility controlled by HideSteeringWheel toggle + self.set_visible(lambda: not (ui_state.starpilot_toggles.get("hide_steering_wheel", False) or + self._params.get_bool("HideSteeringWheel"))) + def set_rect(self, rect: rl.Rectangle) -> None: self._rect.x, self._rect.y = rect.x, rect.y def _update_state(self) -> None: selfdrive_state = ui_state.sm["selfdriveState"] self._experimental_mode = selfdrive_state.experimentalMode - self._engageable = selfdrive_state.engageable or selfdrive_state.enabled + self._engageable = selfdrive_state.engageable or selfdrive_state.enabled or ui_state.always_on_lateral_active + + # Smooth steering angle for rotating wheel + car_state = ui_state.sm["carState"] + self._steer_angle_filter.update(car_state.steeringAngleDeg) + + # Determine background color based on engagement state + simple_mode = ui_state.starpilot_toggles.get("simple_mode", False) + if simple_mode or self.is_pressed or not self._engageable: + self._bg_color = self._bg_colors["disengaged"] + elif ui_state.switchback_mode_enabled: + self._bg_color = self._bg_colors["switchback"] + elif ui_state.always_on_lateral_active: + self._bg_color = self._bg_colors["aol"] + elif ui_state.conditional_status == 1: + self._bg_color = self._bg_colors["cem_disabled"] + elif self._held_or_actual_mode(): + self._bg_color = self._bg_colors["experimental"] + elif ui_state.traffic_mode_enabled: + self._bg_color = self._bg_colors["traffic"] + else: + self._bg_color = self._bg_colors["disengaged"] def _handle_mouse_release(self, _): super()._handle_mouse_release(_) @@ -62,8 +99,16 @@ class ExpButton(Widget): self._white_color.a = 180 if self.is_pressed or not self._engageable else 255 texture = self._txt_exp if self._held_or_actual_mode() else self._txt_wheel - rl.draw_circle(center_x, center_y, self._rect.width / 2, self._black_bg) - rl.draw_texture_ex(texture, rl.Vector2(center_x - texture.width / 2, center_y - texture.height / 2), 0.0, 1.0, self._white_color) + rl.draw_circle(center_x, center_y, self._rect.width / 2, self._bg_color) + + rotating_wheel = ui_state.starpilot_toggles.get("rotating_wheel", False) or self._params.get_bool("RotatingWheel") + if texture == self._txt_wheel and rotating_wheel: + source_rect = rl.Rectangle(0, 0, texture.width, texture.height) + dest_rect = rl.Rectangle(center_x, center_y, texture.width, texture.height) + origin = rl.Vector2(texture.width / 2, texture.height / 2) + rl.draw_texture_pro(texture, source_rect, dest_rect, origin, -self._steer_angle_filter.x, self._white_color) + else: + rl.draw_texture_ex(texture, rl.Vector2(center_x - texture.width / 2, center_y - texture.height / 2), 0.0, 1.0, self._white_color) def _held_or_actual_mode(self): now = time.monotonic() diff --git a/selfdrive/ui/onroad/hud_renderer.py b/selfdrive/ui/onroad/hud_renderer.py index 73df8b396..295053348 100644 --- a/selfdrive/ui/onroad/hud_renderer.py +++ b/selfdrive/ui/onroad/hud_renderer.py @@ -85,10 +85,10 @@ class HudRenderer(Widget): car_state = sm['carState'] v_cruise_cluster = car_state.vCruiseCluster - self.set_speed = ( - controls_state.deprecated.vCruise if v_cruise_cluster == 0.0 else v_cruise_cluster - ) - self.is_cruise_set = 0 < self.set_speed < SET_SPEED_NA + v_cruise = controls_state.deprecated.vCruise if v_cruise_cluster == 0.0 else v_cruise_cluster + offset = ui_state.starpilot_toggles.get("set_speed_offset", 0.0) + self.set_speed = v_cruise + offset if (0 < v_cruise < SET_SPEED_NA) else v_cruise + self.is_cruise_set = 0 < v_cruise < SET_SPEED_NA self.is_cruise_available = self.set_speed != -1 if self.is_cruise_set and not ui_state.is_metric: @@ -96,7 +96,8 @@ class HudRenderer(Widget): 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 + use_wheel_speed = ui_state.starpilot_toggles.get("use_wheel_speed", False) + v_ego = car_state.vEgo if use_wheel_speed else (v_ego_cluster if self.v_ego_cluster_seen else car_state.vEgo) 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) @@ -112,10 +113,11 @@ class HudRenderer(Widget): COLORS.HEADER_GRADIENT_END, ) - if self.is_cruise_available: + if self.is_cruise_available and not ui_state.starpilot_toggles.get("hide_max_speed", False): self._draw_set_speed(rect) - self._draw_current_speed(rect) + if not ui_state.starpilot_toggles.get("hide_speed", False): + self._draw_current_speed(rect) button_x = rect.x + rect.width - UI_CONFIG.border_size - UI_CONFIG.button_size button_y = rect.y + UI_CONFIG.border_size diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index 5c959cd8b..77db62a25 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -60,6 +60,7 @@ class ModelRenderer(Widget): self._lane_line_probs = np.zeros(4, dtype=np.float32) self._road_edge_stds = np.zeros(2, dtype=np.float32) self._lead_vehicles = [LeadVehicle(), LeadVehicle()] + self._adjacent_lead_vehicles = [LeadVehicle(), LeadVehicle()] self._path_offset_z = HEIGHT_INIT[0] # Adjacent path vertices (left, right) @@ -120,7 +121,11 @@ class ModelRenderer(Widget): model = sm['modelV2'] radar_state = sm['radarState'] if sm.valid['radarState'] else None lead_one = radar_state.leadOne if radar_state else None - render_lead_indicator = self._longitudinal_control and radar_state is not None and lead_indicator_enabled(self._params) + + # StarPilot lead indicator visibility conditions + hide_lead_marker = self._params.get_bool("HideLeadMarker") + lead_info_enabled = self._params.get_bool("LeadInfo") + render_lead_indicator = (self._longitudinal_control or lead_info_enabled) and radar_state is not None and not hide_lead_marker # Update model data when needed model_updated = sm.updated['modelV2'] @@ -135,14 +140,23 @@ class ModelRenderer(Widget): self._update_model(lead_one, path_x_array) if render_lead_indicator: self._update_leads(radar_state, path_x_array) + if sm.valid.get("starpilotRadarState", False): + self._update_adjacent_leads(sm["starpilotRadarState"], path_x_array) self._transform_dirty = False + self._lead_text_rects = [] + self._adjacent_lead_text_rects = [] + # Draw elements self._draw_lane_lines() self._draw_path(sm) if render_lead_indicator and radar_state: - self._draw_lead_indicator() + self._draw_lead_indicator(radar_state) + if sm.valid.get("starpilotRadarState", False): + self._draw_adjacent_leads() + + self._draw_radar_tracks() def _update_raw_points(self, model): """Update raw 3D points from model data""" @@ -190,15 +204,15 @@ class ModelRenderer(Widget): def _update_model(self, lead, path_x_array): """Update model visualization data based on model message""" model_ui_enabled = self._params.get_bool('ModelUI', default=True) - custom_path_width = model_ui_enabled and self._param_float_changed('PathWidth', DEFAULT_PATH_WIDTH) - custom_lane_line_width = model_ui_enabled and self._param_float_changed('LaneLinesWidth', DEFAULT_LANE_LINES_WIDTH) - custom_road_edge_width = model_ui_enabled and self._param_float_changed('RoadEdgesWidth', DEFAULT_ROAD_EDGES_WIDTH) - custom_path_edge_width = model_ui_enabled and self._param_float_changed('PathEdgeWidth', DEFAULT_PATH_EDGE_WIDTH) + custom_path_width, pw = self._param_float_changed('PathWidth', DEFAULT_PATH_WIDTH) if model_ui_enabled else (False, DEFAULT_PATH_WIDTH) + custom_lane_line_width, llw = self._param_float_changed('LaneLinesWidth', DEFAULT_LANE_LINES_WIDTH) if model_ui_enabled else (False, DEFAULT_LANE_LINES_WIDTH) + custom_road_edge_width, rew = self._param_float_changed('RoadEdgesWidth', DEFAULT_ROAD_EDGES_WIDTH) if model_ui_enabled else (False, DEFAULT_ROAD_EDGES_WIDTH) + custom_path_edge_width, pew = self._param_float_changed('PathEdgeWidth', DEFAULT_PATH_EDGE_WIDTH) if model_ui_enabled else (False, DEFAULT_PATH_EDGE_WIDTH) - path_width = self._path_width_to_half_m(self._params.get_float('PathWidth', default=DEFAULT_PATH_WIDTH)) if custom_path_width else 0.9 - lane_line_width_m = self._small_distance_to_half_m(self._params.get_float('LaneLinesWidth', default=DEFAULT_LANE_LINES_WIDTH)) if custom_lane_line_width else 0.025 - road_edge_width_m = self._small_distance_to_half_m(self._params.get_float('RoadEdgesWidth', default=DEFAULT_ROAD_EDGES_WIDTH)) if custom_road_edge_width else 0.025 - path_edge_width_pct = np.clip(self._params.get_float('PathEdgeWidth', default=DEFAULT_PATH_EDGE_WIDTH) / 100.0, 0.0, 1.0) if custom_path_edge_width else 0.0 + path_width = self._path_width_to_half_m(pw) if custom_path_width else 0.9 + lane_line_width_m = self._small_distance_to_half_m(llw) if custom_lane_line_width else 0.025 + road_edge_width_m = self._small_distance_to_half_m(rew) if custom_road_edge_width else 0.025 + path_edge_width_pct = np.clip(pew / 100.0, 0.0, 1.0) if custom_path_edge_width else 0.0 # Dynamic path width if model_ui_enabled and self._params.get_bool('DynamicPathWidth', default=False): @@ -420,15 +434,200 @@ class ModelRenderer(Widget): ) draw_polygon(self._rect, self._path.projected_points, gradient=gradient) - def _draw_lead_indicator(self): + def _draw_lead_indicator(self, radar_state): # Draw lead vehicles if available lead_color = get_theme_color("LeadMarker", rl.Color(201, 34, 49, 255)) - for lead in self._lead_vehicles: + leads = [radar_state.leadOne, radar_state.leadTwo] + + # Threshold for Lead 1 + threshold = self._params.get_int("LeadDetectionProbability") + if threshold is None or threshold == 0: + threshold = self._params.get_int("LeadDetectionThreshold") + if threshold is None or threshold == 0: + threshold = 50 + prob_threshold = threshold / 100.0 if threshold > 1.0 else threshold + + for i, lead in enumerate(self._lead_vehicles): + if not lead.glow or not lead.chevron: + continue + + # Choose color + if i == 0 and radar_state.leadOne and radar_state.leadOne.status: + if radar_state.leadOne.modelProb >= prob_threshold: + color = lead_color + else: + color = rl.WHITE + else: + color = lead_color + + rl.draw_triangle_fan(lead.glow, len(lead.glow), rl.Color(218, 202, 37, 255)) + rl.draw_triangle_fan(lead.chevron, len(lead.chevron), with_alpha(color, lead.fill_alpha)) + + # Draw metrics if enabled + lead_info_enabled = self._params.get_bool("LeadInfo") + if lead_info_enabled and i < len(leads) and leads[i] and leads[i].status: + self._draw_lead_metrics(False, lead.chevron, leads[i]) + + def _update_adjacent_leads(self, starpilot_radar_state, path_x_array): + self._adjacent_lead_vehicles = [LeadVehicle(), LeadVehicle()] + leads = [starpilot_radar_state.leadLeft, starpilot_radar_state.leadRight] + + 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(path_x_array, d_rel) + 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: + eff_d_rel = d_rel + abs(y_rel) + self._adjacent_lead_vehicles[i] = self._update_lead_vehicle(eff_d_rel, v_rel, point, self._rect) + + def _draw_adjacent_leads(self): + sm = ui_state.sm + if not sm.valid.get("starpilotRadarState", False): + return + + starpilot_radar_state = sm["starpilotRadarState"] + lead_left = starpilot_radar_state.leadLeft + lead_right = starpilot_radar_state.leadRight + + blue_color = rl.Color(0, 150, 255, 255) + purple_color = rl.Color(180, 0, 255, 255) + + leads_to_draw = [] + if lead_left and lead_left.status: + leads_to_draw.append((0, lead_left, blue_color)) + if lead_right and lead_right.status: + leads_to_draw.append((1, lead_right, purple_color)) + + for idx, lead_data, color in leads_to_draw: + lead = self._adjacent_lead_vehicles[idx] if not lead.glow or not lead.chevron: continue rl.draw_triangle_fan(lead.glow, len(lead.glow), rl.Color(218, 202, 37, 255)) - rl.draw_triangle_fan(lead.chevron, len(lead.chevron), with_alpha(lead_color, lead.fill_alpha)) + rl.draw_triangle_fan(lead.chevron, len(lead.chevron), with_alpha(color, lead.fill_alpha)) + + # Draw metrics if enabled + lead_info_enabled = self._params.get_bool("LeadInfo") + if lead_info_enabled: + self._draw_lead_metrics(True, lead.chevron, lead_data) + + def _draw_lead_metrics(self, adjacent, chevron, lead_data): + is_metric = ui_state.is_metric + use_si_metrics = ui_state.starpilot_toggles.get("UseSiMetrics", False) + + if is_metric or use_si_metrics: + lead_distance_unit = "m" + distance_conversion = 1.0 + lead_speed_unit = " m/s" if use_si_metrics else " km/h" + speed_conversion_metrics = 1.0 if use_si_metrics else CV.MS_TO_KPH + else: + lead_distance_unit = "ft" + distance_conversion = CV.METER_TO_FOOT + lead_speed_unit = " mph" + speed_conversion_metrics = CV.MS_TO_MPH + + y_rel = getattr(lead_data, "yRel", 0.0) + lead_distance = lead_data.dRel + (abs(y_rel) if adjacent else 0.0) + lead_speed = max(getattr(lead_data, "vLead", 0.0), 0.0) + + distance_string = f"{round(lead_distance * distance_conversion)}" + speed_string = f"{round(lead_speed * speed_conversion_metrics)}" + + text_lines = [] + if adjacent: + text_lines.append(f"{distance_string} {lead_distance_unit}") + text_lines.append(f"{speed_string}{lead_speed_unit}") + else: + if self._longitudinal_control: + plan = ui_state.sm["starpilotPlan"] + desired_follow_distance = float(plan.desiredFollowDistance) if plan and plan.desiredFollowDistance > 0 else 0.0 + desired_distance = max(0, round(desired_follow_distance * distance_conversion)) + text_lines.append(f"{distance_string} {lead_distance_unit} (Desired: {desired_distance})") + else: + text_lines.append(f"{distance_string} {lead_distance_unit}") + + text_lines.append(f"{speed_string}{lead_speed_unit}") + + v_ego = max(ui_state.sm["carState"].vEgo, 0.0) + time_gap = lead_distance / max(v_ego, 1.0) + text_lines.append(f"{time_gap:.2f} seconds") + + from openpilot.system.ui.lib.application import gui_app, FontWeight + from openpilot.selfdrive.ui.onroad.starpilot.path import _draw_text_with_outline + font = gui_app.font(FontWeight.SEMI_BOLD) + font_size = 24 + line_height = font_size + 2 + + max_text_width = 0.0 + for line in text_lines: + sz = rl.measure_text_ex(font, line, font_size, 0) + if sz.x > max_text_width: + max_text_width = sz.x + + centerX = chevron[1][0] + startY = max(chevron[0][1], chevron[2][1]) + line_height + 5 + + x_margin = max_text_width * 0.1 + y_margin = line_height * 0.1 + + rect_x = centerX - max_text_width / 2 - x_margin + rect_y = startY - line_height - y_margin + rect_w = max_text_width + 2 * x_margin + rect_h = len(text_lines) * line_height + 2 * y_margin + text_rect = rl.Rectangle(rect_x, rect_y, rect_w, rect_h) + + collision = False + for r in self._lead_text_rects + self._adjacent_lead_text_rects: + if rl.check_collision_recs(text_rect, r): + collision = True + break + + if collision: + return + + if adjacent: + self._adjacent_lead_text_rects.append(text_rect) + else: + self._lead_text_rects.append(text_rect) + + for i, line in enumerate(text_lines): + sz = rl.measure_text_ex(font, line, font_size, 0) + line_x = centerX - sz.x / 2 + line_y = startY + (i * line_height) + _draw_text_with_outline(line, line_x, line_y, font, font_size) + + def _draw_radar_tracks(self): + radar_tracks_enabled = self._params.get_bool("RadarTracksUI") + if not radar_tracks_enabled: + return + + sm = ui_state.sm + if not sm.valid.get("liveTracks", False): + return + + radar_points = sm["liveTracks"].points + if len(radar_points) == 0: + return + + path_x_array = self._path.raw_points[:, 0] + line_z = self._path.raw_points[:, 2] + + radius = 4.0 + red_color = rl.Color(255, 0, 0, 200) + + for point in radar_points: + d_rel = point.dRel + idx = self._get_path_length_idx(path_x_array, d_rel) + z = line_z[idx] if idx < len(line_z) else 0.0 + + calibrated_point = self._map_to_screen(d_rel, -point.yRel, z + self._path_offset_z) + if calibrated_point: + x, y = calibrated_point + x = np.clip(x, self._rect.x, self._rect.x + self._rect.width) + y = np.clip(y, self._rect.y, self._rect.y + self._rect.height) + rl.draw_circle_v(rl.Vector2(x, y), radius, red_color) def _update_adjacent_paths(self, max_idx: int, max_distance: float): """Compute adjacent lane path polygons by averaging lane line pairs.""" @@ -684,14 +883,15 @@ class ModelRenderer(Widget): return value / 2.0 return value * CV.FOOT_TO_METER / 2.0 - def _param_float_changed(self, key: str, default: float) -> bool: + def _param_float_changed(self, key: str, default: float) -> tuple[bool, float]: value = self._params.get(key, encoding="utf-8") if value in (None, ""): - return False + return False, default try: - return not np.isclose(float(value), default) + fval = float(value) + return (not np.isclose(fval, default)), fval except (TypeError, ValueError): - return False + return False, default @staticmethod def _blend_colors(begin_colors, end_colors, t): diff --git a/selfdrive/ui/onroad/starpilot/cem_status.py b/selfdrive/ui/onroad/starpilot/cem_status.py new file mode 100644 index 000000000..72d2b12c2 --- /dev/null +++ b/selfdrive/ui/onroad/starpilot/cem_status.py @@ -0,0 +1,47 @@ +import pyray as rl +from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.selfdrive.ui.lib.starpilot_status import CEM_OVERRIDE_COLOR, EXPERIMENTAL_COLOR + +def render_cem_status(rect: rl.Rectangle, font): + if not ui_state.params.get_bool("ShowCEMStatus"): + return + + experimental_mode = ui_state.sm["selfdriveState"].experimentalMode + cond_status = ui_state.conditional_status + + # Map status to text label + status_labels = { + 1: "CHILL", + 2: "EXP", + 3: "CURVE", + 4: "LEAD", + 5: "TURN", + 6: "SLOW", + 7: "FAST", + 8: "STOP", + } + + label = "CHILL" + border_color = rl.Color(0, 0, 0, 166) + + if cond_status == 1: + label = "CHILL" + border_color = CEM_OVERRIDE_COLOR # Yellow + elif experimental_mode: + label = status_labels.get(cond_status, "EXP") + border_color = EXPERIMENTAL_COLOR # Orange + else: + label = "CHILL" + border_color = rl.Color(80, 80, 80, 255) + + # Draw background + rl.draw_rectangle_rounded(rect, 0.3, 10, rl.Color(0, 0, 0, 166)) + # Draw border + rl.draw_rectangle_rounded_lines_ex(rect, 0.3, 10, 4, border_color) + + # Draw text label centered inside the badge + font_size = 20 + text_sz = rl.measure_text_ex(font, label, font_size, 0) + pos_x = rect.x + (rect.width - text_sz.x) / 2 + pos_y = rect.y + (rect.height - text_sz.y) / 2 + rl.draw_text_ex(font, label, rl.Vector2(int(pos_x), int(pos_y)), font_size, 0, rl.WHITE) diff --git a/selfdrive/ui/onroad/starpilot/compass.py b/selfdrive/ui/onroad/starpilot/compass.py new file mode 100644 index 000000000..73c42c129 --- /dev/null +++ b/selfdrive/ui/onroad/starpilot/compass.py @@ -0,0 +1,74 @@ +import pyray as rl +from openpilot.selfdrive.ui.ui_state import ui_state + +def render_compass(rect: rl.Rectangle, font): + if not ui_state.params.get_bool("Compass"): + return + + # Retrieve bearing + bearing = 0.0 + gps = ui_state.sm["gpsLocationExternal"] if ui_state.sm.valid.get("gpsLocationExternal", False) else None + if gps and gps.bearingDeg != 0: + bearing = gps.bearingDeg + else: + try: + last_gps = ui_state.params_memory.get("LastGPSPosition") + if last_gps: + import json + data = json.loads(last_gps) + bearing = data.get("bearing", 0.0) + except Exception: + pass + + # Draw background + rl.draw_rectangle_rounded(rect, 0.2, 10, rl.Color(0, 0, 0, 166)) + rl.draw_rectangle_rounded_lines_ex(rect, 0.2, 10, 4, rl.Color(0, 0, 0, 255)) + + # Clip ribbon to widget boundary + rl.begin_scissor_mode(int(rect.x + 4), int(rect.y + 4), int(rect.width - 8), int(rect.height - 8)) + + # Display range: +/- 45 degrees + range_deg = 45 + pixels_per_degree = rect.width / (range_deg * 2.0) + + start_deg = int(bearing - range_deg) + end_deg = int(bearing + range_deg) + 1 + + labels = {0: "N", 45: "NE", 90: "E", 135: "SE", 180: "S", 225: "SW", 270: "W", 315: "NW"} + + for deg in range(start_deg, end_deg): + norm_deg = (deg + 360) % 360 + offset_deg = deg - bearing + x = rect.x + rect.width / 2.0 + offset_deg * pixels_per_degree + + if rect.x <= x <= rect.x + rect.width: + if norm_deg % 45 == 0: + notch_height = 25 + notch_width = 3 + lbl = labels.get(norm_deg, "") + if lbl: + lbl_sz = rl.measure_text_ex(font, lbl, 22, 0) + rl.draw_text_ex(font, lbl, rl.Vector2(int(x - lbl_sz.x / 2), int(rect.y + 12)), 22, 0, rl.WHITE) + elif norm_deg % 15 == 0: + notch_height = 15 + notch_width = 2 + elif norm_deg % 5 == 0: + notch_height = 8 + notch_width = 1 + else: + continue + + y_start = rect.y + rect.height - notch_height - 10 + y_end = rect.y + rect.height - 10 + rl.draw_line_ex(rl.Vector2(int(x), int(y_start)), rl.Vector2(int(x), int(y_end)), notch_width, rl.WHITE) + + rl.end_scissor_mode() + + # Draw static triangular pointer pointing UP at bottom center + triangle_size = 12 + tx = rect.x + rect.width / 2 + ty = rect.y + rect.height - 12 + v1 = rl.Vector2(int(tx), int(ty - triangle_size)) + v2 = rl.Vector2(int(tx - triangle_size / 1.5), int(ty)) + v3 = rl.Vector2(int(tx + triangle_size / 1.5), int(ty)) + rl.draw_triangle(v1, v2, v3, rl.WHITE) diff --git a/selfdrive/ui/onroad/starpilot/csc_force_stop.py b/selfdrive/ui/onroad/starpilot/csc_force_stop.py new file mode 100644 index 000000000..3cfb1c2f2 --- /dev/null +++ b/selfdrive/ui/onroad/starpilot/csc_force_stop.py @@ -0,0 +1,125 @@ +import pyray as rl +import math +from openpilot.common.constants import CV +from openpilot.selfdrive.ui.ui_state import ui_state + +def render_csc_force_stop(content_rect: rl.Rectangle, font_bold): + plan = ui_state.sm["starpilotPlan"] if ui_state.sm.valid.get("starpilotPlan", False) else None + if not plan: + return + + forcing_stop = getattr(plan, "forcingStop", False) + csc_enabled = getattr(plan, "curveSpeedControlEnabled", False) + cond_status = ui_state.conditional_status + + if not forcing_stop and not (cond_status == 3 and csc_enabled): + return + + # Calculate layout coordinates matching C++ + # setSpeedRect calculations: + ss_width = 200 if ui_state.is_metric else 172 + ss_x = content_rect.x + 60 + (172 - ss_width) // 2 + ss_y = content_rect.y + 45 + ss_height = 204 + + csc_x = ss_x + ss_width + 30 + csc_y = ss_y + w = 215 + h = 215 + + csc_rect = rl.Rectangle(csc_x, csc_y, w, h) + badge_rect = rl.Rectangle(csc_x, csc_y + h + 10, w, 100) + + distance_conversion = 1.0 if ui_state.is_metric else 3.28084 + speed_conversion = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH + dist_unit = "m" if ui_state.is_metric else "ft" + speed_unit = "km/h" if ui_state.is_metric else "mph" + + car_state = ui_state.sm["carState"] if ui_state.sm.valid.get("carState", False) else None + v_ego = car_state.vEgo if car_state else 0.0 + + if forcing_stop: + # ── FORCE STOP MODE ── + forcing_stop_length = getattr(plan, "forcingStopLength", 0.0) + stop_sign_confirmed = getattr(plan, "stopSignConfirmed", False) + + # Draw Octagon Stop sign in csc_rect + cx = csc_x + w / 2 + cy = csc_y + h / 2 + radius = 65.0 + rl.draw_poly(rl.Vector2(int(cx), int(cy)), 8, radius, 22.5, rl.WHITE) + rl.draw_poly(rl.Vector2(int(cx), int(cy)), 8, radius - 5, 22.5, rl.Color(196, 30, 58, 255)) + + if stop_sign_confirmed: + # Draw white checkmark + rl.draw_line_ex(rl.Vector2(int(cx - 20), int(cy)), rl.Vector2(int(cx - 5), int(cy + 15)), 6, rl.WHITE) + rl.draw_line_ex(rl.Vector2(int(cx - 5), int(cy + 15)), rl.Vector2(int(cx + 25), int(cy - 15)), 6, rl.WHITE) + else: + # Draw white exclamation mark + rl.draw_rectangle(int(cx - 4), int(cy - 25), 8, 30, rl.WHITE) + rl.draw_circle(int(cx), int(cy + 18), 5, rl.WHITE) + + # Draw red badge with stopping distance + rl.draw_rectangle_rounded(badge_rect, 0.24, 16, rl.Color(196, 30, 58, 166)) + rl.draw_rectangle_rounded_lines_ex(badge_rect, 0.24, 16, 4, rl.Color(255, 150, 150, 255)) + + dist_val = int(round(forcing_stop_length * distance_conversion)) + text = f"{dist_val} {dist_unit}" + text_sz = rl.measure_text_ex(font_bold, text, 40, 0) + rl.draw_text_ex(font_bold, text, rl.Vector2(int(csc_x + 20), int(badge_rect.y + (100 - text_sz.y) / 2)), 40, 0, rl.WHITE) + + else: + # ── CURVE SPEED CONTROL MODE ── + csc_speed = getattr(plan, "cscSpeed", 0.0) + road_curvature = getattr(plan, "roadCurvature", 0.0) + + # Pulsing glowing border for CSC icon + phase = (rl.get_time() % 2.0) / 2.0 * 2.0 * math.pi + alpha_factor = 0.5 + 0.5 * math.sin(phase) + glow_color = rl.Color(0, 140, 255, int(255 * (0.3 + 0.7 * alpha_factor))) + glow_width = int(8 + 4 * alpha_factor) + + rl.draw_rectangle_rounded(csc_rect, 0.24, 16, rl.Color(0, 0, 0, 166)) + rl.draw_rectangle_rounded_lines_ex(csc_rect, 0.24, 16, glow_width, glow_color) + + # Draw curvy path line inside csc_rect + cx = csc_x + w / 2 + cy = csc_y + h / 2 + path_color = rl.Color(0, 140, 255, 255) + + # Draw curve left or right + if road_curvature < 0: + # Left curve spline + p1 = rl.Vector2(int(cx), int(cy + 60)) + p2 = rl.Vector2(int(cx), int(cy - 20)) + p3 = rl.Vector2(int(cx - 50), int(cy - 50)) + rl.draw_spline_bezier_quadratic([p1, p2, p3], 3, 8, path_color) + # Left arrowhead + rl.draw_triangle( + rl.Vector2(int(cx - 55), int(cy - 60)), + rl.Vector2(int(cx - 35), int(cy - 40)), + rl.Vector2(int(cx - 45), int(cy - 35)), + path_color + ) + else: + # Right curve spline + p1 = rl.Vector2(int(cx), int(cy + 60)) + p2 = rl.Vector2(int(cx), int(cy - 20)) + p3 = rl.Vector2(int(cx + 50), int(cy - 50)) + rl.draw_spline_bezier_quadratic([p1, p2, p3], 3, 8, path_color) + # Right arrowhead + rl.draw_triangle( + rl.Vector2(int(cx + 55), int(cy - 60)), + rl.Vector2(int(cx + 45), int(cy - 35)), + rl.Vector2(int(cx + 35), int(cy - 40)), + path_color + ) + + # Draw blue badge with CSC speed target + rl.draw_rectangle_rounded(badge_rect, 0.24, 16, rl.Color(0, 140, 255, 166)) + rl.draw_rectangle_rounded_lines_ex(badge_rect, 0.24, 16, 4, rl.Color(100, 200, 255, 255)) + + csc_speed_val = int(round(min(v_ego, csc_speed) * speed_conversion)) + text = f"{csc_speed_val} {speed_unit}" + text_sz = rl.measure_text_ex(font_bold, text, 40, 0) + rl.draw_text_ex(font_bold, text, rl.Vector2(int(csc_x + 20), int(badge_rect.y + (100 - text_sz.y) / 2)), 40, 0, rl.WHITE) diff --git a/selfdrive/ui/onroad/starpilot/developer_sidebar.py b/selfdrive/ui/onroad/starpilot/developer_sidebar.py new file mode 100644 index 000000000..8a2adde93 --- /dev/null +++ b/selfdrive/ui/onroad/starpilot/developer_sidebar.py @@ -0,0 +1,277 @@ +import pyray as rl +import time +import re +from cereal import car +from openpilot.common.params import Params +from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.system.ui.lib.application import gui_app, FontWeight, FONT_SCALE +from openpilot.system.ui.lib.text_measure import measure_text_cached + +SIDEBAR_WIDTH = 300 +METRIC_HEIGHT = 126 +METRIC_WIDTH = 275 +METRIC_MARGIN = 12 +FONT_SIZE = 35 +METER_TO_FOOT = 3.28084 +_WHITE_DIM = rl.Color(255, 255, 255, 85) + +def parse_hex_color(hex_str: str, default_color=rl.WHITE) -> rl.Color: + if not hex_str: + return default_color + hex_str = hex_str.lstrip('#') + try: + if len(hex_str) == 6: + r = int(hex_str[0:2], 16) + g = int(hex_str[2:4], 16) + b = int(hex_str[4:6], 16) + return rl.Color(r, g, b, 255) + elif len(hex_str) == 8: + r = int(hex_str[0:2], 16) + g = int(hex_str[2:4], 16) + b = int(hex_str[4:6], 16) + a = int(hex_str[6:8], 16) + return rl.Color(r, g, b, a) + except ValueError: + pass + return default_color + + +class DeveloperSidebar: + def __init__(self): + self._params = Params() + self._font_bold = gui_app.font(FontWeight.SEMI_BOLD) + self._last_toggles_check = 0.0 + self._cached_sidebar = False + self._cached_metrics = [0] * 7 + self._cached_force_auto_tune_off = False + self._cached_force_auto_tune = False + self._cached_friction_stock = 0.0 + self._cached_friction = 0.0 + self._cached_lat_stock = 0.0 + self._cached_lat = 0.0 + + self.lateral_engagement_time = 0 + self.longitudinal_engagement_time = 0 + self.total_engagement_time = 0 + self.max_acceleration = 0.0 + self.max_steer_angle = 0 + self.max_torque = 0 + self.torque_timer_start = 0.0 + + self._visible = False + self._metric_color = rl.WHITE + self._active_ids: list[int] = [] + self._metrics: dict[int, tuple[str, str]] = {} + + @property + def visible(self) -> bool: + return self._visible + + def reset_variables(self): + self.lateral_engagement_time = 0 + self.longitudinal_engagement_time = 0 + self.total_engagement_time = 0 + self.max_acceleration = 0.0 + self.max_steer_angle = 0 + self.max_torque = 0 + self.torque_timer_start = 0.0 + + def _refresh_cache(self): + now = time.monotonic() + if now - self._last_toggles_check < 1.0: + return + self._last_toggles_check = now + self._cached_sidebar = self._params.get_bool("DeveloperSidebar") + self._cached_metrics = [self._params.get_int(f"DeveloperSidebarMetric{i}") for i in range(1, 8)] + self._cached_force_auto_tune_off = self._params.get_bool("ForceAutoTuneOff") + self._cached_force_auto_tune = self._params.get_bool("ForceAutoTune") + self._cached_friction_stock = self._params.get_float("SteerFrictionStock") + self._cached_friction = self._params.get_float("SteerFriction") + self._cached_lat_stock = self._params.get_float("SteerLatAccelStock") + self._cached_lat = self._params.get_float("SteerLatAccel") + + def _draw_metric(self, sidebar_rect: rl.Rectangle, label_first: str, label_second: str, color: rl.Color, y: float): + card_x = int(sidebar_rect.x + sidebar_rect.width) - METRIC_MARGIN - METRIC_WIDTH + metric_rect = rl.Rectangle(card_x, y, METRIC_WIDTH, METRIC_HEIGHT) + + edge_rect = rl.Rectangle(metric_rect.x + METRIC_WIDTH - 4 - 100, metric_rect.y + 4, 100, 118) + rl.begin_scissor_mode( + int(metric_rect.x + METRIC_WIDTH - 4 - 18), + int(metric_rect.y), + 18, + int(metric_rect.height) + ) + rl.draw_rectangle_rounded(edge_rect, 0.3, 10, color) + rl.end_scissor_mode() + + rl.draw_rectangle_rounded_lines_ex(metric_rect, 0.3, 10, 2, _WHITE_DIM) + + if label_second == "": + text_size = measure_text_cached(self._font_bold, label_first, FONT_SIZE) + text_pos = rl.Vector2( + metric_rect.x + (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, label_first, text_pos, FONT_SIZE, 0, rl.WHITE) + else: + labels = [label_first, label_second] + text_y = metric_rect.y + (metric_rect.height / 2 - len(labels) * FONT_SIZE * FONT_SCALE) + for text in labels: + text_size = measure_text_cached(self._font_bold, text, FONT_SIZE) + text_y += text_size.y + text_pos = rl.Vector2( + metric_rect.x + (metric_rect.width - 22 - text_size.x) / 2, + text_y + ) + rl.draw_text_ex(self._font_bold, text, text_pos, FONT_SIZE, 0, rl.WHITE) + + def update(self): + self._refresh_cache() + + self._visible = ( + self._cached_sidebar or + ui_state.starpilot_toggles.get("developer_sidebar", False) + ) + if not self._visible: + return + + if ui_state.sm.frame < ui_state.started_frame + 2: + self.reset_variables() + + assignments = [] + for i, val in enumerate(self._cached_metrics): + if val == 0: + val = ui_state.starpilot_toggles.get(f"developer_sidebar_metric{i + 1}", 0) + assignments.append(val) + + color_str = ui_state.starpilot_toggles.get("sidebar_color1", "#FFFFFFFF") + self._metric_color = parse_hex_color(color_str) + + self._active_ids = [m for m in assignments if m > 0] + if len(self._active_ids) == 0: + return + + sm = ui_state.sm + car_state = sm["carState"] if sm.valid.get("carState", False) else None + car_control = sm["carControl"] if sm.valid.get("carControl", False) else None + starpilot_plan = sm["starpilotPlan"] if sm.valid.get("starpilotPlan", False) else None + live_delay = sm["liveDelay"] if sm.valid.get("liveDelay", False) else None + live_parameters = sm["liveParameters"] if sm.valid.get("liveParameters", False) else None + live_torque_parameters = sm["liveTorqueParameters"] if sm.valid.get("liveTorqueParameters", False) else None + + is_metric = ui_state.is_metric + use_si = ui_state.starpilot_toggles.get("use_si_metrics", False) + accel_unit = " m/s²" if (is_metric or use_si) else " ft/s²" + accel_conv = 1.0 if (is_metric or use_si) else METER_TO_FOOT + + a_ego = car_state.aEgo if car_state else 0.0 + accel_val = a_ego * accel_conv + gas_pressed = car_state.gasPressed if car_state else False + if not gas_pressed: + self.max_acceleration = max(self.max_acceleration, accel_val) + + lat_active = car_control.latActive if car_control else False + long_active = car_control.longActive if car_control else False + standstill = car_state.standstill if car_state else False + reverse = car_state.gearShifter == car.CarState.GearShifter.reverse if car_state else False + + self.lateral_engagement_time += 1 if (lat_active and not standstill and not reverse) else 0 + self.longitudinal_engagement_time += 1 if (long_active and not standstill and not reverse) else 0 + self.total_engagement_time += 1 if ((not standstill and not reverse) or self.total_engagement_time == 0) else 0 + + curr_steer = int(abs(car_state.steeringAngleDeg)) if car_state else 0 + curr_torque = int(abs(car_control.actuators.torque * 100)) if (car_control and hasattr(car_control.actuators, 'torque')) else 0 + + now = time.monotonic() + if curr_torque >= 50: + self.max_steer_angle = max(self.max_steer_angle, curr_steer) + self.max_torque = max(self.max_torque, curr_torque) + self.torque_timer_start = now + elif self.torque_timer_start > 0.0 and (now - self.torque_timer_start >= 10.0): + self.max_torque = 0 + self.max_steer_angle = 0 + self.torque_timer_start = 0.0 + + steer_label = f"{curr_steer}°" + torque_label = f"{curr_torque}%" + if curr_torque >= 50 or self.torque_timer_start > 0.0: + steer_label += f" - ({self.max_steer_angle}°)" + torque_label += f" - ({self.max_torque}%)" + + force_auto_tune_off = ui_state.starpilot_toggles.get("force_auto_tune_off", False) or self._cached_force_auto_tune_off + force_auto_tune = ui_state.starpilot_toggles.get("force_auto_tune", False) or self._cached_force_auto_tune + use_params = live_torque_parameters.useParams if (live_torque_parameters and hasattr(live_torque_parameters, 'useParams')) else False + using_live_torque = not force_auto_tune_off and (use_params or force_auto_tune) + + if not using_live_torque: + friction_coeff = self._cached_friction_stock + else: + friction_coeff = live_torque_parameters.frictionCoefficientFiltered if (live_torque_parameters and hasattr(live_torque_parameters, 'frictionCoefficientFiltered')) else 0.0 + if friction_coeff == 0.0: + friction_coeff = self._cached_friction if force_auto_tune_off else (live_torque_parameters.frictionCoefficientFiltered if (live_torque_parameters and hasattr(live_torque_parameters, 'frictionCoefficientFiltered')) else 0.0) + + if not using_live_torque: + lat_factor = self._cached_lat_stock + else: + lat_factor = live_torque_parameters.latAccelFactorFiltered if (live_torque_parameters and hasattr(live_torque_parameters, 'latAccelFactorFiltered')) else 0.0 + if lat_factor == 0.0: + lat_factor = self._cached_lat if force_auto_tune_off else (live_torque_parameters.latAccelFactorFiltered if (live_torque_parameters and hasattr(live_torque_parameters, 'latAccelFactorFiltered')) else 0.0) + + lat_delay = live_delay.lateralDelay if live_delay else 0.0 + + tot_time = max(1, self.total_engagement_time) + lat_pct = (self.lateral_engagement_time / tot_time) * 100.0 + long_pct = (self.longitudinal_engagement_time / tot_time) * 100.0 + + accel_jerk = starpilot_plan.accelerationJerk if starpilot_plan else 0.0 + act_accel = (car_control.actuators.accel if (car_control and hasattr(car_control.actuators, 'accel')) else 0.0) * accel_conv + danger_factor = (starpilot_plan.dangerFactor if starpilot_plan else 0.0) * 100.0 + danger_jerk = starpilot_plan.dangerJerk if starpilot_plan else 0.0 + speed_jerk = starpilot_plan.speedJerk if starpilot_plan else 0.0 + + steer_ratio = live_parameters.steerRatio if live_parameters else 0.0 + stiff_factor = live_parameters.stiffnessFactor if live_parameters else 0.0 + + model_name = ui_state.starpilot_toggles.get("model_name", "N/A") + model_name = re.sub(r'\(.*\)', '', model_name) + model_name = re.sub(r'[^a-zA-Z0-9 \-\.:]', '', model_name).strip() + + self._metrics = { + 1: ("ACCEL", f"{accel_val:.2f}{accel_unit}"), + 2: ("MAX ACCEL", f"{self.max_acceleration:.2f}{accel_unit}"), + 3: ("STEER DELAY", f"{lat_delay:.5f}"), + 4: ("FRICTION", f"{friction_coeff:.5f}"), + 5: ("LAT ACCEL", f"{lat_factor:.5f}"), + 6: ("STEER RATIO", f"{steer_ratio:.5f}"), + 7: ("STEER STIFF", f"{stiff_factor:.5f}"), + 8: ("LATERAL %", f"{lat_pct:.2f}%"), + 9: ("LONG %", f"{long_pct:.2f}%"), + 10: ("STEER ANGLE", steer_label), + 11: ("TORQUE %", torque_label), + 12: ("ACT ACCEL", f"{act_accel:.2f}{accel_unit}"), + 13: ("DANGER %", f"{danger_factor:.2f}%"), + 14: ("ACCEL JERK", f"{accel_jerk}"), + 15: ("DANGER JERK", f"{danger_jerk}"), + 16: ("SPEED JERK", f"{speed_jerk}"), + 17: (model_name, "") + } + + def render(self, sidebar_rect: rl.Rectangle): + if not self._visible: + return + + count = len(self._active_ids) + if count == 0: + return + + rl.draw_rectangle_rec(sidebar_rect, rl.BLACK) + + spacing = max(1, (int(sidebar_rect.height) - (count * METRIC_HEIGHT)) // max(1, (count + 1))) + y = sidebar_rect.y + spacing + + for metric_id in self._active_ids: + if metric_id <= 0 or metric_id not in self._metrics: + continue + label_first, label_second = self._metrics[metric_id] + self._draw_metric(sidebar_rect, label_first, label_second, self._metric_color, y) + y += METRIC_HEIGHT + spacing diff --git a/selfdrive/ui/onroad/starpilot/path.py b/selfdrive/ui/onroad/starpilot/path.py index 1a5edb65a..561418afe 100644 --- a/selfdrive/ui/onroad/starpilot/path.py +++ b/selfdrive/ui/onroad/starpilot/path.py @@ -8,22 +8,13 @@ import pyray as rl from openpilot.selfdrive.ui.lib.starpilot_state import starpilot_state from openpilot.selfdrive.ui.lib.starpilot_theme import get_param_color, get_theme_color, is_stock_color_scheme, with_alpha from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.system.ui.lib.application import gui_app, FontWeight from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient -_METRICS_FONT = None _METRICS_FONT_SIZE = 45 _STOCK_LINE_GREEN = rl.Color(0, 255, 0, 241) -def _get_metrics_font(): - global _METRICS_FONT - if _METRICS_FONT is None or _METRICS_FONT.baseSize != _METRICS_FONT_SIZE: - if _METRICS_FONT is not None: - rl.unload_font(_METRICS_FONT) - _METRICS_FONT = rl.load_font_ex("fonts/Inter-SemiBold.ttf", _METRICS_FONT_SIZE, None, 256) - return _METRICS_FONT - - def _hsla_to_color(h: float, s: float, l: float, a: float) -> rl.Color: 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)) @@ -80,7 +71,7 @@ def render_adjacent_paths(renderer) -> None: distance_conversion = 3.28084 if not ui_state.is_metric else 1.0 unit = "ft" if not ui_state.is_metric else "m" - font = _get_metrics_font() + font = gui_app.font(FontWeight.SEMI_BOLD) for i, (verts, lane_width) in enumerate(zip(vertices, [lane_width_left, lane_width_right], strict=True)): if verts.size < 4 or lane_width == 0.0: @@ -92,17 +83,16 @@ def render_adjacent_paths(renderer) -> None: draw_polygon(rect, verts, gradient=gradient) if show_metrics: - is_left = i == 0 mid_index = len(verts) // 2 - anchor_idx = mid_index // 2 if is_left else mid_index + (len(verts) - mid_index) // 2 - anchor = verts[anchor_idx] + left = verts[mid_index // 2] + right = verts[mid_index + (len(verts) - mid_index) // 2] text = f"{lane_width * distance_conversion:.2f}{unit}" text_width = rl.measure_text_ex(font, text, _METRICS_FONT_SIZE, 0).x text_height = rl.measure_text_ex(font, text, _METRICS_FONT_SIZE, 0).y - text_x = anchor[0] - text_width if is_left else anchor[0] - text_y = anchor[1] - text_height / 2 + text_height * 0.75 + text_x = (left[0] + right[0]) / 2.0 - text_width / 2.0 + text_y = (left[1] + right[1]) / 2.0 - text_height / 2.0 + text_height * 0.75 _draw_text_with_outline(text, text_x, text_y, font, _METRICS_FONT_SIZE) diff --git a/selfdrive/ui/onroad/starpilot/pause_indicators.py b/selfdrive/ui/onroad/starpilot/pause_indicators.py new file mode 100644 index 000000000..bae6b9bc5 --- /dev/null +++ b/selfdrive/ui/onroad/starpilot/pause_indicators.py @@ -0,0 +1,49 @@ +import pyray as rl +from openpilot.selfdrive.ui.lib.starpilot_status import TRAFFIC_COLOR + +def draw_pause_symbol(cx: float, cy: float): + # Draw two vertical bars || in the center + rl.draw_rectangle(int(cx - 8), int(cy - 16), 5, 32, rl.WHITE) + rl.draw_rectangle(int(cx + 3), int(cy - 16), 5, 32, rl.WHITE) + +def render_lateral_paused(rect: rl.Rectangle): + # Draw background & red border + rl.draw_rectangle_rounded(rect, 0.3, 10, rl.Color(0, 0, 0, 166)) + rl.draw_rectangle_rounded_lines_ex(rect, 0.3, 10, 4, TRAFFIC_COLOR) + + cx = rect.x + rect.width / 2.0 + cy = rect.y + rect.height / 2.0 + + # Draw turn/curved arrow icon (translucent) + rl.draw_ring(rl.Vector2(int(cx), int(cy)), 20, 24, 45, 315, 0, rl.Color(255, 255, 255, 100)) + # Arrowhead + rl.draw_triangle( + rl.Vector2(int(cx + 12), int(cy - 20)), + rl.Vector2(int(cx + 25), int(cy - 12)), + rl.Vector2(int(cx + 20), int(cy - 25)), + rl.Color(255, 255, 255, 100) + ) + + # Draw pause overlay + draw_pause_symbol(cx, cy) + +def render_longitudinal_paused(rect: rl.Rectangle): + # Draw background & red border + rl.draw_rectangle_rounded(rect, 0.3, 10, rl.Color(0, 0, 0, 166)) + rl.draw_rectangle_rounded_lines_ex(rect, 0.3, 10, 4, TRAFFIC_COLOR) + + cx = rect.x + rect.width / 2.0 + cy = rect.y + rect.height / 2.0 + + # Draw speedometer arc (translucent) + rl.draw_ring(rl.Vector2(int(cx), int(cy + 8)), 20, 24, -45, 225, 0, rl.Color(255, 255, 255, 100)) + # Needle + rl.draw_line_ex( + rl.Vector2(int(cx), int(cy + 8)), + rl.Vector2(int(cx + 14), int(cy - 6)), + 3, + rl.Color(255, 255, 255, 100) + ) + + # Draw pause overlay + draw_pause_symbol(cx, cy) diff --git a/selfdrive/ui/onroad/starpilot/pedal_icons.py b/selfdrive/ui/onroad/starpilot/pedal_icons.py new file mode 100644 index 000000000..dae7d9ebc --- /dev/null +++ b/selfdrive/ui/onroad/starpilot/pedal_icons.py @@ -0,0 +1,49 @@ +import pyray as rl +from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.system.ui.lib.text_measure import measure_text_cached + +_RADIUS = 36 +_FONT_SIZE = 36 + +def render_pedal_icons(start_x: float, start_y: float, font): + params = ui_state.params + if not params.get_bool("PedalsOnUI"): + return + + car_state = ui_state.sm["carState"] if ui_state.sm.valid.get("carState", False) else None + if not car_state: + return + + standstill = getattr(car_state, "standstill", False) + brake_lights = getattr(car_state, "brakeLights", False) + acceleration_ego = getattr(car_state, "aEgo", 0.0) + + dynamic_pedals = params.get_bool("DynamicPedalsOnUI") + static_pedals = params.get_bool("StaticPedalsOnUI") + + brake_opacity = 1.0 + gas_opacity = 1.0 + + if dynamic_pedals: + brake_opacity = 1.0 if standstill else min(1.0, max(0.25, abs(acceleration_ego))) if acceleration_ego < -0.25 else 0.25 + gas_opacity = min(1.0, max(0.25, acceleration_ego)) if acceleration_ego > 0.0 else 0.25 + elif static_pedals: + brake_opacity = 1.0 if (standstill or brake_lights or acceleration_ego < -0.25) else 0.25 + gas_opacity = 1.0 if acceleration_ego > 0.25 else 0.25 + + cx = start_x + 48 + cy = start_y + 48 + + rl.draw_circle(int(cx), int(cy), _RADIUS, rl.Color(201, 34, 49, int(255 * brake_opacity))) + rl.draw_circle_lines(int(cx), int(cy), _RADIUS, rl.Color(255, 255, 255, int(255 * brake_opacity))) + + sz = measure_text_cached(font, "B", _FONT_SIZE) + rl.draw_text_ex(font, "B", rl.Vector2(int(cx - sz.x / 2), int(cy - sz.y / 2)), _FONT_SIZE, 0, rl.Color(255, 255, 255, int(255 * brake_opacity))) + + gx = cx + 96 + + rl.draw_circle(int(gx), int(cy), _RADIUS, rl.Color(22, 127, 64, int(255 * gas_opacity))) + rl.draw_circle_lines(int(gx), int(cy), _RADIUS, rl.Color(255, 255, 255, int(255 * gas_opacity))) + + sz = measure_text_cached(font, "G", _FONT_SIZE) + rl.draw_text_ex(font, "G", rl.Vector2(int(gx - sz.x / 2), int(cy - sz.y / 2)), _FONT_SIZE, 0, rl.Color(255, 255, 255, int(255 * gas_opacity))) diff --git a/selfdrive/ui/onroad/starpilot/slc_speed_limit.py b/selfdrive/ui/onroad/starpilot/slc_speed_limit.py index 1634f20f7..58271dee1 100644 --- a/selfdrive/ui/onroad/starpilot/slc_speed_limit.py +++ b/selfdrive/ui/onroad/starpilot/slc_speed_limit.py @@ -399,6 +399,28 @@ def render_speed_limit(content_rect: rl.Rectangle): if not state['hide']: _draw_speed_limit_sign(state, sign_x, sign_y, sign_width) + # 2.5 Active source label below sign + if not state['show_sources']: + source = state.get('speed_limit_source') + if source and source != "None" and source != "": + source_map = { + "Dashboard": "DASH", + "Map Data": "MAPS", + "Vision": "VISION", + "Mapbox": "MAPB", + "Upcoming": "NAV" + } + label = source_map.get(source, source.upper()) + font = _get_semi_bold() + font_size = 20 + sz = measure_text_cached(font, label, font_size) + cx = sign_x + (EU_SIGN_SIZE if use_vienna else sign_width) / 2 + bottom_y = sign_y + (EU_SIGN_SIZE if use_vienna else US_SIGN_HEIGHT) + rect = rl.Rectangle(cx - sz.x / 2 - 8, bottom_y + 8, sz.x + 16, font_size + 8) + rl.draw_rectangle_rounded(rect, 0.4, 8, rl.Color(0, 0, 0, 180)) + rl.draw_rectangle_rounded_lines_ex(rect, 0.4, 8, 1, rl.Color(255, 255, 255, 100)) + rl.draw_text_ex(font, label, rl.Vector2(cx - sz.x / 2, bottom_y + 12), font_size, 0, rl.WHITE) + # 3. Sources panel if state['show_sources']: sign_rect = _calc_sign_rect(sign_x, sign_y, sign_width, use_vienna) diff --git a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py index e5583baeb..3a65cef1a 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py @@ -26,6 +26,7 @@ class StarPilotOnroadView(AugmentedRoadView): self._standstill_started_at = 0.0 def _render(self, rect: rl.Rectangle): + self._position_personality_button() super()._render(rect) if not ui_state.started: @@ -41,6 +42,8 @@ class StarPilotOnroadView(AugmentedRoadView): int(self._content_rect.width), int(self._content_rect.height), ) render_speed_limit(self._content_rect) + from openpilot.selfdrive.ui.onroad.starpilot.csc_force_stop import render_csc_force_stop + render_csc_force_stop(self._content_rect, self._font_bold) rl.end_scissor_mode() def _render_overlays(self): @@ -48,6 +51,9 @@ class StarPilotOnroadView(AugmentedRoadView): self._personality_button.render() self._render_road_name() self._render_standstill_timer() + self._render_developer_metrics() + self._render_bottom_row_widgets() + self._render_pedals() def _render_path_features(self, rect: rl.Rectangle): """Render path-related features (adjacent paths, blind spot, path edges).""" @@ -70,26 +76,37 @@ class StarPilotOnroadView(AugmentedRoadView): elif blind_spot_enabled and mr._adjacent_path_vertices[0].size >= 4: render_blind_spot_path(mr) + # Render stopping point atop the path + from openpilot.selfdrive.ui.onroad.starpilot.stopping_point import render_stopping_point + render_stopping_point(mr, self._font_bold) + def _position_personality_button(self): dm = self.driver_state_renderer toggle_on = self._params.get_bool("OnroadDistanceButton") + GAP = 10 - if not dm.is_visible or not toggle_on: + if dm and dm.position_x != 0.0: + unshifted = dm.position_x - dm.x_shift + y = dm.position_y - BTN_SIZE / 2 + + if dm.is_rhd: + x = dm.position_x - BTN_SIZE * 2 + else: + x = unshifted - BTN_SIZE // 2 + dm.x_shift = BTN_SIZE + GAP if (dm.is_visible and toggle_on) else 0.0 + + self._personality_button.set_position(x, y) + + if not dm or not dm.is_visible or not toggle_on: self._personality_button.set_visible(False) + if dm and not dm.is_rhd: + dm.x_shift = 0.0 return self._personality_button.set_visible( lambda: ui_state.started and ui_state.has_longitudinal_control ) - y = dm.position_y - BTN_SIZE / 2 - if dm.is_rhd: - x = dm.position_x - BTN_SIZE * 2 - else: - x = dm.position_x + BTN_SIZE - - self._personality_button.set_position(x, y) - def _render_road_name(self): if not self._params.get_bool("RoadNameUI"): return @@ -148,6 +165,27 @@ class StarPilotOnroadView(AugmentedRoadView): minute_size = rl.measure_text_ex(self._font_bold, minute_text, 176, 0) second_size = rl.measure_text_ex(self._font_medium, second_text, 66, 0) + from openpilot.selfdrive.ui.lib.starpilot_status import ENGAGED_COLOR, EXPERIMENTAL_COLOR, TRAFFIC_COLOR + import numpy as np + + def blend_colors(start: rl.Color, end: rl.Color, transition: float) -> rl.Color: + transition = float(np.clip(transition, 0.0, 1.0)) + return rl.Color( + int(start.r + transition * (end.r - start.r)), + int(start.g + transition * (end.g - start.g)), + int(start.b + transition * (end.b - start.b)), + 255, + ) + + if duration < 150: + transition = (duration - 60) / 90.0 + duration_color = blend_colors(ENGAGED_COLOR, EXPERIMENTAL_COLOR, transition) + elif duration < 300: + transition = (duration - 150) / 150.0 + duration_color = blend_colors(EXPERIMENTAL_COLOR, TRAFFIC_COLOR, transition) + else: + duration_color = TRAFFIC_COLOR + x = gui_app.width / 2 rl.draw_text_ex( self._font_bold, @@ -155,7 +193,7 @@ class StarPilotOnroadView(AugmentedRoadView): rl.Vector2(x - minute_size.x / 2, 210 - minute_size.y / 2), 176, 0, - rl.Color(255, 255, 255, 255), + duration_color, ) rl.draw_text_ex( self._font_medium, @@ -163,7 +201,7 @@ class StarPilotOnroadView(AugmentedRoadView): rl.Vector2(x - second_size.x / 2, 290 - second_size.y / 2), 66, 0, - rl.Color(255, 255, 255, 255), + rl.Color(255, 255, 255, 242), ) def _draw_border(self, rect: rl.Rectangle): @@ -182,6 +220,9 @@ class StarPilotOnroadView(AugmentedRoadView): # Layer 5: Amber filament (on top of standard border) render_filament(border_rect, border_width) + # Layer 6: Turn Signal, Blind Spot, and Steering Torque Borders (Phase 6) + self._render_border_effects(rect) + def _handle_mouse_press(self, mouse_pos: MousePos): border_width = self._get_border_width() content_rect = rl.Rectangle( @@ -198,3 +239,262 @@ class StarPilotOnroadView(AugmentedRoadView): if self._personality_button.is_interacting: return super()._handle_mouse_press(mouse_pos) + + def _render_developer_metrics(self): + if not self._params.get_bool("ShowFPS"): + return + + # Track FPS + fps = rl.get_fps() + if not hasattr(self, "_min_fps"): + self._min_fps = 99.9 + self._max_fps = 0.0 + self._avg_fps = 0.0 + + if fps > 0: + self._min_fps = min(self._min_fps, fps) + self._max_fps = max(self._max_fps, fps) + alpha = 1.0 / (60.0 * 5.0) + if self._avg_fps == 0.0: + self._avg_fps = fps + else: + self._avg_fps = alpha * fps + (1.0 - alpha) * self._avg_fps + + # Gather device stats + device_state = ui_state.sm["deviceState"] if ui_state.sm.valid.get("deviceState", False) else None + cpu_val = 0 + temp_val = 0 + mem_val = 0 + mem_gb = 0.0 + if device_state: + cpu_list = list(device_state.cpuUsagePercent) + cpu_val = int(sum(cpu_list) / len(cpu_list)) if cpu_list else 0 + temp_val = int(device_state.maxTempC) + mem_val = int(device_state.memoryUsagePercent) + mem_gb = 8.0 * mem_val / 100.0 + + # Format text lines for top-right developer metrics overlay + text_lines = [ + f"FPS: {round(fps)}", + f"CPU: {cpu_val}%", + f"TEMP: {temp_val}°C", + f"RAM: {mem_gb:.1f} GB ({mem_val}%)" + ] + + # Helper function for outlined text drawing + font = self._font_medium + font_size = 24 + line_height = font_size + 4 + + def draw_text_with_outline(text, pos_x, pos_y, color): + pos = rl.Vector2(pos_x, pos_y) + rl.draw_text_ex(font, text, rl.Vector2(pos.x - 1, pos.y - 1), font_size, 0, rl.BLACK) + rl.draw_text_ex(font, text, rl.Vector2(pos.x + 1, pos.y - 1), font_size, 0, rl.BLACK) + rl.draw_text_ex(font, text, rl.Vector2(pos.x - 1, pos.y + 1), font_size, 0, rl.BLACK) + rl.draw_text_ex(font, text, rl.Vector2(pos.x + 1, pos.y + 1), font_size, 0, rl.BLACK) + rl.draw_text_ex(font, text, pos, font_size, 0, color) + + # 1. Render top-right developer metrics block + x = self._content_rect.x + self._content_rect.width - 30 + y = self._content_rect.y + 40 + for i, line in enumerate(text_lines): + sz = rl.measure_text_ex(font, line, font_size, 0) + draw_text_with_outline(line, x - sz.x, y + i * line_height, rl.WHITE) + + # 2. Render bottom-center detailed FPS tracker string (min/max/avg) + fps_str = f"FPS: {round(fps)} | Min: {round(self._min_fps)} | Max: {round(self._max_fps)} | Avg: {round(self._avg_fps)}" + sz = rl.measure_text_ex(font, fps_str, font_size, 0) + bx = self._content_rect.x + (self._content_rect.width - sz.x) / 2 + by = self._content_rect.y + self._content_rect.height - sz.y - 10 + draw_text_with_outline(fps_str, bx, by, rl.WHITE) + + def _render_border_effects(self, rect: rl.Rectangle): + car_state = ui_state.sm["carState"] if ui_state.sm.valid.get("carState", False) else None + car_control = ui_state.sm["carControl"] if ui_state.sm.valid.get("carControl", False) else None + if not car_state: + return + + show_steering = self._params.get_bool("ShowSteering") + show_signal = self._params.get_bool("SignalMetrics") + show_blindspot = self._params.get_bool("BlindSpotMetrics") + border_width = self._get_border_width() + + # 1. Turn Signal and Blind Spot warning borders + left_blindspot = car_state.leftBlindspot + right_blindspot = car_state.rightBlindspot + left_blinker = car_state.leftBlinker + right_blinker = car_state.rightBlinker + + if (show_signal and (left_blinker or right_blinker)) or (show_blindspot and (left_blindspot or right_blindspot)): + interval = 250 if show_blindspot and (left_blindspot or right_blindspot) else 500 + flicker_active = (int(rl.get_time() * 1000) % (interval * 2)) < interval + + from openpilot.selfdrive.ui.lib.starpilot_status import TRAFFIC_COLOR, CEM_OVERRIDE_COLOR + + def get_half_border_color(blindspot, turn_signal): + if turn_signal and show_signal: + if blindspot: + return TRAFFIC_COLOR if flicker_active else CEM_OVERRIDE_COLOR + else: + return CEM_OVERRIDE_COLOR if flicker_active else rl.Color(0, 0, 0, 0) + elif blindspot and show_blindspot: + return TRAFFIC_COLOR + else: + return rl.Color(0, 0, 0, 0) + + left_color = get_half_border_color(left_blindspot, left_blinker) + right_color = get_half_border_color(right_blindspot, right_blinker) + + # Draw left side borders + if left_color.a > 0: + rl.draw_rectangle(int(rect.x), int(rect.y), int(border_width), int(rect.height), left_color) + rl.draw_rectangle(int(rect.x), int(rect.y), int(rect.width // 2), int(border_width), left_color) + rl.draw_rectangle(int(rect.x), int(rect.y + rect.height - border_width), int(rect.width // 2), int(border_width), left_color) + + # Draw right side borders + if right_color.a > 0: + rl.draw_rectangle(int(rect.x + rect.width - border_width), int(rect.y), int(border_width), int(rect.height), right_color) + rl.draw_rectangle(int(rect.x + rect.width // 2), int(rect.y), int(rect.width // 2), int(border_width), right_color) + rl.draw_rectangle(int(rect.x + rect.width // 2), int(rect.y + rect.height - border_width), int(rect.width // 2), int(border_width), right_color) + + # 2. Steering Torque Border + if show_steering and car_control: + torque = -car_control.actuators.torque + abs_torque = abs(torque) + + if not hasattr(self, "_smoothed_steer"): + self._smoothed_steer = 0.0 + + self._smoothed_steer = 0.25 * abs_torque + 0.75 * self._smoothed_steer + if abs(self._smoothed_steer - abs_torque) < 0.01: + self._smoothed_steer = abs_torque + + visible_height = int(rect.height * self._smoothed_steer) + x_pos = int(rect.x) if torque < 0 else int(rect.x + rect.width - border_width) + y_pos = int(rect.y + rect.height - visible_height) + + from openpilot.selfdrive.ui.lib.starpilot_status import TRAFFIC_COLOR, EXPERIMENTAL_COLOR, CEM_OVERRIDE_COLOR, ENGAGED_COLOR + + if self._smoothed_steer < 0.25: + t = self._smoothed_steer / 0.25 + col = rl.color_alpha_blend(ENGAGED_COLOR, CEM_OVERRIDE_COLOR, rl.Color(255, 255, 255, int(t * 255))) + elif self._smoothed_steer < 0.5: + t = (self._smoothed_steer - 0.25) / 0.25 + col = rl.color_alpha_blend(CEM_OVERRIDE_COLOR, EXPERIMENTAL_COLOR, rl.Color(255, 255, 255, int(t * 255))) + else: + t = min(1.0, (self._smoothed_steer - 0.5) / 0.5) + col = rl.color_alpha_blend(EXPERIMENTAL_COLOR, TRAFFIC_COLOR, rl.Color(255, 255, 255, int(t * 255))) + + rl.draw_rectangle(int(x_pos), int(y_pos), int(border_width), int(visible_height), col) + + def _render_bottom_row_widgets(self): + # Hide if alerts are active + from cereal import log + AlertSize = log.SelfdriveState.AlertSize + if ui_state.sm["selfdriveState"].alertSize != AlertSize.none: + return + + dm = self.driver_state_renderer + # Ensure DM position has been initialized/calculated + if not dm or dm.position_x == 0.0: + return + + # Check pause/CEM states + starpilot_car_state = ui_state.sm["starpilotCarState"] if ui_state.sm.valid.get("starpilotCarState", False) else None + lateral_paused = starpilot_car_state.pauseLateral if starpilot_car_state else False + longitudinal_paused = (starpilot_car_state.pauseLongitudinal or starpilot_car_state.forceCoast) if starpilot_car_state else False + show_cem_status = self._params.get_bool("ShowCEMStatus") + + # Build the list of active left-side (DM-adjacent) badges in order of priority: + # 1. Lateral Paused, 2. Longitudinal Paused, 3. CEM Status + active_badges = [] + if lateral_paused: + active_badges.append("lateral_paused") + if longitudinal_paused: + active_badges.append("longitudinal_paused") + if show_cem_status: + active_badges.append("cem_status") + + # Dimensions + badge_w = 120 + badge_h = 72 + spacing = 20 + + # DM button size is 192 (radius 96) + dm_r = 96 + + # Render DM-adjacent badges sequentially + for i, badge in enumerate(active_badges): + if not dm.is_rhd: + # LHD: grow to the right + bx = dm.position_x + dm_r + spacing + i * (badge_w + spacing) + else: + # RHD: grow to the left + bx = dm.position_x - dm_r - spacing - badge_w - i * (badge_w + spacing) + + by = dm.position_y - badge_h / 2 + badge_rect = rl.Rectangle(bx, by, badge_w, badge_h) + + if badge == "lateral_paused": + from openpilot.selfdrive.ui.onroad.starpilot.pause_indicators import render_lateral_paused + render_lateral_paused(badge_rect) + elif badge == "longitudinal_paused": + from openpilot.selfdrive.ui.onroad.starpilot.pause_indicators import render_longitudinal_paused + render_longitudinal_paused(badge_rect) + elif badge == "cem_status": + from openpilot.selfdrive.ui.onroad.starpilot.cem_status import render_cem_status + render_cem_status(badge_rect, self._font_medium) + + # 2. Render Compass & Weather (on the opposite side of DM icon) + # Dimensions + compass_w = 120 + compass_h = 120 + weather_w = 120 + weather_h = 120 + + # Determine compass position + if not dm.is_rhd: + # LHD: Compass on the far right + cx = self._content_rect.x + self._content_rect.width - 30 - compass_w + else: + # RHD: Compass on the far left + cx = self._content_rect.x + 30 + + cy = dm.position_y - compass_h / 2 + compass_rect = rl.Rectangle(cx, cy, compass_w, compass_h) + + # Render Compass + from openpilot.selfdrive.ui.onroad.starpilot.compass import render_compass + render_compass(compass_rect, self._font_medium) + + # Render Weather next to Compass + plan = ui_state.sm["starpilotPlan"] if ui_state.sm.valid.get("starpilotPlan", False) else None + if plan and plan.weatherId != 0: + if not dm.is_rhd: + # LHD: Weather to the left of Compass + wx = cx - spacing - weather_w + else: + # RHD: Weather to the right of Compass + wx = cx + compass_w + spacing + + weather_rect = rl.Rectangle(wx, cy, weather_w, weather_h) + from openpilot.selfdrive.ui.onroad.starpilot.weather_icon import render_weather_icon + render_weather_icon(weather_rect) + + def _render_pedals(self): + from cereal import log + AlertSize = log.SelfdriveState.AlertSize + if ui_state.sm["selfdriveState"].alertSize != AlertSize.none: + return + + dm = self.driver_state_renderer + if not dm or dm.position_x == 0.0: + return + + anchor = dm.position_x if dm.is_rhd else dm.position_x - dm.x_shift + start_x = anchor - 96 + start_y = dm.position_y - 198 + + from openpilot.selfdrive.ui.onroad.starpilot.pedal_icons import render_pedal_icons + render_pedal_icons(start_x, start_y, self._font_bold) + diff --git a/selfdrive/ui/onroad/starpilot/stopping_point.py b/selfdrive/ui/onroad/starpilot/stopping_point.py new file mode 100644 index 000000000..94f7245c0 --- /dev/null +++ b/selfdrive/ui/onroad/starpilot/stopping_point.py @@ -0,0 +1,64 @@ +import pyray as rl +from openpilot.selfdrive.ui.ui_state import ui_state + +def render_stopping_point(renderer, font): + params = ui_state.params + if not params.get_bool("ShowStoppingPoint"): + return + + plan = ui_state.sm["starpilotPlan"] if ui_state.sm.valid.get("starpilotPlan", False) else None + if not plan or not plan.redLight: + return + + model = ui_state.sm["modelV2"] if ui_state.sm.valid.get("modelV2", False) else None + if not model or not len(model.position.x): + return + + stopping_distance = model.position.x[min(32, len(model.position.x) - 1)] + + # Get the end of the projected path on the screen + projected = renderer._path.projected_points + if projected.size < 4: + return + + mid_idx = len(projected) // 2 + v_left = projected[mid_idx - 1] + v_right = projected[mid_idx] + cx = (v_left[0] + v_right[0]) / 2.0 + cy = (v_left[1] + v_right[1]) / 2.0 + + # Draw programmatic stop sign (octagon) + radius = 35.0 + # Draw white outer octagon + rl.draw_poly(rl.Vector2(int(cx), int(cy - radius)), 8, radius, 22.5, rl.WHITE) + # Draw red inner octagon + rl.draw_poly(rl.Vector2(int(cx), int(cy - radius)), 8, radius - 4, 22.5, rl.Color(196, 30, 58, 255)) + + # Draw "STOP" text centered in octagon + font_size = 18 + lbl_sz = rl.measure_text_ex(font, "STOP", font_size, 0) + rl.draw_text_ex( + font, "STOP", + rl.Vector2(int(cx - lbl_sz.x / 2), int(cy - radius - lbl_sz.y / 2)), + font_size, 0, rl.WHITE + ) + + # Draw metrics if enabled + if params.get_bool("ShowStoppingPointMetrics"): + is_metric = ui_state.is_metric + if is_metric: + dist_text = f"{int(round(stopping_distance))} m" + else: + dist_text = f"{int(round(stopping_distance * 3.28084))} ft" + + text_sz = rl.measure_text_ex(font, dist_text, 24, 0) + tx = cx - text_sz.x / 2 + ty = cy - radius * 2 - text_sz.y - 5 + + # Draw black outline/shadow text + rl.draw_text_ex(font, dist_text, rl.Vector2(int(tx - 1), int(ty - 1)), 24, 0, rl.BLACK) + rl.draw_text_ex(font, dist_text, rl.Vector2(int(tx + 1), int(ty - 1)), 24, 0, rl.BLACK) + rl.draw_text_ex(font, dist_text, rl.Vector2(int(tx - 1), int(ty + 1)), 24, 0, rl.BLACK) + rl.draw_text_ex(font, dist_text, rl.Vector2(int(tx + 1), int(ty + 1)), 24, 0, rl.BLACK) + # Draw white text + rl.draw_text_ex(font, dist_text, rl.Vector2(int(tx), int(ty)), 24, 0, rl.WHITE) diff --git a/selfdrive/ui/onroad/starpilot/weather_icon.py b/selfdrive/ui/onroad/starpilot/weather_icon.py new file mode 100644 index 000000000..467b9876e --- /dev/null +++ b/selfdrive/ui/onroad/starpilot/weather_icon.py @@ -0,0 +1,104 @@ +import pyray as rl +import math +from openpilot.selfdrive.ui.ui_state import ui_state + +def render_weather_icon(rect: rl.Rectangle): + # Get weather parameters from starpilotPlan + plan = ui_state.sm["starpilotPlan"] if ui_state.sm.valid.get("starpilotPlan", False) else None + if not plan or plan.weatherId == 0: + return + + weather_id = plan.weatherId + weather_daytime = plan.weatherDaytime + + # Background color based on day/night + bg_color = rl.Color(135, 206, 235, 255) if weather_daytime else rl.Color(25, 25, 112, 255) + + # Draw background badge + rl.draw_rectangle_rounded(rect, 0.2, 10, bg_color) + rl.draw_rectangle_rounded_lines_ex(rect, 0.2, 10, 4, rl.Color(0, 0, 0, 255)) + + # Define weather conditions + is_rain = (200 <= weather_id <= 232) or (300 <= weather_id <= 321) or (500 <= weather_id <= 531) + is_snow = (600 <= weather_id <= 622) + is_fog = (701 <= weather_id <= 762) + + # Scissor to avoid overflowing the rounded rectangle + rl.begin_scissor_mode(int(rect.x + 4), int(rect.y + 4), int(rect.width - 8), int(rect.height - 8)) + + cx = rect.x + rect.width / 2.0 + cy = rect.y + rect.height / 2.0 + + if is_rain: + # 1. Draw Gray Clouds + rl.draw_circle(int(cx - 15), int(cy - 15), 18, rl.Color(180, 180, 180, 255)) + rl.draw_circle(int(cx + 15), int(cy - 12), 16, rl.Color(160, 160, 160, 255)) + rl.draw_circle(int(cx), int(cy - 20), 20, rl.Color(200, 200, 200, 255)) + + # 2. Draw falling rain drops (slanted lines) + drops = [ + (cx - 20, cy + 5), + (cx - 5, cy + 12), + (cx + 10, cy + 3), + (cx + 22, cy + 10), + (cx - 10, cy + 22), + (cx + 12, cy + 20) + ] + rain_color = rl.Color(0, 191, 255, 255) + for dx, dy in drops: + rl.draw_line_ex(rl.Vector2(int(dx), int(dy)), rl.Vector2(int(dx - 3), int(dy + 12)), 3, rain_color) + + elif is_snow: + # 1. Draw Clouds + rl.draw_circle(int(cx - 15), int(cy - 15), 18, rl.Color(180, 180, 180, 255)) + rl.draw_circle(int(cx + 15), int(cy - 12), 16, rl.Color(160, 160, 160, 255)) + rl.draw_circle(int(cx), int(cy - 20), 20, rl.Color(200, 200, 200, 255)) + + # 2. Draw snowflakes (white asterisks) + flakes = [ + (cx - 20, cy + 10), + (cx - 5, cy + 20), + (cx + 12, cy + 8), + (cx + 20, cy + 22) + ] + for fx, fy in flakes: + # Draw asterisk snowflake + rl.draw_line_ex(rl.Vector2(int(fx - 5), int(fy)), rl.Vector2(int(fx + 5), int(fy)), 2, rl.WHITE) + rl.draw_line_ex(rl.Vector2(int(fx), int(fy - 5)), rl.Vector2(int(fx), int(fy + 5)), 2, rl.WHITE) + rl.draw_line_ex(rl.Vector2(int(fx - 4), int(fy - 4)), rl.Vector2(int(fx + 4), int(fy + 4)), 2, rl.WHITE) + rl.draw_line_ex(rl.Vector2(int(fx - 4), int(fy + 4)), rl.Vector2(int(fx + 4), int(fy - 4)), 2, rl.WHITE) + + elif is_fog: + # Draw gray horizontal bands representing fog + fog_color = rl.Color(220, 220, 220, 180) + rl.draw_rectangle_rounded(rl.Rectangle(cx - 40, cy - 25, 80, 8), 0.5, 4, fog_color) + rl.draw_rectangle_rounded(rl.Rectangle(cx - 50, cy - 10, 100, 8), 0.5, 4, fog_color) + rl.draw_rectangle_rounded(rl.Rectangle(cx - 35, cy + 5, 70, 8), 0.5, 4, fog_color) + rl.draw_rectangle_rounded(rl.Rectangle(cx - 45, cy + 20, 90, 8), 0.5, 4, fog_color) + + else: + # Clear / Sun or Moon + if weather_daytime: + # Sun + rl.draw_circle(int(cx), int(cy), 22, rl.GOLD) + # Rays + ray_color = rl.ORANGE + for i in range(8): + angle = i * (math.pi / 4.0) + x1 = cx + 26 * math.cos(angle) + y1 = cy + 26 * math.sin(angle) + x2 = cx + 38 * math.cos(angle) + y2 = cy + 38 * math.sin(angle) + rl.draw_line_ex(rl.Vector2(int(x1), int(y1)), rl.Vector2(int(x2), int(y2)), 4, ray_color) + else: + # Moon + rl.draw_circle(int(cx + 5), int(cy - 5), 24, rl.Color(255, 255, 224, 255)) + # Subtracted shadow to make crescent + rl.draw_circle(int(cx - 3), int(cy - 11), 24, bg_color) + + # Tiny stars + rl.draw_circle(int(cx - 25), int(cy - 20), 2, rl.WHITE) + rl.draw_circle(int(cx + 28), int(cy + 15), 3, rl.WHITE) + rl.draw_circle(int(cx - 20), int(cy + 25), 1.5, rl.WHITE) + + rl.end_scissor_mode() diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index a6c278baa..220515bc0 100644 --- a/selfdrive/ui/ui_state.py +++ b/selfdrive/ui/ui_state.py @@ -1,3 +1,4 @@ +import json import pyray as rl import numpy as np import time @@ -59,6 +60,11 @@ class UIState: "rawAudioData", "starpilotCarState", "starpilotPlan", + "starpilotRadarState", + "starpilotSelfdriveState", + "liveTracks", + "liveDelay", + "liveTorqueParameters", ] ) @@ -88,6 +94,22 @@ class UIState: self.switchback_mode_enabled: bool = False self.traffic_mode_enabled: bool = False self.conditional_status: int = 0 + self.starpilot_toggles: dict = { + "debug_mode": False, + "driver_camera_in_reverse": False, + "force_offroad": False, + "force_onroad": False, + "screen_brightness": 101, + "screen_brightness_onroad": 101, + "screen_timeout": 30, + "screen_timeout_onroad": 10, + "sidebar_color1": "#FFFFFFFF", + "sidebar_color2": "#FFFFFFFF", + "sidebar_color3": "#FFFFFFFF", + "simple_mode": False, + "standby_mode": False, + "tethering_config": 0, + } # Callbacks self._offroad_transition_callbacks: list[Callable[[], None]] = [] @@ -164,6 +186,20 @@ class UIState: self.conditional_status = self.params_memory.get_int("CEStatus", default=0) if self.started else 0 + if self.sm.updated["starpilotPlan"]: + plan = self.sm["starpilotPlan"] + toggles_str = plan.starpilotToggles + if toggles_str: + try: + parsed = json.loads(toggles_str) + if isinstance(parsed, dict): + self.starpilot_toggles.update(parsed) + except Exception as e: + cloudlog.warning(f"Error parsing starpilot_toggles: {e}") + + self.starpilot_toggles["force_offroad"] = self.params.get_bool("ForceOffroad") + self.starpilot_toggles["force_onroad"] = self.params.get_bool("ForceOnroad") + def _update_status(self) -> None: if self.started and self.sm.updated["selfdriveState"]: ss = self.sm["selfdriveState"]