diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index bb7b2f30f..444d45a4d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -10,6 +10,7 @@ from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N @@ -30,6 +31,14 @@ RAW_LEAD_SAFETY_MIN_CLOSING_SPEED = 0.5 RAW_LEAD_SAFETY_TTC = 7.0 RAW_LEAD_SAFETY_DISTANCE = 40.0 CLOSE_LEAD_BRAKE_CAP_MAX_TTC = 25.0 +VISION_LEAD_APPROACH_MIN_CLOSING_SPEED = 2.0 +VISION_LEAD_APPROACH_TRIGGER_TIME = 4.0 +VISION_LEAD_APPROACH_FULL_TIME = 1.0 +VISION_LEAD_APPROACH_TIGHT_BUFFER = 2.0 +VISION_LEAD_APPROACH_MAX_DECEL = 0.45 +VISION_LEAD_APPROACH_MIN_DECEL = 0.12 +VISION_LEAD_APPROACH_MIN_MODEL_PROB = 0.85 +VISION_LEAD_APPROACH_FULL_MODEL_PROB = 0.98 # Uncertainty-based filter disable thresholds UNCERT_SLOPE_TRIG = 0.12 # per second @@ -248,6 +257,41 @@ class LongitudinalPlanner: return max(accel_min, -required_decel) + def get_vision_lead_approach_cap(self, lead, v_ego, accel_min, t_follow): + if lead is None or not lead.status or bool(getattr(lead, "radar", False)): + return None + + lead_prob = float(getattr(lead, "modelProb", 0.0)) + if lead_prob < VISION_LEAD_APPROACH_MIN_MODEL_PROB: + return None + + lead_brake = max(0.0, -float(lead.aLeadK)) + reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) + closing_speed = max(0.0, v_ego - lead.vLead) + projected_closing_speed = closing_speed + lead_brake * reaction_t + if projected_closing_speed < VISION_LEAD_APPROACH_MIN_CLOSING_SPEED: + return None + + tight_follow_gap = float(t_follow * v_ego + VISION_LEAD_APPROACH_TIGHT_BUFFER) + gap_to_tight_follow = float(lead.dRel) - tight_follow_gap + time_to_tight_follow = gap_to_tight_follow / max(projected_closing_speed, 0.1) + if time_to_tight_follow > VISION_LEAD_APPROACH_TRIGGER_TIME: + return None + + desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow)) + if float(lead.dRel) > desired_gap + VISION_LEAD_APPROACH_TIGHT_BUFFER: + return None + + time_factor = float(np.clip((VISION_LEAD_APPROACH_TRIGGER_TIME - time_to_tight_follow) / + (VISION_LEAD_APPROACH_TRIGGER_TIME - VISION_LEAD_APPROACH_FULL_TIME), 0.0, 1.0)) + prob_factor = float(np.clip((lead_prob - VISION_LEAD_APPROACH_MIN_MODEL_PROB) / + (VISION_LEAD_APPROACH_FULL_MODEL_PROB - VISION_LEAD_APPROACH_MIN_MODEL_PROB), 0.0, 1.0)) + approach_decel = VISION_LEAD_APPROACH_MAX_DECEL * time_factor * prob_factor + if approach_decel < VISION_LEAD_APPROACH_MIN_DECEL: + return None + + return max(accel_min, -approach_decel) + @staticmethod def raw_close_lead_needs_control(lead, v_ego): if lead is None or not lead.status: @@ -537,6 +581,9 @@ class LongitudinalPlanner: cap = self.get_close_lead_brake_cap(lead, v_ego, output_accel_min) if cap is not None: close_lead_caps.append(cap) + approach_cap = self.get_vision_lead_approach_cap(lead, v_ego, output_accel_min, sm['starpilotPlan'].tFollow) + if approach_cap is not None: + close_lead_caps.append(approach_cap) if close_lead_caps: close_lead_brake_cap = min(close_lead_caps) self.a_desired = min(self.a_desired, close_lead_brake_cap) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 63498c3a0..62f111c3a 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -201,6 +201,67 @@ def test_soften_far_radar_lead_accel_keeps_close_closing_brake(): assert softened == pytest.approx(baseline) +def test_vision_lead_approach_cap_brakes_before_hard_cap(): + v_ego = 21.535 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=38.9, v_lead=18.04, a_lead=-0.026, radar=False, model_prob=0.984) + + hard_cap = planner.get_close_lead_brake_cap(lead, v_ego, -1.0) + approach_cap = planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.45) + + assert hard_cap == pytest.approx(-0.212, abs=1e-2) + assert approach_cap is not None + assert approach_cap < hard_cap + assert approach_cap > -0.6 + + +def test_vision_lead_approach_cap_ignores_opening_lead_with_large_gap(): + v_ego = 19.37 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=66.168, v_lead=20.751, a_lead=0.261, radar=False, model_prob=0.975) + + assert planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.45) is None + + +@pytest.mark.parametrize("model_version", ["v11", "v12"]) +def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_version): + approach_v_ego = 21.535 + close_v_ego = 21.435 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner_approach = LongitudinalPlanner(CP, init_v=approach_v_ego) + planner_close = LongitudinalPlanner(CP, init_v=close_v_ego) + + sm_approach = make_sm( + approach_v_ego, + desired_accel=0.2, + min_accel=-3.0, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=38.9, v_lead=18.04, a_lead=-0.026, radar=False, model_prob=0.984), + ) + sm_close = make_sm( + close_v_ego, + desired_accel=0.2, + min_accel=-3.0, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=27.18, v_lead=15.76, a_lead=-0.824, radar=False, model_prob=0.988), + ) + sm_approach["starpilotPlan"].vCruise = approach_v_ego + 8.0 + sm_close["starpilotPlan"].vCruise = close_v_ego + 8.0 + + planner_approach.update(sm_approach, make_toggles(model_version)) + planner_close.update(sm_close, make_toggles(model_version)) + + assert planner_approach.mode == "acc" + assert planner_close.mode == "acc" + assert planner_approach.output_a_target < -0.3 + assert planner_close.output_a_target < planner_approach.output_a_target - 0.25 + + @pytest.mark.parametrize("model_version", ["v11", "v12"]) def test_acc_mode_damps_far_radar_mild_lead_brake_more_than_close_brake(model_version): far_v_ego = 29.26