mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 13:33:42 +08:00
firestar5683
d8708b65c7
Implement conditional driving modes with explicit scene, button and traffic state ownership. Co-authored-by: Dom <168790843+firestar5683@users.noreply.github.com> Co-authored-by: Harald Schäfer <harald.the.engineer@gmail.com> Co-authored-by: whoisdomi <dcantu83@gmail.com>
211 lines
9.5 KiB
Python
211 lines
9.5 KiB
Python
"""Frozen stop-scene filtering with explicit current-message and owner inputs."""
|
|
|
|
from dataclasses import replace
|
|
import unittest
|
|
|
|
from openpilot.cereal import messaging
|
|
from openpilot.starpilot.conditional_mode.stop import StopFrame, StopLead, StopLightDetector
|
|
|
|
|
|
def wire_model(horizon_m: float, should_stop: bool):
|
|
event = messaging.new_message('modelV2', valid=True)
|
|
event.modelV2.timestampEof = 2_000_000_000
|
|
event.modelV2.position.x = [horizon_m * index / 32 for index in range(33)]
|
|
event.modelV2.position.y = [0.0] * 33
|
|
event.modelV2.action.shouldStop = should_stop
|
|
return messaging.log_from_bytes(event.to_bytes()).modelV2
|
|
|
|
|
|
def frame(
|
|
tick: int,
|
|
*,
|
|
horizon=40.0,
|
|
speed=10.0,
|
|
lead=None,
|
|
standstill=False,
|
|
stop_sign=False,
|
|
dashboard_sign=False,
|
|
pedal=False,
|
|
forcing=False,
|
|
traffic=False,
|
|
left=False,
|
|
right=False,
|
|
steering=0.0,
|
|
measured_curve=False,
|
|
model_time=8.0,
|
|
model_should_stop=False,
|
|
):
|
|
return StopFrame(
|
|
observed_mono_s=100.0 + tick * 0.05,
|
|
now_mono_s=100.005 + tick * 0.05,
|
|
speed_mps=speed,
|
|
model_horizon_m=horizon,
|
|
model_stop_time_s=model_time,
|
|
traffic_mode=traffic,
|
|
stop_sign_confirmed=stop_sign,
|
|
forcing_stop=forcing,
|
|
lead=lead if lead is not None else StopLead(False),
|
|
standstill=standstill,
|
|
left_blinker=left,
|
|
right_blinker=right,
|
|
steering_angle_deg=steering,
|
|
driving_in_curve=measured_curve,
|
|
car_fingerprint='HYUNDAI_IONIQ_6',
|
|
dashboard_stop_sign=dashboard_sign,
|
|
pedal_override=pedal,
|
|
model_should_stop=model_should_stop,
|
|
)
|
|
|
|
|
|
class TestStopDetector(unittest.TestCase):
|
|
def test_standstill_model_threshold_replay_has_spatial_release_band(self):
|
|
detector = StopLightDetector()
|
|
# Preserve the 50 m entry threshold across jitter without a timer.
|
|
first = detector.step(frame(0, horizon=50.1, speed=0., standstill=True))
|
|
self.assertFalse(first.standstill_hold)
|
|
for tick, horizon in enumerate((49.9, 50.1, 49.9, 50.1, 53.999), 1):
|
|
current = detector.step(frame(tick, horizon=horizon, speed=0., standstill=True))
|
|
self.assertTrue(current.standstill_hold)
|
|
self.assertEqual(current.standstill_reason, 'light')
|
|
released = detector.step(frame(6, horizon=54., speed=0., standstill=True))
|
|
self.assertFalse(released.standstill_hold)
|
|
self.assertFalse(detector.step(frame(7, horizon=50.1, speed=0., standstill=True)).standstill_hold)
|
|
|
|
def test_standstill_model_band_withdraws_on_pedal_movement_and_unknown(self):
|
|
for withdrawal in ({'pedal': True}, {'pedal': None}, {'dashboard_sign': None}, {'standstill': False}, {'traffic': None}):
|
|
detector = StopLightDetector()
|
|
self.assertTrue(detector.step(frame(0, horizon=49.9, speed=0., standstill=True)).standstill_hold)
|
|
values = {'horizon': 51., 'speed': 0., 'standstill': True}
|
|
values.update(withdrawal)
|
|
detector.step(frame(1, **values))
|
|
self.assertFalse(detector.step(frame(2, horizon=51., speed=0., standstill=True)).standstill_hold)
|
|
|
|
def test_speed_dependent_light_boost_and_cap_match_stop_horizon(self):
|
|
# Below35mph: model seconds unchanged. Above45mph:1.2x, capped9s.
|
|
for mph, seconds, adjusted in ((25., 8., 8.), (35., 8., 8.), (40., 8., 8.5),
|
|
(45., 8., 9.), (60., 5., 6.), (75., 10., 9.)):
|
|
with self.subTest(mph=mph, seconds=seconds):
|
|
threshold = mph * .44704 * adjusted
|
|
below = StopLightDetector().step(frame(0, speed=mph * .44704, horizon=threshold - 2.501, model_time=seconds))
|
|
above = StopLightDetector().step(frame(0, speed=mph * .44704, horizon=threshold - 2.499, model_time=seconds))
|
|
self.assertTrue(below.model_stopping)
|
|
self.assertFalse(above.model_stopping)
|
|
|
|
def test_current_should_stop_not_frozen_stop_light_alias(self):
|
|
model = wire_model(192.0, True)
|
|
self.assertTrue(model.action.shouldStop)
|
|
detector = StopLightDetector()
|
|
samples = [detector.step(frame(tick, horizon=float(model.position.x[-1]))) for tick in range(25)]
|
|
self.assertTrue(all(sample.model_stopping is False for sample in samples))
|
|
self.assertFalse(samples[-1].light_detected)
|
|
short = wire_model(30.0, False)
|
|
self.assertFalse(short.action.shouldStop)
|
|
samples = [detector.step(frame(tick + 25, horizon=float(short.position.x[-1]))) for tick in range(25)]
|
|
self.assertTrue(samples[-1].light_detected)
|
|
|
|
def test_relevant_radar_lead_blocks_false_light_and_vision_handoff(self):
|
|
detector = StopLightDetector()
|
|
radar = StopLead(True, distance_m=30.0, speed_mps=7.0, radar=True, model_probability=0.95, tracked=True)
|
|
blocked = [detector.step(frame(tick, lead=radar)) for tick in range(30)]
|
|
self.assertFalse(blocked[-1].light_detected)
|
|
vision = StopLead(True, distance_m=10.0, speed_mps=1.0, radar=False, model_probability=0.95, tracked=False)
|
|
detector.reset()
|
|
handoff = detector.step(frame(0, horizon=2.0, speed=1.0, lead=vision))
|
|
self.assertTrue(handoff.light_detected)
|
|
|
|
def test_standstill_sign_retains_until_pedal_and_light_can_release(self):
|
|
detector = StopLightDetector()
|
|
sign = detector.step(frame(0, horizon=192, speed=0, standstill=True, stop_sign=True, dashboard_sign=False, pedal=False))
|
|
self.assertTrue(sign.light_detected)
|
|
self.assertTrue(sign.standstill_hold)
|
|
self.assertEqual(sign.standstill_reason, 'sign')
|
|
lost = detector.step(frame(1, horizon=192, speed=0, standstill=True, dashboard_sign=False, pedal=False))
|
|
self.assertTrue(lost.standstill_hold) # Sign reason is retained at standstill.
|
|
release = detector.step(frame(2, horizon=192, speed=0, standstill=True, dashboard_sign=False, pedal=True))
|
|
self.assertFalse(release.standstill_hold)
|
|
self.assertIsNone(release.standstill_reason)
|
|
# A pure light/model hold has no stop-sign latch after its model clears.
|
|
light = StopLightDetector()
|
|
for tick in range(25):
|
|
output = light.step(frame(tick, horizon=30.0, speed=10.0))
|
|
self.assertTrue(output.light_detected)
|
|
at_stop = light.step(frame(25, horizon=30.0, speed=0.0, standstill=True))
|
|
self.assertTrue(at_stop.standstill_hold)
|
|
self.assertEqual(at_stop.standstill_reason, 'light')
|
|
for tick in range(26, 126):
|
|
cleared = light.step(frame(tick, horizon=192.0, speed=0.0, standstill=True))
|
|
self.assertFalse(cleared.standstill_hold)
|
|
self.assertIsNone(cleared.standstill_reason)
|
|
|
|
def test_turn_high_speed_and_unknown_source_reset(self):
|
|
detector = StopLightDetector()
|
|
for tick in range(25):
|
|
result = detector.step(frame(tick))
|
|
self.assertTrue(result.light_detected)
|
|
turn = detector.step(frame(25, speed=5.0, left=True, steering=45.0))
|
|
self.assertTrue(turn.light_detected)
|
|
self.assertTrue(detector.committed)
|
|
for tick in range(26, 50):
|
|
detector.step(frame(tick))
|
|
high = detector.step(frame(50, speed=34.0))
|
|
self.assertTrue(high.light_detected)
|
|
unknown = detector.step(replace(frame(51), traffic_mode=None))
|
|
self.assertIsNone(unknown.light_detected)
|
|
self.assertFalse(detector.light_detected)
|
|
|
|
def test_confirmed_sign_precedes_turn_and_missing_active_curve_stays_unknown(self):
|
|
detector = StopLightDetector()
|
|
sign = detector.step(frame(0, speed=5.0, left=True, steering=45.0, stop_sign=True))
|
|
self.assertTrue(sign.light_detected)
|
|
missing = detector.step(frame(1, speed=5.0, left=True, steering=45.0, measured_curve=None))
|
|
self.assertIsNone(missing.light_detected)
|
|
self.assertFalse(detector.light_detected)
|
|
recovered = detector.step(frame(2, speed=5.0, left=True, steering=44.0, measured_curve=True))
|
|
self.assertFalse(recovered.light_detected)
|
|
self.assertFalse(detector.light_detected)
|
|
|
|
def test_repeated_source_gap_and_missing_sign_owner(self):
|
|
detector = StopLightDetector()
|
|
first = frame(0)
|
|
self.assertFalse(detector.step(first).light_detected)
|
|
value = detector.light_filter.x
|
|
repeated = detector.step(replace(first, now_mono_s=100.02))
|
|
self.assertIsNone(repeated.light_detected)
|
|
self.assertEqual(detector.light_filter.x, value)
|
|
gap = detector.step(frame(6))
|
|
self.assertIsNone(gap.light_detected)
|
|
self.assertEqual(detector.light_filter.x, 0)
|
|
missing = detector.step(replace(frame(7), stop_sign_confirmed=None))
|
|
self.assertIsNone(missing.light_detected)
|
|
|
|
def test_model_cadence_advances_with_staggered_fresh_owner_observations(self):
|
|
detector = StopLightDetector()
|
|
for tick in range(28):
|
|
model_time = 100.0 + tick * 0.05
|
|
owner_time = 100.0 + (tick // 2) * 0.1
|
|
result = detector.step(replace(frame(tick), observed_mono_s=owner_time, model_tick_mono_s=model_time))
|
|
self.assertTrue(result.light_detected)
|
|
value = detector.light_filter.x
|
|
repeated = detector.step(replace(frame(27), observed_mono_s=model_time, model_tick_mono_s=model_time, now_mono_s=model_time + 0.01))
|
|
self.assertIsNone(repeated.light_detected)
|
|
self.assertEqual(detector.light_filter.x, value)
|
|
|
|
def test_exact_two_frame_gap_does_not_reset_the_stop_filter(self):
|
|
detector = StopLightDetector()
|
|
for tick in range(40):
|
|
stamp = 100.0 + tick / 10
|
|
output = detector.step(replace(frame(tick), observed_mono_s=stamp, now_mono_s=stamp + .005))
|
|
self.assertIsNotNone(output.light_detected)
|
|
self.assertTrue(output.light_detected)
|
|
stamp += .100000001
|
|
output = detector.step(replace(frame(40), observed_mono_s=stamp, now_mono_s=stamp + .005))
|
|
self.assertTrue(output.light_detected)
|
|
stamp += .250000001
|
|
output = detector.step(replace(frame(41), observed_mono_s=stamp, now_mono_s=stamp + .005))
|
|
self.assertIsNone(output.light_detected)
|
|
self.assertFalse(detector.light_detected)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
unittest.main()
|