mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-28 01:52:06 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
c91ac533efafcd5df701367244a42861baaae424
StarPilot/selfdrive/controls/tests/test_conditional_experimental_mode.py
T
firestar5683 6b0e5aed24 I did the backstroke in college
2026-06-16 21:19:57 -05:00

794 lines
25 KiB
Python
Raw Blame History

from pathlib import Path
from types import SimpleNamespace
from openpilot.common.constants import CV
from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner
import openpilot.starpilot.controls.starpilot_planner as starpilot_planner_module
import openpilot.starpilot.controls.lib.conditional_experimental_mode as conditional_experimental_mode_module
from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
class FakeParams:
def __init__(self, initial=None):
self._store = dict(initial or {})
def get_bool(self, key):
return bool(self._store.get(key, False))
def put_bool(self, key, value):
self._store[key] = bool(value)
def get_int(self, key, default=0):
return int(self._store.get(key, default))
def put_int(self, key, value):
self._store[key] = int(value)
def make_cem(*, model_length: float, model_stopped: bool = False, tracking_lead: bool = False,
lead_status: bool = False, lead_d_rel: float = float("inf"),
lead_v_lead: float = 0.0, lead_model_prob: float = 0.0, lead_radar: bool = False,
stop_sign_confirmed: bool = False, forcing_stop: bool = False):
planner = SimpleNamespace(
params=FakeParams(),
params_memory=FakeParams(),
model_length=model_length,
model_stopped=model_stopped,
tracking_lead=tracking_lead,
starpilot_vcruise=SimpleNamespace(
stop_sign_confirmed=stop_sign_confirmed,
forcing_stop=forcing_stop,
slc=SimpleNamespace(experimental_mode=False),
),
starpilot_following=SimpleNamespace(slower_lead=False, following_lead=False),
lead_one=SimpleNamespace(status=lead_status, dRel=lead_d_rel, vLead=lead_v_lead,
modelProb=lead_model_prob, radar=lead_radar),
road_curvature_detected=False,
driving_in_curve=False,
lane_width_left=0.0,
lane_width_right=0.0,
)
return ConditionalExperimentalMode(planner)
def make_sm(traffic_mode_enabled: bool = False):
return {
"starpilotCarState": SimpleNamespace(trafficModeEnabled=traffic_mode_enabled),
}
def run_stop_light_detector(cem, v_ego, *, steps: int, tracking_lead: bool = False):
for _ in range(steps):
cem.starpilot_planner.tracking_lead = tracking_lead
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
def make_update_sm(*, standstill: bool):
return {
"carState": SimpleNamespace(standstill=standstill, leftBlinker=False, rightBlinker=False, gasPressed=False),
"starpilotCarState": SimpleNamespace(trafficModeEnabled=False, dashboardStopSign=0, accelPressed=False),
}
def make_update_toggles():
return SimpleNamespace(
conditional_limit=0.0,
conditional_limit_lead=0.0,
conditional_signal=0.0,
conditional_signal_lane_detection=False,
lane_detection_width=0.0,
conditional_curves=False,
conditional_curves_lead=False,
conditional_lead=False,
conditional_model_stop_time=7.0,
conditional_slower_lead=False,
conditional_stopped_lead=False,
)
def test_low_speed_cruise_does_not_trigger_stop_light_from_model_stopped():
v_ego = 10 * CV.MPH_TO_MS
model_length = v_ego * 10.0
cem = make_cem(model_length=model_length, model_stopped=True)
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert not cem.stop_light_detected
def test_predicted_stop_within_threshold_triggers_stop_light():
v_ego = 30 * CV.MPH_TO_MS
model_length = v_ego * 4.0
cem = make_cem(model_length=model_length)
run_stop_light_detector(cem, v_ego, steps=20)
assert cem.stop_light_detected
def test_chattering_lead_does_not_trigger_stop_light():
v_ego = 22 * CV.MPH_TO_MS
model_length = v_ego * 4.0
cem = make_cem(model_length=model_length)
for i in range(30):
cem.starpilot_planner.tracking_lead = (i % 2 == 0)
cem.starpilot_planner.lead_one.status = (i % 2 == 0)
cem.starpilot_planner.lead_one.dRel = model_length + 5.0
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert not cem.stop_light_detected
def test_close_visible_but_untracked_lead_blocks_stop_light():
v_ego = 22 * CV.MPH_TO_MS
model_length = v_ego * 4.0
cem = make_cem(model_length=model_length, lead_status=True, lead_d_rel=model_length + 5.0)
run_stop_light_detector(cem, v_ego, steps=30)
assert not cem.stop_light_detected
def test_far_visible_lead_does_not_block_stop_light():
v_ego = 22 * CV.MPH_TO_MS
model_length = v_ego * 4.0
cem = make_cem(model_length=model_length, lead_status=True, lead_d_rel=v_ego * 7.0 + 30.0)
run_stop_light_detector(cem, v_ego, steps=30)
assert cem.stop_light_detected
def test_stop_light_stays_latched_until_untracked_stopped_lead_handoff():
v_ego = 45 * CV.MPH_TO_MS
model_length = v_ego * 4.0
cem = make_cem(model_length=model_length)
run_stop_light_detector(cem, v_ego, steps=30)
assert cem.stop_light_detected
cem.starpilot_planner.lead_one.status = True
cem.starpilot_planner.lead_one.dRel = model_length - 5.0
cem.starpilot_planner.lead_one.vLead = 0.5
run_stop_light_detector(cem, v_ego, steps=10, tracking_lead=False)
assert cem.stop_light_detected
def test_stop_light_latch_holds_slow_high_confidence_vision_lead_during_model_flicker(monkeypatch):
v_ego = 40 * CV.MPH_TO_MS
model_length = v_ego * 3.8
cem = make_cem(
model_length=model_length,
lead_status=True,
lead_d_rel=model_length - 5.0,
lead_v_lead=3.0,
lead_model_prob=0.98,
)
run_stop_light_detector(cem, v_ego, steps=20)
assert cem.stop_light_detected
monotonic_values = iter([10.0, 10.2])
monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values))
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
cem.stop_light_detected = False
cem.stop_light_model_detected = False
cem.stop_light_filter.x = 0.0
cem.starpilot_planner.model_length = v_ego * 9.0
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert cem.stop_light_detected
def test_stop_light_hold_bridges_short_no_lead_model_flicker(monkeypatch):
v_ego = 40 * CV.MPH_TO_MS
cem = make_cem(model_length=v_ego * 3.8)
run_stop_light_detector(cem, v_ego, steps=20)
assert cem.stop_light_detected
cem.stop_light_detected_hold_until = 11.75
monotonic_values = iter([10.0, 11.0, 14.5, 16.5, 18.5])
monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values))
cem.starpilot_planner.model_length = v_ego * 9.0
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert cem.stop_light_detected
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert cem.stop_light_detected
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert cem.stop_light_detected
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert cem.stop_light_detected
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert not cem.stop_light_detected
def test_stop_light_hold_refreshes_through_stopped_approach_lead(monkeypatch):
v_ego = 20 * CV.MPH_TO_MS
model_length = v_ego * 4.0
cem = make_cem(
model_length=model_length,
lead_status=True,
lead_d_rel=model_length - 5.0,
lead_v_lead=0.5,
lead_model_prob=0.98,
)
run_stop_light_detector(cem, v_ego, steps=20)
assert cem.stop_light_detected
monotonic_values = iter([20.0, 21.2, 22.4])
monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values))
cem.starpilot_planner.model_length = v_ego * 9.0
cem.stop_light_detected = False
cem.stop_light_model_detected = False
cem.stop_light_filter.x = 0.0
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert cem.stop_light_detected
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert cem.stop_light_detected
cem.starpilot_planner.lead_one.vLead = 6.0
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert not cem.stop_light_detected
def test_stop_light_approach_latch_clears_once_tracked_lead_takes_over(monkeypatch):
v_ego = 20 * CV.MPH_TO_MS
model_length = v_ego * 4.0
cem = make_cem(
model_length=model_length,
lead_status=True,
lead_d_rel=model_length - 5.0,
lead_v_lead=0.5,
lead_model_prob=0.98,
)
run_stop_light_detector(cem, v_ego, steps=20)
assert cem.stop_light_detected
monotonic_values = iter([20.0, 20.2])
monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values))
cem.starpilot_planner.model_length = v_ego * 9.0
cem.stop_light_detected = False
cem.stop_light_model_detected = False
cem.stop_light_filter.x = 0.0
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert cem.stop_light_detected
cem.starpilot_planner.tracking_lead = True
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert not cem.stop_light_detected
def test_stopped_lead_handoff_does_not_hold_cem_on_empty_road(monkeypatch):
v_ego = 40 * CV.MPH_TO_MS
cem = make_cem(
model_length=v_ego * 9.0,
tracking_lead=False,
lead_status=False,
)
cem.stop_light_detected = True
cem.stop_light_model_detected = False
cem.stop_light_filter.x = 0.0
cem.stop_approach_hold_until = 11.0
cem.stop_light_detected_hold_until = 0.0
monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: 10.2)
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert not cem.stop_light_detected
def test_stopped_lead_toggle_does_not_self_trigger_without_a_real_lead():
v_ego = 25 * CV.MPH_TO_MS
cem = make_cem(
model_length=v_ego * 6.0,
tracking_lead=False,
lead_status=False,
lead_v_lead=0.0,
)
toggles = SimpleNamespace(conditional_slower_lead=False, conditional_stopped_lead=True)
for _ in range(24):
cem.slow_lead(toggles, v_ego)
assert not cem.slow_lead_detected
def test_stopped_lead_toggle_still_triggers_for_real_close_stopped_lead():
v_ego = 25 * CV.MPH_TO_MS
cem = make_cem(
model_length=v_ego * 4.0,
tracking_lead=True,
lead_status=True,
lead_d_rel=18.0,
lead_v_lead=0.4,
lead_model_prob=0.99,
)
toggles = SimpleNamespace(conditional_slower_lead=False, conditional_stopped_lead=True)
for _ in range(24):
cem.slow_lead(toggles, v_ego)
assert cem.slow_lead_detected
def test_borderline_empty_road_model_dip_does_not_refresh_long_hold(monkeypatch):
v_ego = 20 * CV.MPH_TO_MS
stop_threshold = v_ego * 7.0
cem = make_cem(model_length=stop_threshold - 4.0)
cem.stop_light_filter.x = conditional_experimental_mode_module.THRESHOLD ** 2
monotonic_values = iter([10.0, 10.2])
monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values))
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert cem.stop_light_detected_hold_until == 0.0
cem.starpilot_planner.model_length = stop_threshold + 20.0
cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
assert not cem.stop_light_detected
def test_standstill_red_light_keeps_exp_on_even_when_model_stopped_clears(monkeypatch):
cem = make_cem(model_length=80.0, model_stopped=False)
toggles = make_update_toggles()
sm = make_update_sm(standstill=True)
cem.stop_light_detected = True
cem.experimental_mode = False
monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None)
cem.update(0.0, sm, toggles)
assert cem.experimental_mode
assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"]
def test_standstill_update_can_activate_exp_from_red_light_detection(monkeypatch):
cem = make_cem(model_length=80.0, model_stopped=False)
toggles = make_update_toggles()
sm = make_update_sm(standstill=True)
def detect_red_light(*args, **kwargs):
cem.stop_light_detected = True
monkeypatch.setattr(cem, "stop_sign_and_light", detect_red_light)
cem.update(0.0, sm, toggles)
assert cem.experimental_mode
assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"]
def test_first_post_standstill_pullaway_frame_does_not_blip_into_exp(monkeypatch):
cem = make_cem(
model_length=70.0,
tracking_lead=True,
lead_status=True,
lead_d_rel=8.4,
lead_v_lead=2.7,
lead_model_prob=0.999,
)
toggles = make_update_toggles()
toggles.conditional_lead = True
toggles.conditional_slower_lead = True
toggles.conditional_stopped_lead = True
standstill_sm = make_update_sm(standstill=True)
moving_sm = make_update_sm(standstill=False)
now = [100.0]
monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0])
cem.update(0.0, standstill_sm, toggles)
assert not cem.experimental_mode
now[0] = 100.1
cem.update(0.12, moving_sm, toggles)
assert not cem.experimental_mode
assert cem.status_value == conditional_experimental_mode_module.CEStatus["OFF"]
def test_standstill_update_can_activate_exp_from_dashboard_stop_sign(monkeypatch):
cem = make_cem(model_length=80.0, model_stopped=False)
toggles = make_update_toggles()
sm = make_update_sm(standstill=True)
sm["starpilotCarState"].dashboardStopSign = 1
monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None)
cem.update(0.0, sm, toggles)
assert cem.experimental_mode
assert cem.standstill_stop_reason == "sign"
assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"]
def test_standstill_green_light_clears_exp_immediately(monkeypatch):
cem = make_cem(model_length=80.0, model_stopped=False)
toggles = make_update_toggles()
sm = make_update_sm(standstill=True)
cem.stop_light_detected = True
cem.experimental_mode = True
def clear_red_light(*args, **kwargs):
cem.stop_light_detected = False
cem.stop_light_model_detected = False
monkeypatch.setattr(cem, "stop_sign_and_light", clear_red_light)
cem.update(0.0, sm, toggles)
assert not cem.experimental_mode
assert cem.status_value == conditional_experimental_mode_module.CEStatus["OFF"]
def test_standstill_dashboard_stop_sign_keeps_exp_on(monkeypatch):
cem = make_cem(model_length=80.0, model_stopped=False, stop_sign_confirmed=True)
toggles = make_update_toggles()
sm = make_update_sm(standstill=True)
monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None)
cem.update(0.0, sm, toggles)
assert cem.experimental_mode
assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"]
def test_standstill_force_stop_keeps_exp_on_even_if_red_light_latch_clears(monkeypatch):
cem = make_cem(model_length=80.0, model_stopped=False, forcing_stop=True)
toggles = make_update_toggles()
sm = make_update_sm(standstill=True)
cem.stop_light_detected = True
cem.experimental_mode = True
def clear_red_light(*args, **kwargs):
cem.stop_light_detected = False
cem.stop_light_model_detected = False
monkeypatch.setattr(cem, "stop_sign_and_light", clear_red_light)
cem.update(0.0, sm, toggles)
assert cem.experimental_mode
assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"]
def test_standstill_stop_sign_latches_until_pedal_even_after_force_stop_ends(monkeypatch):
cem = make_cem(model_length=80.0, model_stopped=False)
toggles = make_update_toggles()
sm = make_update_sm(standstill=True)
sm["starpilotCarState"].dashboardStopSign = 1
monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None)
cem.update(0.0, sm, toggles)
assert cem.experimental_mode
assert cem.standstill_stop_reason == "sign"
sm["starpilotCarState"].dashboardStopSign = 0
cem.update(0.0, sm, toggles)
assert cem.experimental_mode
assert cem.standstill_stop_reason == "sign"
def test_standstill_stop_sign_releases_on_pedal(monkeypatch):
cem = make_cem(model_length=80.0, model_stopped=False)
toggles = make_update_toggles()
sm = make_update_sm(standstill=True)
sm["starpilotCarState"].dashboardStopSign = 1
monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None)
cem.update(0.0, sm, toggles)
assert cem.experimental_mode
sm["starpilotCarState"].dashboardStopSign = 0
sm["carState"].gasPressed = True
cem.update(0.0, sm, toggles)
assert not cem.experimental_mode
assert cem.standstill_stop_reason is None
def test_slow_lead_holds_through_tracking_flap_for_high_confidence_vision_lead():
v_ego = 35 * CV.MPH_TO_MS
cem = make_cem(
model_length=v_ego * 5.0,
tracking_lead=True,
lead_status=True,
lead_d_rel=v_ego * 3.5,
lead_v_lead=8.0 * CV.MPH_TO_MS,
lead_model_prob=0.95,
)
toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False)
cem.slow_lead_filter.x = 1.0
cem.slow_lead_detected = True
cem.starpilot_planner.tracking_lead = False
cem.starpilot_planner.starpilot_following.slower_lead = False
cem.slow_lead(toggles, v_ego)
assert cem.slow_lead_detected
def test_untracked_raw_slow_lead_does_not_self_trigger_without_recent_tracking(monkeypatch):
v_ego = 35 * CV.MPH_TO_MS
cem = make_cem(
model_length=v_ego * 5.0,
tracking_lead=False,
lead_status=True,
lead_d_rel=v_ego * 3.0,
lead_v_lead=8.0 * CV.MPH_TO_MS,
lead_model_prob=0.95,
)
toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False)
monotonic_values = iter([20.0 + 0.1 * i for i in range(24)])
monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values))
for _ in range(12):
cem.slow_lead(toggles, v_ego)
assert not cem.slow_lead_detected
def test_untracked_raw_slow_lead_continuity_expires_after_tracking_flap(monkeypatch):
v_ego = 35 * CV.MPH_TO_MS
cem = make_cem(
model_length=v_ego * 5.0,
tracking_lead=True,
lead_status=True,
lead_d_rel=v_ego * 3.0,
lead_v_lead=8.0 * CV.MPH_TO_MS,
lead_model_prob=0.95,
)
toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False)
cem.slow_lead_filter.x = 1.0
cem.slow_lead_detected = True
monotonic_values = iter([30.0, 30.1, 31.6])
monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values))
cem.slow_lead(toggles, v_ego)
assert cem.slow_lead_detected
cem.starpilot_planner.tracking_lead = False
cem.starpilot_planner.starpilot_following.slower_lead = False
cem.slow_lead(toggles, v_ego)
assert cem.slow_lead_detected
cem.slow_lead(toggles, v_ego)
assert not cem.slow_lead_detected
def test_tracked_highway_mild_closing_lead_does_not_trigger_raw_slow_lead():
v_ego = 65 * CV.MPH_TO_MS
cem = make_cem(
model_length=v_ego * 5.0,
tracking_lead=True,
lead_status=True,
lead_d_rel=46.0,
lead_v_lead=v_ego - 1.4,
lead_model_prob=1.0,
)
toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False)
cem.slow_lead_filter.x = 0.0
cem.slow_lead_detected = False
cem.starpilot_planner.starpilot_following.slower_lead = False
for _ in range(12):
cem.slow_lead(toggles, v_ego)
assert not cem.slow_lead_detected
def test_tracked_following_slower_lead_still_triggers_slow_lead():
v_ego = 65 * CV.MPH_TO_MS
cem = make_cem(
model_length=v_ego * 5.0,
tracking_lead=True,
lead_status=True,
lead_d_rel=36.0,
lead_v_lead=v_ego - 3.0,
lead_model_prob=1.0,
)
toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False)
cem.slow_lead_filter.x = 0.0
cem.slow_lead_detected = False
cem.starpilot_planner.starpilot_following.slower_lead = True
for _ in range(24):
cem.slow_lead(toggles, v_ego)
assert cem.slow_lead_detected
def test_slow_lead_does_not_linger_at_crawl_when_stopped_lead_disabled():
v_ego = 1.5
cem = make_cem(
model_length=20.0,
tracking_lead=True,
lead_status=True,
lead_d_rel=8.0,
lead_v_lead=1.2,
lead_model_prob=0.99,
)
toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False)
cem.slow_lead_filter.x = 1.0
cem.slow_lead_detected = True
cem.starpilot_planner.starpilot_following.slower_lead = False
cem.slow_lead(toggles, v_ego)
assert not cem.slow_lead_detected
def test_far_untracked_slow_lead_does_not_trigger_slow_lead():
v_ego = 50 * CV.MPH_TO_MS
cem = make_cem(
model_length=v_ego * 5.0,
tracking_lead=False,
lead_status=True,
lead_d_rel=v_ego * 4.8,
lead_v_lead=v_ego - 2.0,
lead_model_prob=0.97,
)
toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False)
cem.slow_lead_filter.x = 1.0
cem.slow_lead_detected = True
cem.starpilot_planner.starpilot_following.slower_lead = False
cem.slow_lead(toggles, v_ego)
assert not cem.slow_lead_detected
def test_pace_matched_lead_clears_slow_lead_quickly():
v_ego = 30 * CV.MPH_TO_MS
cem = make_cem(
model_length=v_ego * 4.0,
tracking_lead=True,
lead_status=True,
lead_d_rel=24.0,
lead_v_lead=v_ego + 0.2,
lead_model_prob=0.99,
)
toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False)
cem.slow_lead_filter.x = 1.0
cem.slow_lead_detected = True
cem.starpilot_planner.starpilot_following.slower_lead = False
cem.slow_lead(toggles, v_ego)
assert not cem.slow_lead_detected
def test_tracked_stale_slow_lead_clears_after_short_timeout(monkeypatch):
v_ego = 65 * CV.MPH_TO_MS
cem = make_cem(
model_length=v_ego * 5.0,
tracking_lead=True,
lead_status=True,
lead_d_rel=55.0,
lead_v_lead=v_ego - 0.8,
lead_model_prob=0.98,
)
toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False)
cem.slow_lead_filter.x = 1.0
cem.slow_lead_detected = True
cem.starpilot_planner.starpilot_following.slower_lead = False
monotonic_values = iter([50.0, 50.3, 50.9])
monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values))
cem.slow_lead(toggles, v_ego)
assert cem.slow_lead_detected
cem.slow_lead(toggles, v_ego)
assert cem.slow_lead_detected
cem.slow_lead(toggles, v_ego)
assert not cem.slow_lead_detected
class DummyThemeManager:
def update_wheel_image(self, *args, **kwargs):
pass
def test_starpilot_planner_updates_cem_with_current_frame_state(monkeypatch):
planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager())
try:
monkeypatch.setattr(starpilot_planner_module, "calculate_road_curvature", lambda model, v_ego: (0.01, 1.0))
monkeypatch.setattr(planner.starpilot_acceleration, "update", lambda *args, **kwargs: None)
monkeypatch.setattr(planner.starpilot_events, "update", lambda *args, **kwargs: None)
monkeypatch.setattr(planner.starpilot_vcruise, "update", lambda *args, **kwargs: 0.0)
monkeypatch.setattr(planner.starpilot_weather, "update_weather", lambda *args, **kwargs: None)
def following_update(*args, **kwargs):
planner.starpilot_following.following_lead = planner.tracking_lead
planner.starpilot_following.slower_lead = planner.tracking_lead
monkeypatch.setattr(planner.starpilot_following, "update", following_update)
seen = {}
def cem_update(v_ego, sm, starpilot_toggles):
seen.update({
"tracking_lead": planner.tracking_lead,
"following_lead": planner.starpilot_following.following_lead,
"slower_lead": planner.starpilot_following.slower_lead,
"model_length": planner.model_length,
"driving_in_curve": planner.driving_in_curve,
"road_curvature_detected": planner.road_curvature_detected,
})
monkeypatch.setattr(planner.starpilot_cem, "update", cem_update)
planner.model_length = 0.0
planner.tracking_lead = False
planner.driving_in_curve = False
planner.lateral_acceleration = 0.0
planner.road_curvature_detected = False
planner.starpilot_following.following_lead = False
planner.starpilot_following.slower_lead = False
starpilot_toggles = SimpleNamespace(
set_speed_offset=0,
conditional_experimental_mode=True,
minimum_lane_change_speed=100.0,
pause_lateral_below_speed=0.0,
pause_lateral_below_signal=False,
stop_distance=6.0,
weather_presets=False,
)
for controls_enabled, aol_enabled in ((True, False), (False, True)):
seen.clear()
sm = {
"radarState": SimpleNamespace(leadOne=SimpleNamespace(status=True, dRel=25.0, vLead=0.5)),
"selfdriveState": SimpleNamespace(enabled=controls_enabled),
"carState": SimpleNamespace(vCruise=50.0, vEgo=20.0, standstill=False, leftBlinker=False, rightBlinker=False),
"controlsState": SimpleNamespace(curvature=0.02),
"modelV2": SimpleNamespace(position=SimpleNamespace(x=[0.0, 30.0]), laneLines=[None] * 4, roadEdges=[None] * 2),
"starpilotCarState": SimpleNamespace(pauseLateral=False, alwaysOnLateralEnabled=aol_enabled),
planner.gps_location_service: SimpleNamespace(latitude=1.0, longitude=1.0, bearingDeg=90.0),
}
planner.tracking_lead_filter.x = 1.0
planner.update(0.0, False, sm, starpilot_toggles)
assert seen == {
"tracking_lead": True,
"following_lead": True,
"slower_lead": True,
"model_length": 30.0,
"driving_in_curve": True,
"road_curvature_detected": True,
}
finally:
planner.shutdown()
Reference in New Issue View Git Blame Copy Permalink