diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 2f8e7b8f6d..af74f3ed89 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -12,7 +12,7 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] clip = np.clip interp = np.interp STOPPING_RELEASE_HYSTERESIS = 0.35 -STOPPING_RELEASE_MIN_ACCEL = 0.2 +STOPPING_RELEASE_MIN_ACCEL = 0.15 LongCtrlState = car.CarControl.Actuators.LongControlState diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index 7b94e4ee20..c71b3c8e36 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -181,6 +181,7 @@ def test_stop_light_hold_bridges_short_no_lead_model_flicker(monkeypatch): 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)) @@ -235,6 +236,73 @@ def test_stop_light_hold_refreshes_through_stopped_approach_lead(monkeypatch): 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_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() diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index d116a5f453..b73da55e74 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -188,6 +188,43 @@ def test_update_requires_sustained_positive_target_to_leave_stopping(): assert lc.long_control_state == LongCtrlState.starting +def test_update_releases_stopping_on_small_sustained_positive_target(): + CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) + CP.longitudinalTuning.kpBP = [0.0] + CP.longitudinalTuning.kpV = [0.1] + CP.longitudinalTuning.kiBP = [0.0] + CP.longitudinalTuning.kiV = [0.03] + + lc = LongControl(CP) + lc.long_control_state = LongCtrlState.stopping + CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False) + CS.cruiseState.standstill = False + + release_frames = int(round(longcontrol.STOPPING_RELEASE_HYSTERESIS / longcontrol.DT_CTRL)) + for _ in range(release_frames - 1): + output_accel = lc.update( + active=True, + CS=CS, + a_target=0.16, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(startAccel=1.5), + ) + assert lc.long_control_state == LongCtrlState.stopping + assert output_accel <= 0.0 + + lc.update( + active=True, + CS=CS, + a_target=0.16, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(startAccel=1.5), + ) + + assert lc.long_control_state == LongCtrlState.starting + + def test_update_releases_stopping_with_cruise_standstill_latched(): CP = car.CarParams.new_message(vEgoStarting=0.5) CP.longitudinalTuning.kpBP = [0.0] diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index a060e7d1ce..0d4c8cbdae 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -43,6 +43,7 @@ class ConditionalExperimentalMode: LEAD_CLEAR_FILTER_TIME_HIGH = 0.35 STOP_LIGHT_ON_MARGIN = 2.5 STOP_LIGHT_OFF_MARGIN = 4.0 + STOP_LIGHT_MODEL_HOLD_STRONG_MARGIN = 10.0 STOP_LIGHT_LEAD_BLOCK_MARGIN = 15.0 STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED = 2.0 STOP_LIGHT_DETECTED_HOLD_TIME = 1.75 @@ -365,6 +366,7 @@ class ConditionalExperimentalMode: lead_speed = float(getattr(lead, "vLead", float("inf"))) lead_radar = bool(getattr(lead, "radar", False)) lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) + tracking_lead = bool(self.starpilot_planner.tracking_lead) lead_relevant = bool(getattr(lead, "status", False)) and lead_distance < stop_threshold + self.STOP_LIGHT_LEAD_BLOCK_MARGIN vision_stop_approach = ( lead_relevant and @@ -373,12 +375,13 @@ class ConditionalExperimentalMode: lead_speed < self.STOP_APPROACH_MAX_LEAD_SPEED ) stop_approach_hold_active = now < self.stop_approach_hold_until - if (self.stop_light_detected or self.stop_light_model_detected or stop_approach_hold_active) and vision_stop_approach: + trackable_stop_approach = vision_stop_approach and not tracking_lead + if (self.stop_light_detected or self.stop_light_model_detected or stop_approach_hold_active) and trackable_stop_approach: self.stop_approach_hold_until = now + self.STOP_APPROACH_LATCH_TIME - stop_approach_latched = now < self.stop_approach_hold_until and vision_stop_approach + stop_approach_latched = now < self.stop_approach_hold_until and trackable_stop_approach handoff_to_stopped_lead = ( lead_relevant and - not self.starpilot_planner.tracking_lead and + not tracking_lead and ( (self.stop_light_detected and lead_speed < self.STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED) or stop_approach_latched @@ -390,13 +393,16 @@ class ConditionalExperimentalMode: self.lead_clear_filter.update(not lead_relevant) lead_cleared = self.lead_clear_filter.x >= THRESHOLD self.stop_light_filter.update(model_stopping and lead_cleared) - detector_active = bool( - (self.stop_light_filter.x >= THRESHOLD**2 and lead_cleared) or handoff_to_stopped_lead or stop_approach_latched + model_detector_active = bool(self.stop_light_filter.x >= THRESHOLD**2 and lead_cleared) + detector_active = bool(model_detector_active or handoff_to_stopped_lead or stop_approach_latched) + model_hold_qualifies = bool( + self.starpilot_planner.model_stopped or + self.starpilot_planner.model_length < max(stop_threshold - self.STOP_LIGHT_MODEL_HOLD_STRONG_MARGIN, 0.0) ) - if detector_active: + if model_detector_active and model_hold_qualifies: self.stop_light_detected_hold_until = now + self.STOP_LIGHT_DETECTED_HOLD_TIME - hold_context_ok = bool(not lead_relevant or vision_stop_approach) + hold_context_ok = bool((not lead_relevant) or trackable_stop_approach) self.stop_light_detected = bool( detector_active or (hold_context_ok and now < self.stop_light_detected_hold_until)