mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-28 18:12:05 +08:00
firestar5683
463 lines
15 KiB
Python
463 lines
15 KiB
Python
from types import SimpleNamespace
|
|
|
|
from openpilot.common.constants import CV
|
|
from openpilot.starpilot.common.experimental_state import CCStatus
|
|
from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode
|
|
|
|
|
|
class FakeParams:
|
|
def __init__(self, bools=None, ints=None):
|
|
self.bools = dict(bools or {})
|
|
self.ints = dict(ints or {})
|
|
|
|
def get_bool(self, key):
|
|
return bool(self.bools.get(key, False))
|
|
|
|
def put_bool(self, key, value):
|
|
self.bools[key] = bool(value)
|
|
|
|
def get_int(self, key, default=0):
|
|
return int(self.ints.get(key, default))
|
|
|
|
def put_int(self, key, value):
|
|
self.ints[key] = int(value)
|
|
|
|
|
|
class FakeDetector:
|
|
def __init__(self):
|
|
self.curve_detected = False
|
|
self.slow_lead_detected = False
|
|
self.stop_light_detected = False
|
|
self.stop_light_model_detected = False
|
|
|
|
def curve_detection(self, *_args, **_kwargs):
|
|
return None
|
|
|
|
def slow_lead(self, *_args, **_kwargs):
|
|
return None
|
|
|
|
def stop_sign_and_light(self, *_args, **_kwargs):
|
|
return None
|
|
|
|
|
|
class FakeSubMaster:
|
|
def __init__(self, services):
|
|
self.services = dict(services)
|
|
|
|
def __getitem__(self, key):
|
|
return self.services[key]
|
|
|
|
|
|
def make_sm():
|
|
return {
|
|
"carState": SimpleNamespace(standstill=False, leftBlinker=False, rightBlinker=False),
|
|
"selfdriveState": SimpleNamespace(enabled=True),
|
|
"longitudinalPlan": SimpleNamespace(allowThrottle=True, shouldStop=False),
|
|
"starpilotCarState": SimpleNamespace(trafficModeEnabled=False),
|
|
"starpilotPlan": SimpleNamespace(redLight=False, forcingStop=False),
|
|
"starpilotRadarState": SimpleNamespace(
|
|
leadLeft=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0),
|
|
leadRight=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0),
|
|
),
|
|
}
|
|
|
|
|
|
def make_submaster_sm():
|
|
return FakeSubMaster(make_sm())
|
|
|
|
|
|
def make_toggles():
|
|
return SimpleNamespace(
|
|
conditional_chill_speed=45 * CV.MPH_TO_MS,
|
|
conditional_chill_speed_lead=35 * CV.MPH_TO_MS,
|
|
conditional_chill_speed_margin=3 * CV.MPH_TO_MS,
|
|
conditional_chill_lead=True,
|
|
conditional_chill_launch_assist=False,
|
|
)
|
|
|
|
|
|
def make_ccm():
|
|
planner = SimpleNamespace(
|
|
params=FakeParams(),
|
|
params_memory=FakeParams(),
|
|
starpilot_vcruise=SimpleNamespace(
|
|
stop_sign_confirmed=False,
|
|
forcing_stop=False,
|
|
slc=SimpleNamespace(experimental_mode=False),
|
|
),
|
|
raw_model_stopped=False,
|
|
model_stopped=False,
|
|
tracking_lead=False,
|
|
lead_one=SimpleNamespace(
|
|
status=False,
|
|
dRel=float("inf"),
|
|
vLead=0.0,
|
|
vRel=0.0,
|
|
aLeadK=0.0,
|
|
modelProb=0.0,
|
|
radar=False,
|
|
),
|
|
)
|
|
detector = FakeDetector()
|
|
return planner, detector, ConditionalChillMode(planner, detector)
|
|
|
|
|
|
def test_ccm_stays_experimental_when_no_chill_condition_matches(monkeypatch):
|
|
planner, _detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
toggles = make_toggles()
|
|
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 1.0)
|
|
ccm.update(20 * CV.MPH_TO_MS, 21 * CV.MPH_TO_MS, sm, toggles)
|
|
|
|
assert ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["OFF"]
|
|
|
|
|
|
def test_ccm_enters_chill_for_open_road_speed_recovery(monkeypatch):
|
|
planner, _detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
toggles = make_toggles()
|
|
monotonic_values = iter([10.0, 10.5])
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
|
|
|
|
v_ego = 55 * CV.MPH_TO_MS
|
|
v_cruise = v_ego + 5 * CV.MPH_TO_MS
|
|
ccm.update(v_ego, v_cruise, sm, toggles)
|
|
ccm.update(v_ego, v_cruise, sm, toggles)
|
|
|
|
assert not ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["SPEED"]
|
|
assert planner.params_memory.get_int("CCStatus") == CCStatus["SPEED"]
|
|
|
|
|
|
def test_ccm_enters_chill_for_stable_lead_cruising(monkeypatch):
|
|
planner, _detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
toggles = make_toggles()
|
|
monotonic_values = iter([10.0, 11.1])
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
|
|
|
|
planner.tracking_lead = True
|
|
planner.lead_one.status = True
|
|
planner.lead_one.dRel = 45.0
|
|
planner.lead_one.vLead = 24.8
|
|
planner.lead_one.radar = True
|
|
|
|
v_ego = 58 * CV.MPH_TO_MS
|
|
ccm.update(v_ego, v_ego, sm, toggles)
|
|
ccm.update(v_ego, v_ego, sm, toggles)
|
|
|
|
assert not ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["LEAD"]
|
|
|
|
|
|
def test_ccm_stable_lead_requires_longer_entry_debounce(monkeypatch):
|
|
planner, _detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
toggles = make_toggles()
|
|
monotonic_values = iter([10.0, 10.6])
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
|
|
|
|
planner.tracking_lead = True
|
|
planner.lead_one.status = True
|
|
planner.lead_one.dRel = 45.0
|
|
planner.lead_one.vLead = 24.8
|
|
planner.lead_one.radar = True
|
|
|
|
v_ego = 58 * CV.MPH_TO_MS
|
|
ccm.update(v_ego, v_ego, sm, toggles)
|
|
ccm.update(v_ego, v_ego, sm, toggles)
|
|
|
|
assert ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["OFF"]
|
|
|
|
|
|
def test_ccm_hard_vetoes_force_experimental(monkeypatch):
|
|
planner, detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
toggles = make_toggles()
|
|
v_ego = 60 * CV.MPH_TO_MS
|
|
v_cruise = v_ego + 5 * CV.MPH_TO_MS
|
|
|
|
veto_scenes = []
|
|
|
|
detector.slow_lead_detected = True
|
|
veto_scenes.append(("slow_lead", make_sm()))
|
|
detector.slow_lead_detected = False
|
|
|
|
traffic_sm = make_sm()
|
|
traffic_sm["starpilotCarState"].trafficModeEnabled = True
|
|
veto_scenes.append(("traffic_mode", traffic_sm))
|
|
|
|
adjacent_sm = make_sm()
|
|
adjacent_sm["starpilotRadarState"].leadLeft = SimpleNamespace(status=True, dRel=25.0, vLead=12.0)
|
|
veto_scenes.append(("adjacent_lead", adjacent_sm))
|
|
|
|
for index, (_name, scene_sm) in enumerate(veto_scenes, start=1):
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda idx=index: float(idx))
|
|
if _name == "slow_lead":
|
|
detector.slow_lead_detected = True
|
|
else:
|
|
detector.slow_lead_detected = False
|
|
ccm.update(v_ego, v_cruise, scene_sm, toggles)
|
|
assert ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["OFF"]
|
|
|
|
planner.starpilot_vcruise.slc.experimental_mode = True
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 10.0)
|
|
ccm.update(v_ego, v_cruise, sm, toggles)
|
|
assert ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["OFF"]
|
|
|
|
|
|
def test_ccm_far_lateral_adjacent_lead_does_not_block_open_road_chill(monkeypatch):
|
|
planner, _detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
toggles = make_toggles()
|
|
monotonic_values = iter([10.0, 10.5])
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
|
|
|
|
sm["starpilotRadarState"].leadLeft = SimpleNamespace(status=True, dRel=18.0, vLead=12.0, yRel=6.5)
|
|
|
|
v_ego = 55 * CV.MPH_TO_MS
|
|
v_cruise = v_ego + 5 * CV.MPH_TO_MS
|
|
ccm.update(v_ego, v_cruise, sm, toggles)
|
|
ccm.update(v_ego, v_cruise, sm, toggles)
|
|
|
|
assert not ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["SPEED"]
|
|
|
|
|
|
def test_ccm_immediate_adjacent_lead_still_blocks_open_road_chill(monkeypatch):
|
|
planner, _detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
toggles = make_toggles()
|
|
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 10.0)
|
|
sm["starpilotRadarState"].leadLeft = SimpleNamespace(status=True, dRel=18.0, vLead=12.0, yRel=4.5)
|
|
|
|
v_ego = 55 * CV.MPH_TO_MS
|
|
v_cruise = v_ego + 5 * CV.MPH_TO_MS
|
|
ccm.update(v_ego, v_cruise, sm, toggles)
|
|
|
|
assert ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["OFF"]
|
|
|
|
|
|
def test_ccm_immediately_exits_chill_when_scene_turns_into_slow_lead(monkeypatch):
|
|
planner, detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
toggles = make_toggles()
|
|
monotonic_values = iter([10.0, 10.5, 10.6])
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
|
|
|
|
planner.tracking_lead = True
|
|
planner.lead_one.status = True
|
|
planner.lead_one.dRel = 40.0
|
|
planner.lead_one.vLead = 24.9
|
|
planner.lead_one.radar = True
|
|
|
|
v_ego = 58 * CV.MPH_TO_MS
|
|
ccm.update(v_ego, v_ego, sm, toggles)
|
|
ccm.update(v_ego, v_ego, sm, toggles)
|
|
assert not ccm.experimental_mode
|
|
|
|
detector.slow_lead_detected = True
|
|
ccm.update(v_ego, v_ego, sm, toggles)
|
|
|
|
assert ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["OFF"]
|
|
|
|
|
|
def test_ccm_launch_assist_enters_chill_from_standstill_when_planner_wants_to_go(monkeypatch):
|
|
planner, _detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
sm["carState"].standstill = True
|
|
toggles = make_toggles()
|
|
toggles.conditional_chill_launch_assist = True
|
|
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 20.0)
|
|
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
|
|
|
|
assert not ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["SPEED"]
|
|
|
|
|
|
def test_ccm_launch_assist_does_not_bypass_real_stop_scene(monkeypatch):
|
|
planner, detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
sm["carState"].standstill = True
|
|
sm["longitudinalPlan"].shouldStop = True
|
|
toggles = make_toggles()
|
|
toggles.conditional_chill_launch_assist = True
|
|
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 21.0)
|
|
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
|
|
|
|
assert ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["OFF"]
|
|
|
|
detector.stop_light_detected = True
|
|
sm["longitudinalPlan"].shouldStop = False
|
|
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
|
|
assert ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["OFF"]
|
|
|
|
|
|
def test_ccm_launch_assist_rejects_red_light_stationary_lead_even_if_planner_says_go(monkeypatch):
|
|
planner, _detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
sm["carState"].standstill = True
|
|
sm["starpilotPlan"].redLight = True
|
|
toggles = make_toggles()
|
|
toggles.conditional_chill_launch_assist = True
|
|
|
|
planner.tracking_lead = True
|
|
planner.lead_one.status = True
|
|
planner.lead_one.dRel = 6.1
|
|
planner.lead_one.vLead = 0.01
|
|
planner.lead_one.vRel = -0.08
|
|
planner.lead_one.aLeadK = 0.0
|
|
planner.lead_one.radar = True
|
|
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 22.0)
|
|
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
|
|
|
|
assert ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["OFF"]
|
|
|
|
|
|
def test_ccm_launch_assist_requires_real_lead_departure_signal(monkeypatch):
|
|
planner, _detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
sm["carState"].standstill = True
|
|
toggles = make_toggles()
|
|
toggles.conditional_chill_launch_assist = True
|
|
|
|
planner.tracking_lead = True
|
|
planner.lead_one.status = True
|
|
planner.lead_one.dRel = 18.0
|
|
planner.lead_one.vLead = 0.12
|
|
planner.lead_one.vRel = 0.05
|
|
planner.lead_one.aLeadK = 0.02
|
|
planner.lead_one.radar = True
|
|
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 23.0)
|
|
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
|
|
|
|
assert ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["OFF"]
|
|
|
|
planner.lead_one.vLead = 0.8
|
|
planner.lead_one.vRel = 0.5
|
|
planner.lead_one.aLeadK = 0.2
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 23.2)
|
|
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
|
|
|
|
assert not ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["LEAD"]
|
|
|
|
|
|
def test_ccm_launch_assist_exits_once_launch_speed_is_reached(monkeypatch):
|
|
planner, _detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
sm["carState"].standstill = True
|
|
toggles = make_toggles()
|
|
toggles.conditional_chill_launch_assist = True
|
|
monotonic_values = iter([30.0, 30.2])
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
|
|
|
|
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
|
|
assert not ccm.experimental_mode
|
|
|
|
sm["carState"].standstill = False
|
|
ccm.update(16 * CV.MPH_TO_MS, 25 * CV.MPH_TO_MS, sm, toggles)
|
|
|
|
assert ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["OFF"]
|
|
|
|
|
|
def test_ccm_launch_assist_exits_immediately_if_lead_slows_again(monkeypatch):
|
|
planner, _detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
sm["carState"].standstill = True
|
|
toggles = make_toggles()
|
|
toggles.conditional_chill_launch_assist = True
|
|
monotonic_values = iter([40.0, 40.1])
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
|
|
|
|
planner.tracking_lead = True
|
|
planner.lead_one.status = True
|
|
planner.lead_one.dRel = 18.0
|
|
planner.lead_one.vLead = 2.0
|
|
planner.lead_one.vRel = 2.0
|
|
planner.lead_one.radar = True
|
|
|
|
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
|
|
assert not ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["LEAD"]
|
|
|
|
sm["carState"].standstill = False
|
|
planner.lead_one.vLead = 0.2
|
|
planner.lead_one.vRel = 0.0
|
|
planner.lead_one.aLeadK = -0.4
|
|
ccm.update(3 * CV.MPH_TO_MS, 25 * CV.MPH_TO_MS, sm, toggles)
|
|
|
|
assert ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["OFF"]
|
|
|
|
|
|
def test_ccm_respects_manual_chill_override(monkeypatch):
|
|
planner, _detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
toggles = make_toggles()
|
|
planner.params_memory.put_int("CCStatus", CCStatus["USER_CHILL"])
|
|
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 1.0)
|
|
ccm.update(55 * CV.MPH_TO_MS, 60 * CV.MPH_TO_MS, sm, toggles)
|
|
|
|
assert not ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["USER_CHILL"]
|
|
|
|
|
|
def test_ccm_launch_assist_is_disabled_by_default(monkeypatch):
|
|
planner, _detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
sm["carState"].standstill = True
|
|
toggles = make_toggles()
|
|
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 50.0)
|
|
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
|
|
|
|
assert ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["OFF"]
|
|
|
|
|
|
def test_ccm_restores_persisted_manual_experimental_override(monkeypatch):
|
|
planner, _detector, ccm = make_ccm()
|
|
sm = make_sm()
|
|
toggles = make_toggles()
|
|
planner.params.put_bool("PersistChillState", True)
|
|
planner.params.put_int("PersistedCCStatus", CCStatus["USER_EXPERIMENTAL"])
|
|
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 1.0)
|
|
ccm.update(55 * CV.MPH_TO_MS, 60 * CV.MPH_TO_MS, sm, toggles)
|
|
|
|
assert ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["USER_EXPERIMENTAL"]
|
|
assert planner.params_memory.get_int("CCStatus") == CCStatus["USER_EXPERIMENTAL"]
|
|
|
|
|
|
def test_ccm_adjacent_lead_veto_works_with_submaster_like_input(monkeypatch):
|
|
planner, _detector, ccm = make_ccm()
|
|
sm = make_submaster_sm()
|
|
toggles = make_toggles()
|
|
sm["starpilotRadarState"].leadLeft = SimpleNamespace(status=True, dRel=20.0, vLead=12.0)
|
|
|
|
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 10.0)
|
|
ccm.update(60 * CV.MPH_TO_MS, 65 * CV.MPH_TO_MS, sm, toggles)
|
|
|
|
assert ccm.experimental_mode
|
|
assert ccm.status_value == CCStatus["OFF"]
|