diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index 3b0e65db7..005d3b022 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -8,16 +8,34 @@ import openpilot.starpilot.controls.lib.conditional_experimental_mode as conditi 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): + lead_v_lead: float = 0.0, lead_model_prob: float = 0.0, lead_radar: bool = False, + stop_sign_confirmed: bool = False): planner = SimpleNamespace( - params=None, - params_memory=None, + params=FakeParams(), + params_memory=FakeParams(), model_length=model_length, model_stopped=model_stopped, tracking_lead=tracking_lead, - starpilot_vcruise=SimpleNamespace(stop_sign_confirmed=False), + starpilot_vcruise=SimpleNamespace(stop_sign_confirmed=stop_sign_confirmed), 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), @@ -37,6 +55,29 @@ def run_stop_light_detector(cem, v_ego, *, steps: int, tracking_lead: bool = Fal 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), + "starpilotCarState": SimpleNamespace(trafficModeEnabled=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 @@ -134,6 +175,70 @@ def test_stop_light_latch_holds_slow_high_confidence_vision_lead_during_model_fl assert 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_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_slow_lead_holds_through_tracking_flap_for_high_confidence_vision_lead(): v_ego = 35 * CV.MPH_TO_MS cem = make_cem( diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index 15d40b713..383409752 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -95,10 +95,11 @@ class ConditionalExperimentalMode: def update(self, v_ego, sm, starpilot_toggles): now = time.monotonic() + standstill = bool(sm["carState"].standstill) self.status_value = CEStatus["OFF"] if self.params.get_bool("SafeMode") else restore_persisted_ce_state(self.params, self.params_memory) - if not is_manual_ce_status(self.status_value) and not sm["carState"].standstill: + if not is_manual_ce_status(self.status_value) and not standstill: self.update_conditions(v_ego, sm, starpilot_toggles) triggered = self.check_conditions(v_ego, sm, starpilot_toggles) @@ -114,11 +115,25 @@ class ConditionalExperimentalMode: self.experimental_mode = triggered or hold_active or transition_buffer_active self.prev_experimental_mode = self.experimental_mode self.params_memory.put_int("CEStatus", self.status_value if self.experimental_mode else CEStatus["OFF"]) + elif not is_manual_ce_status(self.status_value): + self.mode_hold_until = 0.0 + self.mode_false_since = 0.0 + + # Keep the stop-light path live at standstill so EXP stays pinned for a red + # light / stop sign, but can immediately release to CHILL when the model + # clears the stop (green light / open path). + self.stop_sign_and_light(v_ego, sm, starpilot_toggles.conditional_model_stop_time) + standstill_stop_hold = self.stop_light_detected or getattr(self.starpilot_planner.starpilot_vcruise, "stop_sign_confirmed", False) + + self.experimental_mode = standstill_stop_hold + self.prev_experimental_mode = self.experimental_mode + self.status_value = CEStatus["STOP_LIGHT"] if self.experimental_mode else CEStatus["OFF"] + self.params_memory.put_int("CEStatus", self.status_value if self.experimental_mode else CEStatus["OFF"]) else: self.mode_hold_until = 0.0 self.mode_false_since = 0.0 - self.experimental_mode = (self.status_value == CEStatus["USER_OVERRIDDEN"] or - (sm["carState"].standstill and self.experimental_mode and self.starpilot_planner.model_stopped)) + self.experimental_mode = self.status_value == CEStatus["USER_OVERRIDDEN"] + self.prev_experimental_mode = self.experimental_mode self.stop_light_detected &= not is_manual_ce_status(self.status_value) self.stop_light_filter.x = 0