red light stop

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(

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