diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 39405a6c6..2de094b5a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -102,8 +102,10 @@ VISION_LEAD_TFOLLOW_GAP_BUFFER_MIN = 8.0 VISION_LEAD_TFOLLOW_GAP_BUFFER_GAIN = 0.35 VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED = 6.5 VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED = 3.25 +VISION_LOW_SPEED_STOP_BUFFER_HOLD_MAX_LEAD_SPEED = 4.0 VISION_LOW_SPEED_STOP_BUFFER_MIN_MODEL_PROB = 0.9 VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED = 0.35 +VISION_LOW_SPEED_STOP_BUFFER_MIN_HOLD_REL_SPEED = -0.2 VISION_LOW_SPEED_STOP_BUFFER_BASE = 3.8 VISION_LOW_SPEED_STOP_BUFFER_EGO_GAIN = 0.80 VISION_LOW_SPEED_STOP_BUFFER_LEAD_GAIN = 0.25 @@ -646,23 +648,32 @@ class LongitudinalPlanner: return None, False lead_speed = max(float(lead.vLead), 0.0) + relative_speed = float(v_ego) - lead_speed closing_speed = max(0.0, v_ego - lead_speed) - valid_context = ( + entry_context = ( v_ego <= VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED and lead_speed <= VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED and closing_speed >= VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED ) + hold_context = ( + v_ego <= VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED and + lead_speed <= VISION_LOW_SPEED_STOP_BUFFER_HOLD_MAX_LEAD_SPEED and + relative_speed >= VISION_LOW_SPEED_STOP_BUFFER_MIN_HOLD_REL_SPEED + ) now_t = time.monotonic() entry_buffer = max(3.2, VISION_LOW_SPEED_STOP_BUFFER_BASE + VISION_LOW_SPEED_STOP_BUFFER_EGO_GAIN * float(v_ego) + VISION_LOW_SPEED_STOP_BUFFER_LEAD_GAIN * lead_speed) release_buffer = entry_buffer + VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN - if valid_context and float(lead.dRel) <= entry_buffer: + if entry_context and float(lead.dRel) <= entry_buffer: self.vision_low_speed_stop_hold_until = now_t + VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME latched = now_t < self.vision_low_speed_stop_hold_until - active = valid_context and (float(lead.dRel) <= entry_buffer or (latched and float(lead.dRel) <= release_buffer)) + active = bool( + (entry_context and float(lead.dRel) <= entry_buffer) or + (latched and hold_context and float(lead.dRel) <= release_buffer) + ) if not active: return None, False diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 2b253f6f5..eb4020244 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -8,6 +8,7 @@ import pytest from cereal import log from opendbc.car.honda.interface import CarInterface from opendbc.car.honda.values import CAR +import openpilot.selfdrive.controls.lib.longitudinal_planner as longitudinal_planner_module from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_coast_accel, get_vehicle_min_accel from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import soften_far_radar_lead_accel, should_trigger_planner_fcw @@ -781,6 +782,25 @@ def test_acc_mode_low_speed_vision_stop_buffer_brakes_harder_for_close_slow_visi assert planner.output_a_target <= -2.7 +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_acc_mode_low_speed_vision_stop_buffer_stays_latched_when_closure_softens_near_stop(model_version, monkeypatch): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.55) + lead = make_lead(status=True, d_rel=2.1, v_lead=0.05, a_lead=-0.02, radar=False, model_prob=0.99) + + monotonic_values = iter([10.0, 10.1]) + monkeypatch.setattr(longitudinal_planner_module.time, "monotonic", lambda: next(monotonic_values)) + + cap_armed, active_armed = planner.get_vision_low_speed_stop_buffer_cap(lead, 0.55, -2.0) + cap_held, active_held = planner.get_vision_low_speed_stop_buffer_cap(lead, 0.34, -2.0) + + assert active_armed + assert cap_armed is not None + assert active_held + assert cap_held is not None + assert cap_held <= -1.25 + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_standstill_moving_lead_does_not_force_resume_while_should_stop(model_version): v_ego = 0.0