mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-03 06:33:50 +08:00
firestarsdog
310 lines
9.8 KiB
Python
310 lines
9.8 KiB
Python
import types
|
|
|
|
import pytest
|
|
|
|
|
|
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(),
|
|
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,
|
|
_is_stop_light,
|
|
_lead_data,
|
|
)
|
|
from openpilot.starpilot.common.experimental_state import CEStatus
|
|
|
|
aethergauge.ui_state = mock_ui_state
|
|
|
|
|
|
@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
|
|
|
|
|
|
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)
|
|
|
|
|
|
|
|
|
|
def test_priority_downgrade_updates_live_immediately(monkeypatch):
|
|
# Start at Priority 0 (Force Stop)
|
|
_set_plan(forcingStop=True, redLight=False, forcingStopLength=20.0)
|
|
mock_ui_state.sm.valid["carState"] = True
|
|
mock_ui_state.sm["carState"] = types.SimpleNamespace(gearShifter=1)
|
|
time_val = [1.0]
|
|
monkeypatch.setattr(aethergauge.rl, "get_time", lambda: time_val[0])
|
|
|
|
gauge = AetherGauge()
|
|
data0 = gauge.get_active_data()
|
|
assert data0.indicator_type is IndicatorType.FORCE_STOP
|
|
assert data0.indicator_value == 20.0
|
|
|
|
# Downgrade to Priority 1 (Stop Light)
|
|
time_val[0] = 1.05 # only 50ms later (well before 0.5s cooldown)
|
|
_set_plan(forcingStop=False, redLight=True, forcingStopLength=15.0)
|
|
data1 = gauge.get_active_data()
|
|
assert data1 is not None
|
|
assert data1.indicator_type is IndicatorType.STOP_LIGHT
|
|
assert data1.indicator_value == 15.0
|
|
|
|
|
|
def test_lead_data_schmitt_trigger_hysteresis():
|
|
mock_ui_state.sm.valid["radarState"] = True
|
|
|
|
# 1. Start fast -> SLOW
|
|
mock_ui_state.sm["radarState"] = types.SimpleNamespace(
|
|
leadOne=types.SimpleNamespace(status=True, vLead=3.0, dRel=20.0)
|
|
)
|
|
data = _lead_data()
|
|
assert data.text == "SLOW"
|
|
|
|
# 2. Slow down to 0.9 m/s (below 1.0, but above 0.8 entry) -> should STAY SLOW
|
|
mock_ui_state.sm["radarState"].leadOne.vLead = 0.9
|
|
data = _lead_data()
|
|
assert data.text == "SLOW"
|
|
|
|
# 3. Slow down to 0.7 m/s (below 0.8 entry) -> transitions to STOPPED
|
|
mock_ui_state.sm["radarState"].leadOne.vLead = 0.7
|
|
data = _lead_data()
|
|
assert data.text == "STOPPED"
|
|
|
|
# 4. Accelerate to 1.1 m/s (above 1.0, but below 1.2 exit) -> should STAY STOPPED
|
|
mock_ui_state.sm["radarState"].leadOne.vLead = 1.1
|
|
data = _lead_data()
|
|
assert data.text == "STOPPED"
|
|
|
|
# 5. Accelerate to 1.3 m/s (above 1.2 exit) -> transitions to SLOW
|
|
mock_ui_state.sm["radarState"].leadOne.vLead = 1.3
|
|
data = _lead_data()
|
|
assert data.text == "SLOW"
|
|
|
|
|
|
def test_monotonic_ratchet_clamp_prevents_upward_bounce(monkeypatch):
|
|
mock_ui_state.sm.valid["carState"] = True
|
|
mock_ui_state.sm["carState"] = types.SimpleNamespace(vEgo=5.0, standstill=False)
|
|
mock_ui_state.is_metric = True
|
|
|
|
rendered_data = []
|
|
gauge = AetherGauge()
|
|
monkeypatch.setattr(gauge, "_render_unified_road", lambda rect, cx, cy, data, fb, fm, alpha: rendered_data.append(data))
|
|
|
|
# Initial frame at 30m
|
|
_set_plan(redLight=True, forcingStopLength=30.0)
|
|
gauge.render(None, None, None, current_speed=10.0, cx=100.0, bottom=200.0)
|
|
assert int(rendered_data[-1].text) == 30
|
|
|
|
# Step closer: raw jumps UPWARD to 35m due to optical camera noise
|
|
_set_plan(redLight=True, forcingStopLength=35.0)
|
|
gauge.render(None, None, None, current_speed=10.0, cx=100.0, bottom=200.0)
|
|
# Ratchet clamp must prevent the display number from increasing above 30m!
|
|
assert int(rendered_data[-1].text) <= 30
|