diff --git a/selfdrive/ui/onroad/starpilot/aethergauge.py b/selfdrive/ui/onroad/starpilot/aethergauge.py index 9c45a0751..c6aefc695 100644 --- a/selfdrive/ui/onroad/starpilot/aethergauge.py +++ b/selfdrive/ui/onroad/starpilot/aethergauge.py @@ -59,7 +59,7 @@ COLOR_STOP_LINE_GLOW = rl.Color(255, 30, 60, 255) COLOR_STOP_LINE_CORE = rl.Color(255, 200, 200, 255) # Set to True to force test-cycle mode (flip back to False before pushing) -TEST_CYCLE = True +TEST_CYCLE = False # --- Conversion helpers --- @@ -170,7 +170,7 @@ def _build_curve_gauge_data(curvature: float, target_speed: float, v_cruise: flo # --- Force stop --- def _is_force_stop() -> bool: - return _get_val("starpilotPlan", "forcingStop", False) + return _get_val("starpilotPlan", "forcingStop", False) and not _get_val("starpilotPlan", "redLight", False) def _force_stop_data() -> AetherGaugeData: v_ego = _get_val("carState", "vEgo", 0.0) @@ -184,7 +184,7 @@ def _force_stop_data() -> AetherGaugeData: # --- CEM: Stop light / stop sign --- def _is_stop_light() -> bool: - return ui_state.conditional_status == CEM_STATUS_STOP_LIGHT and _sm_valid("starpilotPlan") + return _get_val("starpilotPlan", "experimentalMode", False) and _get_val("starpilotPlan", "redLight", False) def _stop_light_data() -> AetherGaugeData: dist = 0.0 @@ -220,7 +220,7 @@ def _curve_speed_data() -> AetherGaugeData: # --- CEM: Curvature (non-CSC) --- def _is_curvature() -> bool: - return ui_state.conditional_status == CEM_STATUS_CURVE and _sm_valid("starpilotPlan") + return _get_val("starpilotPlan", "experimentalMode", False) and abs(_get_val("starpilotPlan", "roadCurvature", 0.0)) > 0.0012 def _curvature_data() -> AetherGaugeData: csc_speed = _get_val("starpilotPlan", "cscSpeed", 0.0) @@ -234,7 +234,8 @@ def _curvature_data() -> AetherGaugeData: # --- CEM: Lead vehicle (graphic only, no numeric) --- def _is_lead() -> bool: - return (ui_state.conditional_status == CEM_STATUS_LEAD + return (_get_val("starpilotPlan", "experimentalMode", False) + and _get_val("starpilotPlan", "trackingLead", False) and _sm_valid("radarState") and ui_state.sm["radarState"].leadOne.status) @@ -249,6 +250,12 @@ def _lead_data() -> AetherGaugeData: ) + + + + + + # --- Test cycle source (debug only, module-level state) --- _TEST_STATES = [ diff --git a/selfdrive/ui/onroad/starpilot/cem_status.py b/selfdrive/ui/onroad/starpilot/cem_status.py deleted file mode 100644 index 08734215a..000000000 --- a/selfdrive/ui/onroad/starpilot/cem_status.py +++ /dev/null @@ -1,48 +0,0 @@ -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 -from openpilot.system.ui.lib.text_measure import measure_text_cached - -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 = measure_text_cached(font, label, font_size) - 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/starpilot_onroad_view.py b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py index d0f60046c..51da5df0d 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py @@ -295,17 +295,14 @@ class StarPilotOnroadView(AugmentedRoadView): 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 + # 1. Lateral Paused, 2. Longitudinal Paused 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 @@ -333,9 +330,6 @@ class StarPilotOnroadView(AugmentedRoadView): 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 diff --git a/selfdrive/ui/tests/test_aethergauge.py b/selfdrive/ui/tests/test_aethergauge.py new file mode 100644 index 000000000..10b444406 --- /dev/null +++ b/selfdrive/ui/tests/test_aethergauge.py @@ -0,0 +1,160 @@ +import sys +import types +import unittest +from unittest.mock import MagicMock + +from collections import namedtuple + +ColorClass = namedtuple("Color", ["r", "g", "b", "a"]) + +# 1. Register Mock/Stub for pyray +rl = types.SimpleNamespace( + Color=lambda r, g, b, a=255: ColorClass(r, g, b, a), + Rectangle=lambda x=0, y=0, width=0, height=0: types.SimpleNamespace(x=x, y=y, width=width, height=height), + Vector2=lambda x=0, y=0: types.SimpleNamespace(x=x, y=y), + Texture2D=type("Texture2D", (), {}), + Font=type("Font", (), {}), + WHITE=ColorClass(255, 255, 255, 255), + BLACK=ColorClass(0, 0, 0, 255), + get_time=lambda: 1.0, +) +sys.modules["pyray"] = rl + +# 2. Register Mock/Stub for text_measure +text_measure = types.SimpleNamespace( + measure_text_cached=lambda *a, **k: types.SimpleNamespace(x=100, y=20) +) +sys.modules["openpilot.system.ui.lib.text_measure"] = text_measure + +# 3. Register Mock/Stub for starpilot_border +starpilot_border = types.SimpleNamespace( + _csc_state=lambda: None, + _intensity=lambda c: 0.0, + _glow_color=lambda i: rl.Color(0, 255, 0, 255), +) +sys.modules["openpilot.selfdrive.ui.onroad.starpilot.starpilot_border"] = starpilot_border + +class MockSubMaster: + def __init__(self): + self.valid = {} + self.updated = {} + self.data = {} + + def __getitem__(self, key): + return self.data.get(key) + + def __setitem__(self, key, value): + self.data[key] = value + + def reset(self): + self.valid.clear() + self.updated.clear() + self.data.clear() + +mock_ui_state = types.SimpleNamespace( + is_metric=False, + sm=MockSubMaster(), +) +ui_state_mod = types.ModuleType("openpilot.selfdrive.ui.ui_state") +ui_state_mod.ui_state = mock_ui_state +sys.modules["openpilot.selfdrive.ui.ui_state"] = ui_state_mod + +# Now import aethergauge +from openpilot.selfdrive.ui.onroad.starpilot.aethergauge import ( + AetherGauge, + AetherGaugeData, + IndicatorType, + _is_lead, + _lead_data, + _is_stop_light, + _is_curvature, +) + +class TestAetherGaugeLeadLogic(unittest.TestCase): + def setUp(self): + mock_ui_state.sm.reset() + + def test_is_lead_inactive_if_not_experimental(self): + mock_ui_state.sm.valid["starpilotPlan"] = True + mock_ui_state.sm.valid["radarState"] = True + mock_ui_state.sm["starpilotPlan"] = types.SimpleNamespace( + experimentalMode=False, + trackingLead=True, + ) + mock_ui_state.sm["radarState"] = types.SimpleNamespace( + leadOne=types.SimpleNamespace(status=True, vLead=5.0, dRel=20.0) + ) + self.assertFalse(_is_lead()) + + def test_is_lead_inactive_if_not_tracking_lead(self): + mock_ui_state.sm.valid["starpilotPlan"] = True + mock_ui_state.sm.valid["radarState"] = True + mock_ui_state.sm["starpilotPlan"] = types.SimpleNamespace( + experimentalMode=True, + trackingLead=False, + ) + mock_ui_state.sm["radarState"] = types.SimpleNamespace( + leadOne=types.SimpleNamespace(status=True, vLead=5.0, dRel=20.0) + ) + self.assertFalse(_is_lead()) + + def test_is_lead_active_when_experimental_and_tracking(self): + mock_ui_state.sm.valid["starpilotPlan"] = True + mock_ui_state.sm.valid["radarState"] = True + mock_ui_state.sm["starpilotPlan"] = types.SimpleNamespace( + experimentalMode=True, + trackingLead=True, + ) + mock_ui_state.sm["radarState"] = types.SimpleNamespace( + leadOne=types.SimpleNamespace(status=True, vLead=5.0, dRel=20.0) + ) + self.assertTrue(_is_lead()) + + def test_lead_data_slow(self): + mock_ui_state.sm.valid["radarState"] = True + mock_ui_state.sm["radarState"] = types.SimpleNamespace( + leadOne=types.SimpleNamespace(status=True, vLead=5.0, dRel=25.0) + ) + data = _lead_data() + self.assertEqual(data.text, "SLOW") + self.assertEqual(data.indicator_extra, "slower") + self.assertEqual(data.indicator_value, 25.0) + self.assertEqual(data.indicator_type, IndicatorType.LEAD) + + def test_lead_data_stopped(self): + mock_ui_state.sm.valid["radarState"] = True + mock_ui_state.sm["radarState"] = types.SimpleNamespace( + leadOne=types.SimpleNamespace(status=True, vLead=0.5, dRel=12.0) + ) + data = _lead_data() + self.assertEqual(data.text, "STOPPED") + self.assertEqual(data.indicator_extra, "stopped") + self.assertEqual(data.indicator_value, 12.0) + self.assertEqual(data.indicator_type, IndicatorType.LEAD) + + def test_is_stop_light(self): + mock_ui_state.sm.valid["starpilotPlan"] = True + mock_ui_state.sm["starpilotPlan"] = types.SimpleNamespace( + experimentalMode=True, + redLight=True, + ) + self.assertTrue(_is_stop_light()) + + mock_ui_state.sm["starpilotPlan"].redLight = False + self.assertFalse(_is_stop_light()) + + def test_is_curvature(self): + mock_ui_state.sm.valid["starpilotPlan"] = True + mock_ui_state.sm["starpilotPlan"] = types.SimpleNamespace( + experimentalMode=True, + roadCurvature=0.002, + ) + self.assertTrue(_is_curvature()) + + mock_ui_state.sm["starpilotPlan"].roadCurvature = 0.0005 + self.assertFalse(_is_curvature()) + + + +if __name__ == "__main__": + unittest.main()