mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 13:33:42 +08:00
firestar5683
83 lines
3.7 KiB
Python
83 lines
3.7 KiB
Python
from dataclasses import replace
|
|
|
|
from openpilot.starpilot.conditional_mode.policy import Authority, ConditionalModePolicy, ManualIntent, ModeChoice, SceneEvidence
|
|
from openpilot.starpilot.conditional_mode.stop import StopLightDetector
|
|
from openpilot.starpilot.conditional_mode.tests.test_stop import frame
|
|
|
|
|
|
def prime():
|
|
detector = StopLightDetector()
|
|
for tick in range(30):
|
|
result = detector.step(frame(tick))
|
|
assert result.light_detected and detector.committed
|
|
return detector
|
|
|
|
|
|
def test_acquired_stop_survives_fresh_invalid_geometry_until_valid_clear():
|
|
detector = prime()
|
|
for tick in range(30, 140):
|
|
result = detector.step(frame(tick, horizon=None, speed=4.27))
|
|
assert result.light_detected and detector.committed
|
|
for tick in range(140, 160):
|
|
result = detector.step(frame(tick, horizon=192., speed=4.27))
|
|
assert not result.light_detected and not detector.committed
|
|
|
|
|
|
def test_traffic_and_turn_cannot_abandon_short_committed_stop():
|
|
for changes in ({'traffic': True}, {'left': True, 'steering': 45.}):
|
|
detector = prime()
|
|
for tick in range(30, 140):
|
|
result = detector.step(frame(tick, horizon=6.684, speed=4.27, **changes))
|
|
assert result.light_detected and detector.committed
|
|
|
|
|
|
def test_unknown_source_and_cold_invalid_geometry_cannot_authorize_commit():
|
|
detector = StopLightDetector()
|
|
assert detector.step(frame(0, horizon=None)).light_detected is None
|
|
assert not detector.committed
|
|
detector = prime()
|
|
assert detector.step(replace(frame(30), traffic_mode=None)).light_detected is None
|
|
assert detector.committed
|
|
assert not detector.standstill_committed
|
|
|
|
|
|
def test_manual_chill_and_personality_mode_change_do_not_override_committed_stop():
|
|
policy = ConditionalModePolicy()
|
|
authority = Authority(True, True, False, True, True, True)
|
|
for tick, choice in enumerate((ModeChoice.CEM, ModeChoice.CCM, ModeChoice.CEM)):
|
|
now = 100. + tick * .05
|
|
scene = SceneEvidence(observed_mono_s=now, speed_mps=4.27, committed_stop=True)
|
|
result = policy.step(now, choice, ManualIntent.FORCE_CHILL, authority, scene)
|
|
assert result.requested_experimental and result.reason.value == 'cem_stop'
|
|
scene = replace(scene, observed_mono_s=now+.05, committed_stop=False)
|
|
result = policy.step(now+.05, ModeChoice.CEM, ManualIntent.FORCE_CHILL, authority, scene)
|
|
assert not result.requested_experimental and result.reason.value == 'manual_chill'
|
|
|
|
|
|
def test_pedal_override_releases_acquired_stop_without_authority_recreation():
|
|
detector = prime()
|
|
result = detector.step(frame(30, horizon=6.684, speed=4.27, pedal=True))
|
|
assert not result.light_detected and not detector.committed
|
|
|
|
|
|
def test_standstill_commit_survives_unknowns_and_requires_continuous_clear_evidence():
|
|
detector = prime()
|
|
detector.step(frame(30, speed=0., standstill=True, horizon=6., model_should_stop=True))
|
|
assert detector.standstill_committed
|
|
for tick in range(31, 45):
|
|
observation = detector.step(replace(frame(tick), traffic_mode=None))
|
|
assert observation.light_detected is None
|
|
assert detector.standstill_committed
|
|
for tick in range(45, 65):
|
|
detector.step(frame(tick, speed=0., standstill=True, horizon=192., model_should_stop=True))
|
|
assert detector.standstill_committed
|
|
for tick in range(65, 71):
|
|
detector.step(frame(tick, speed=0., standstill=True, horizon=192., model_should_stop=False))
|
|
detector.step(replace(frame(71), traffic_mode=None))
|
|
for tick in range(72, 79):
|
|
detector.step(frame(tick, speed=0., standstill=True, horizon=192., model_should_stop=False))
|
|
assert detector.standstill_committed # Unknown time contributes no clear dwell.
|
|
for tick in range(79, 86):
|
|
detector.step(frame(tick, speed=0., standstill=True, horizon=192., model_should_stop=False))
|
|
assert not detector.committed and not detector.standstill_committed
|