mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-20 15:54:13 +08:00
Curve Widget Cleanup
This commit is contained in:
firestarsdog
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user