134 lines
4.4 KiB
Python
134 lines
4.4 KiB
Python
"""
|
|
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
|
"""
|
|
from types import SimpleNamespace
|
|
|
|
import pytest
|
|
|
|
import iqpilot.cereal.messaging as messaging
|
|
from iqpilot.cereal import custom
|
|
from iqpilot.common.realtime import DT_MDL
|
|
from iqpilot.selfdrive.controls.lib.helpers.e2e_alerts import (
|
|
EndToEndAlertEngine, CONFIRM_S, SETTLE_S, HORIZON_TAIL, PATH_SPEED_MPS, LEAD_SPEED_MPS, LEAD_GAP_M)
|
|
|
|
E2E_CHIME = custom.IQOnroadEvent.EventName.e2eChime
|
|
|
|
|
|
class _Events(list):
|
|
def add(self, name):
|
|
self.append(name)
|
|
|
|
|
|
def _model(horizon):
|
|
msg = messaging.new_message('modelV2')
|
|
msg.modelV2.velocity.x = [0.0] * (33 - HORIZON_TAIL) + [horizon] * HORIZON_TAIL
|
|
return msg.as_reader().modelV2
|
|
|
|
|
|
def _sm(*, v_ego=0.0, standstill=True, gas=False, enabled=False, cruise=False,
|
|
horizon=0.0, lead=None):
|
|
lead = lead or SimpleNamespace(status=False, dRel=0.0, vLead=0.0)
|
|
return {
|
|
'carState': SimpleNamespace(vEgo=v_ego, standstill=standstill, gasPressed=gas,
|
|
cruiseState=SimpleNamespace(enabled=cruise)),
|
|
'selfdriveState': SimpleNamespace(enabled=enabled),
|
|
'radarState': SimpleNamespace(leadOne=lead),
|
|
'modelV2': _model(horizon),
|
|
}
|
|
|
|
|
|
def _engine(path=True, lead=True):
|
|
engine = EndToEndAlertEngine()
|
|
engine._refresh_params = lambda: None
|
|
engine._on = {"path": path, "lead": lead}
|
|
return engine
|
|
|
|
|
|
def _run(engine, sm, seconds):
|
|
events = _Events()
|
|
chimes = 0
|
|
for _ in range(int(seconds / DT_MDL)):
|
|
engine.update(sm, events)
|
|
chimes += events.count(E2E_CHIME)
|
|
events.clear()
|
|
return chimes
|
|
|
|
|
|
def _lead(d_rel, v_lead=0.0):
|
|
return SimpleNamespace(status=True, dRel=d_rel, vLead=v_lead)
|
|
|
|
|
|
def test_path_opens_chimes_once():
|
|
engine = _engine()
|
|
assert _run(engine, _sm(horizon=0.0), SETTLE_S + 1.0) == 0
|
|
assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), CONFIRM_S + 1.0) == 1
|
|
assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), 5.0) == 0
|
|
|
|
|
|
def test_path_needs_the_settle_dwell():
|
|
engine = _engine()
|
|
assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), SETTLE_S - 0.2) == 0
|
|
|
|
|
|
def test_path_silent_while_openpilot_long_is_engaged():
|
|
engine = _engine()
|
|
_run(engine, _sm(horizon=0.0, enabled=True), SETTLE_S + 1.0)
|
|
assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0, enabled=True), 5.0) == 0
|
|
|
|
|
|
def test_path_silent_while_stock_acc_holds_the_car():
|
|
engine = _engine()
|
|
_run(engine, _sm(horizon=0.0, cruise=True), SETTLE_S + 1.0)
|
|
assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0, cruise=True), 5.0) == 0
|
|
|
|
|
|
def test_path_chimes_under_aol():
|
|
engine = _engine()
|
|
_run(engine, _sm(horizon=0.0), SETTLE_S + 1.0)
|
|
assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), CONFIRM_S + 1.0) == 1
|
|
|
|
|
|
def test_path_ignores_a_visible_lead():
|
|
engine = _engine(lead=False)
|
|
_run(engine, _sm(horizon=0.0, lead=_lead(6.0)), SETTLE_S + 1.0)
|
|
assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0, lead=_lead(6.0)), 5.0) == 0
|
|
|
|
|
|
def test_lead_pullaway_chimes_once():
|
|
engine = _engine()
|
|
assert _run(engine, _sm(lead=_lead(6.0)), SETTLE_S + 1.0) == 0
|
|
moving = _sm(lead=_lead(6.0 + LEAD_GAP_M + 0.5, LEAD_SPEED_MPS + 1.0))
|
|
assert _run(engine, moving, CONFIRM_S + 1.0) == 1
|
|
assert _run(engine, moving, 5.0) == 0
|
|
|
|
|
|
def test_lead_creep_inside_the_gap_stays_silent():
|
|
engine = _engine()
|
|
_run(engine, _sm(lead=_lead(6.0)), SETTLE_S + 1.0)
|
|
assert _run(engine, _sm(lead=_lead(6.0 + LEAD_GAP_M / 2, LEAD_SPEED_MPS + 1.0)), 5.0) == 0
|
|
|
|
|
|
def test_lead_far_ahead_is_not_a_queue():
|
|
engine = _engine()
|
|
_run(engine, _sm(lead=_lead(40.0)), SETTLE_S + 1.0)
|
|
assert _run(engine, _sm(lead=_lead(44.0, LEAD_SPEED_MPS + 1.0)), 5.0) == 0
|
|
|
|
|
|
def test_gas_and_motion_rearm_the_dwell():
|
|
engine = _engine()
|
|
_run(engine, _sm(horizon=0.0), SETTLE_S + 1.0)
|
|
_run(engine, _sm(v_ego=5.0, standstill=False, horizon=8.0), 2.0)
|
|
assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), SETTLE_S - 0.2) == 0
|
|
assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), CONFIRM_S + 1.0) == 1
|
|
|
|
|
|
@pytest.mark.parametrize("param_off", ["path", "lead"])
|
|
def test_each_param_gates_only_its_own_trigger(param_off):
|
|
engine = _engine(path=param_off != "path", lead=param_off != "lead")
|
|
if param_off == "path":
|
|
_run(engine, _sm(horizon=0.0), SETTLE_S + 1.0)
|
|
assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), 5.0) == 0
|
|
else:
|
|
_run(engine, _sm(lead=_lead(6.0)), SETTLE_S + 1.0)
|
|
assert _run(engine, _sm(lead=_lead(8.0, LEAD_SPEED_MPS + 1.0)), 5.0) == 0
|