diff --git a/selfdrive/ui/onroad/starpilot/aethergauge.py b/selfdrive/ui/onroad/starpilot/aethergauge.py index af34d720c..5c4f7a912 100644 --- a/selfdrive/ui/onroad/starpilot/aethergauge.py +++ b/selfdrive/ui/onroad/starpilot/aethergauge.py @@ -44,9 +44,7 @@ CHEVRON_COUNT = 6 CHEVRON_SPACING = 0.3 CHEVRON_STEP = 0.05 -CEM_STATUS_CURVE = 3 -CEM_STATUS_LEAD = 4 -CEM_STATUS_STOP_LIGHT = 8 +CEM_STATUS_CURVE = CEStatus["CURVATURE"] LEAD_STOPPED_SPEED_THRESHOLD = 1.0 @@ -250,12 +248,24 @@ def _curve_speed_data() -> AetherGaugeData: return _build_curve_gauge_data(state['curvature'], csc_speed, v_cruise) -# --- CEM: Curvature (non-CSC) --- +# --- CEM-selected curvature --- -def _is_curvature() -> bool: - return _get_val("starpilotPlan", "experimentalMode", False) and abs(_get_val("starpilotPlan", "roadCurvature", 0.0)) > 0.0012 +def _is_cem_curvature() -> bool: + # CEM owns detection; the UI consumes its selected reason while tracking is active. + toggles = getattr(ui_state, "starpilot_toggles", {}) + tracking_active = ( + _get_val("selfdriveState", "enabled", False) or + _get_val("starpilotCarState", "alwaysOnLateralEnabled", False) + ) + return ( + bool(toggles.get("conditional_experimental_mode", False)) + and bool(toggles.get("conditional_curves", False)) + and tracking_active + and _get_val("starpilotPlan", "experimentalMode", False) + and ui_state.conditional_status == CEM_STATUS_CURVE + ) -def _curvature_data() -> AetherGaugeData: +def _cem_curvature_data() -> AetherGaugeData: csc_speed = _get_val("starpilotPlan", "cscSpeed", 0.0) v_ego = _get_val("carState", "vEgo", 0.0) v_cruise = _get_val("starpilotPlan", "vCruise", v_ego) @@ -397,7 +407,7 @@ class AetherGauge: (_is_force_stop, _force_stop_data), (_is_stop_light, _stop_light_data), (_is_curve_speed, _curve_speed_data), - (_is_curvature, _curvature_data), + (_is_cem_curvature, _cem_curvature_data), (_is_lead, _lead_data), ] if TEST_CYCLE: @@ -424,13 +434,13 @@ class AetherGauge: now = rl.get_time() best_priority = 999 new_data = None - + for i, (is_active, get_data) in enumerate(self._sources): if is_active(): best_priority = i new_data = get_data() break - + # Treat None as a priority 999 state: switch immediately if higher/equal priority, # or wait for cooldown to downgrade/hide. if best_priority <= self._active_priority or (now - self._last_active_time > self._cooldown): @@ -440,7 +450,8 @@ class AetherGauge: return self._cached_data - def render(self, rect: rl.Rectangle, font_bold: rl.Font, font_medium: rl.Font, current_speed: float, cx: float | None = None, bottom: float | None = None, alpha: float = 1.0): + def render(self, rect: rl.Rectangle, font_bold: rl.Font, font_medium: rl.Font, current_speed: float, + cx: float | None = None, bottom: float | None = None, alpha: float = 1.0): data = self.get_active_data() if not data: return diff --git a/selfdrive/ui/tests/test_aethergauge.py b/selfdrive/ui/tests/test_aethergauge.py index 312482c05..cef3ad1a9 100644 --- a/selfdrive/ui/tests/test_aethergauge.py +++ b/selfdrive/ui/tests/test_aethergauge.py @@ -1,5 +1,7 @@ import types -import unittest + +import pytest + class MockSubMaster: def __init__(self): @@ -18,107 +20,214 @@ class MockSubMaster: self.updated.clear() self.data.clear() + mock_ui_state = types.SimpleNamespace( is_metric=False, sm=MockSubMaster(), + conditional_status=0, + starpilot_toggles={ + "conditional_experimental_mode": True, + "conditional_curves": True, + }, ) from openpilot.selfdrive.ui.onroad.starpilot import aethergauge from openpilot.selfdrive.ui.onroad.starpilot.aethergauge import ( AetherGauge, AetherGaugeData, IndicatorType, + _cem_curvature_data, + _is_cem_curvature, + _is_curve_speed, _is_lead, - _lead_data, _is_stop_light, - _is_curvature, + _lead_data, ) +from openpilot.starpilot.common.experimental_state import CEStatus + aethergauge.ui_state = mock_ui_state -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()) +@pytest.fixture(autouse=True) +def reset_ui_state(): + mock_ui_state.sm.reset() + mock_ui_state.conditional_status = CEStatus["OFF"] + mock_ui_state.starpilot_toggles.update({ + "conditional_experimental_mode": True, + "conditional_curves": True, + }) + mock_ui_state.sm.valid["selfdriveState"] = True + mock_ui_state.sm["selfdriveState"] = types.SimpleNamespace(enabled=True) +def _set_plan(**overrides): + plan = types.SimpleNamespace( + experimentalMode=True, + roadCurvature=0.002, + cscSpeed=0.0, + vCruise=20.0, + redLight=False, + forcingStop=False, + trackingLead=False, + ) + for key, value in overrides.items(): + setattr(plan, key, value) + mock_ui_state.sm.valid["starpilotPlan"] = True + mock_ui_state.sm["starpilotPlan"] = plan + return plan -if __name__ == "__main__": - unittest.main() + +def test_is_lead_inactive_if_not_experimental(): + _set_plan(experimentalMode=False, trackingLead=True) + mock_ui_state.sm.valid["radarState"] = True + mock_ui_state.sm["radarState"] = types.SimpleNamespace( + leadOne=types.SimpleNamespace(status=True, vLead=5.0, dRel=20.0) + ) + assert not _is_lead() + + +def test_is_lead_inactive_if_not_tracking_lead(): + _set_plan(trackingLead=False) + mock_ui_state.sm.valid["radarState"] = True + mock_ui_state.sm["radarState"] = types.SimpleNamespace( + leadOne=types.SimpleNamespace(status=True, vLead=5.0, dRel=20.0) + ) + assert not _is_lead() + + +def test_is_lead_active_when_experimental_and_tracking(): + _set_plan(trackingLead=True) + mock_ui_state.sm.valid["radarState"] = True + mock_ui_state.sm["radarState"] = types.SimpleNamespace( + leadOne=types.SimpleNamespace(status=True, vLead=5.0, dRel=20.0) + ) + assert _is_lead() + + +def test_lead_data_slow(): + 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() + assert data.text == "SLOW" + assert data.indicator_extra == "slower" + assert data.indicator_value == 25.0 + assert data.indicator_type is IndicatorType.LEAD + + +def test_lead_data_stopped(): + 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() + assert data.text == "STOPPED" + assert data.indicator_extra == "stopped" + assert data.indicator_value == 12.0 + assert data.indicator_type is IndicatorType.LEAD + + +def test_is_stop_light(): + _set_plan(redLight=True) + assert _is_stop_light() + + mock_ui_state.sm["starpilotPlan"].redLight = False + assert not _is_stop_light() + + +def test_is_curve_speed_follows_csc_activation_without_mode_gate(monkeypatch): + monkeypatch.setattr(aethergauge, "_csc_state", lambda: {"active": True, "curvature": 0.002}) + assert _is_curve_speed() + + monkeypatch.setattr(aethergauge, "_csc_state", lambda: {"active": False, "curvature": 0.002}) + assert not _is_curve_speed() + + +def test_is_cem_curvature_requires_selected_cem_reason(): + _set_plan() + mock_ui_state.conditional_status = CEStatus["SPEED"] + assert not _is_cem_curvature() + + mock_ui_state.conditional_status = CEStatus["CURVATURE"] + assert _is_cem_curvature() + + +def test_is_cem_curvature_uses_status_not_curvature_threshold(): + _set_plan(roadCurvature=0.00001) + mock_ui_state.conditional_status = CEStatus["CURVATURE"] + assert _is_cem_curvature() + + +def test_is_cem_curvature_requires_experimental_mode(): + _set_plan(experimentalMode=False) + mock_ui_state.conditional_status = CEStatus["CURVATURE"] + assert not _is_cem_curvature() + + +def test_is_cem_curvature_requires_cem_mode_enabled(): + _set_plan() + mock_ui_state.conditional_status = CEStatus["CURVATURE"] + mock_ui_state.starpilot_toggles["conditional_experimental_mode"] = False + assert not _is_cem_curvature() + + +def test_is_cem_curvature_requires_curve_toggle(): + _set_plan() + mock_ui_state.conditional_status = CEStatus["CURVATURE"] + mock_ui_state.starpilot_toggles["conditional_curves"] = False + assert not _is_cem_curvature() + + +def test_is_cem_curvature_rejects_stale_status_when_tracking_stops(): + _set_plan() + mock_ui_state.conditional_status = CEStatus["CURVATURE"] + mock_ui_state.sm["selfdriveState"].enabled = False + assert not _is_cem_curvature() + + +def test_cem_curvature_data_preserves_curve_metrics(monkeypatch): + _set_plan(roadCurvature=0.003, cscSpeed=8.0, vCruise=10.0) + mock_ui_state.sm.valid["carState"] = True + mock_ui_state.sm["carState"] = types.SimpleNamespace(vEgo=12.0) + monkeypatch.setattr(aethergauge, "get_border_color", lambda _: aethergauge.COLOR_CEM_SPEED) + + data = _cem_curvature_data() + + assert isinstance(data, AetherGaugeData) + assert data.indicator_type is IndicatorType.ROAD_CURVE + assert data.indicator_value == pytest.approx(0.003) + assert data.reduction_text + + +def test_widget_wires_cem_source_to_road_curve_data(monkeypatch): + _set_plan(roadCurvature=0.003, cscSpeed=8.0, vCruise=10.0) + mock_ui_state.sm.valid["carState"] = True + mock_ui_state.sm["carState"] = types.SimpleNamespace(vEgo=12.0) + mock_ui_state.conditional_status = CEStatus["CURVATURE"] + monkeypatch.setattr(aethergauge, "_is_curve_speed", lambda: False) + monkeypatch.setattr(aethergauge, "get_border_color", lambda _: aethergauge.COLOR_CEM_SPEED) + monkeypatch.setattr(aethergauge.rl, "get_time", lambda: 1.0) + + gauge = AetherGauge() + data = gauge.get_active_data() + + assert data is not None + assert data.indicator_type is IndicatorType.ROAD_CURVE + assert data.indicator_value == pytest.approx(0.003) + + +def test_csc_source_precedes_cem_source(monkeypatch): + _set_plan(roadCurvature=0.003, cscSpeed=8.0, vCruise=10.0) + mock_ui_state.sm.valid["carState"] = True + mock_ui_state.sm["carState"] = types.SimpleNamespace(vEgo=12.0) + mock_ui_state.conditional_status = CEStatus["CURVATURE"] + monkeypatch.setattr(aethergauge, "_csc_state", lambda: {"active": True, "curvature": 0.01}) + monkeypatch.setattr(aethergauge, "get_border_color", lambda _: aethergauge.COLOR_CEM_SPEED) + monkeypatch.setattr(aethergauge.rl, "get_time", lambda: 1.0) + + gauge = AetherGauge() + data = gauge.get_active_data() + + assert data is not None + assert data.indicator_type is IndicatorType.ROAD_CURVE + assert data.indicator_value == pytest.approx(0.01)