import time import sys import types from types import SimpleNamespace import numpy as np import pytest from cereal import log from opendbc.car.honda.interface import CarInterface from opendbc.car.honda.values import CAR from opendbc.car.gm.values import CAR as GM_CAR, GMFlags import openpilot.selfdrive.controls.lib.longitudinal_planner as longitudinal_planner_module from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_coast_accel, get_vehicle_min_accel, should_publish_planner_fcw from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import soften_far_radar_lead_accel, should_trigger_planner_fcw from openpilot.selfdrive.modeld.constants import ModelConstants, Plan def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead: float = 0.0, radar: bool = False, model_prob: float = 0.0, y_rel: float = 0.0): lead = log.RadarState.LeadData.new_message() lead.status = status lead.dRel = d_rel lead.vLead = v_lead lead.vLeadK = v_lead lead.aLeadK = a_lead lead.vRel = 0.0 lead.aRel = 0.0 lead.yRel = y_rel lead.modelProb = model_prob lead.radar = radar return lead def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0, brake_press_prob: float = 0.0): model = log.ModelDataV2.new_message() model.init('leadsV3', 3) t_idxs = ModelConstants.T_IDXS model.position.x = [float(v_ego * t) for t in t_idxs] model.position.y = [0.0] * len(t_idxs) model.position.z = [0.0] * len(t_idxs) model.position.t = [float(t) for t in t_idxs] model.velocity.x = [float(v_ego)] * len(t_idxs) model.velocity.y = [0.0] * len(t_idxs) model.velocity.z = [0.0] * len(t_idxs) model.velocity.t = [float(t) for t in t_idxs] model.acceleration.x = [0.0] * len(t_idxs) model.acceleration.y = [0.0] * len(t_idxs) model.acceleration.z = [0.0] * len(t_idxs) model.acceleration.t = [float(t) for t in t_idxs] model.meta.disengagePredictions.gasPressProbs = [float(gas_press_prob)] * 6 model.meta.disengagePredictions.brakePressProbs = [float(brake_press_prob)] * 6 model.action.desiredAcceleration = desired_accel model.action.shouldStop = False return model def set_model_lead(model, idx: int, *, prob: float, x0: float, y0: float, v0: float, a0: float = 0.0): lead = model.leadsV3[idx] lead.prob = float(prob) lead.x = [float(x0)] lead.y = [float(y0)] lead.v = [float(v0)] lead.a = [float(a0)] def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimental_mode: bool = True, tracking_lead: bool = False, lead_one=None, lead_two=None, gas_press_prob: float = 1.0, brake_press_prob: float = 0.0, disable_throttle: bool = False): return { "carControl": SimpleNamespace(orientationNED=[0.0, 0.0, 0.0]), "carState": SimpleNamespace( vEgo=v_ego, vEgoCluster=v_ego, aEgo=0.0, vCruise=100.0, standstill=False, steeringAngleDeg=0.0, ), "controlsState": SimpleNamespace( longControlState=LongCtrlState.pid, forceDecel=False, ), "liveParameters": SimpleNamespace(angleOffsetDeg=0.0), "modelV2": make_model(v_ego, desired_accel, gas_press_prob=gas_press_prob, brake_press_prob=brake_press_prob), "radarState": SimpleNamespace( leadOne=lead_one if lead_one is not None else make_lead(status=False), leadTwo=lead_two if lead_two is not None else make_lead(status=False), ), "selfdriveState": SimpleNamespace(enabled=True, experimentalMode=experimental_mode, personality=0), "starpilotCarState": SimpleNamespace(accelPressed=False), "starpilotPlan": SimpleNamespace( vCruise=v_ego + 5.0, minAcceleration=min_accel, maxAcceleration=2.0, disableThrottle=disable_throttle, trackingLead=tracking_lead, accelerationJerk=5.0, dangerJerk=5.0, speedJerk=5.0, dangerFactor=1.0, tFollow=1.45, forcingStop=False, redLight=False, forcingStopLength=2, ), } def make_toggles(model_version: str = "v11", radar_takeoffs: bool = False, prioritize_smooth_following: bool = False): return SimpleNamespace( taco_tune=False, classic_model=False, tinygrad_model=True, model_version=model_version, stop_distance=6.0, vEgoStopping=0.5, radar_takeoffs=radar_takeoffs, prioritize_smooth_following=prioritize_smooth_following, ) @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_experimental_mlsim_uses_vehicle_min_accel_floor(model_version): v_ego = 18.0 desired_accel = -1.0 comfort_min_accel = -0.5 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm(v_ego, desired_accel, comfort_min_accel) vehicle_min_accel = get_vehicle_min_accel(CP, v_ego) assert vehicle_min_accel < comfort_min_accel planner.update(sm, make_toggles(model_version)) assert planner.mode == "blended" assert planner.mlsim assert planner.output_a_target == pytest.approx(desired_accel, abs=1e-3) assert planner.output_a_target < comfort_min_accel def test_gm_pedal_vehicle_min_accel_uses_brand_when_car_name_is_missing(): CP = SimpleNamespace( carName=None, brand="gm", enableGasInterceptorDEPRECATED=True, flags=GMFlags.PEDAL_LONG.value, carFingerprint=GM_CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL, ) assert get_vehicle_min_accel(CP, 32.4) == pytest.approx(-2.95) @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_uses_close_raw_lead_when_tracking_lead_is_debounced(model_version): v_ego = 5.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( v_ego, desired_accel=-0.6, min_accel=-1.0, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=24.0, v_lead=0.3), ) sm["starpilotPlan"].vCruise = v_ego + 12.0 planner.update(sm, make_toggles(model_version)) assert planner.mode == "acc" assert planner.raw_close_lead_needs_control(sm["radarState"].leadOne, v_ego) assert planner.output_a_target == pytest.approx( planner.get_close_lead_brake_cap(sm["radarState"].leadOne, v_ego, sm["starpilotPlan"].minAcceleration) ) @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_matches_no_lead_baseline_for_far_vision_only_lead_without_tracking(model_version): v_ego = 29.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) planner_far_vision = LongitudinalPlanner(CP, init_v=v_ego) sm_no_lead = make_sm( v_ego, desired_accel=0.2, min_accel=-1.0, experimental_mode=False, tracking_lead=False, ) sm_far_vision = make_sm( v_ego, desired_accel=0.2, min_accel=-1.0, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=82.0, v_lead=25.0, radar=False, model_prob=0.9), ) sm_no_lead["starpilotPlan"].vCruise = v_ego + 2.0 sm_far_vision["starpilotPlan"].vCruise = v_ego + 2.0 no_lead_outputs = [] far_vision_outputs = [] for _ in range(8): planner_no_lead.update(sm_no_lead, make_toggles(model_version)) planner_far_vision.update(sm_far_vision, make_toggles(model_version)) no_lead_outputs.append(planner_no_lead.output_a_target) far_vision_outputs.append(planner_far_vision.output_a_target) assert planner_far_vision.mode == "acc" assert not planner_far_vision.raw_close_lead_needs_control(sm_far_vision["radarState"].leadOne, v_ego) np.testing.assert_allclose(far_vision_outputs, no_lead_outputs, atol=1e-6) def test_soften_far_radar_lead_accel_reduces_gentle_far_brake(): softened = soften_far_radar_lead_accel(114.8, 28.88, -0.75, 29.26, 1.45, radar=True) assert softened > -0.35 assert softened < 0.0 def test_soften_far_radar_lead_accel_keeps_close_closing_brake(): baseline = -0.76 softened = soften_far_radar_lead_accel(68.0, 26.38, baseline, 29.38, 1.45, radar=True) assert softened == pytest.approx(baseline) def test_planner_fcw_suppresses_low_speed_opening_or_low_ttc_false_positives(): assert not should_trigger_planner_fcw( make_lead(status=True, d_rel=7.156, v_lead=0.798, a_lead=0.021, radar=False, model_prob=0.99), 0.402, ) assert not should_trigger_planner_fcw( make_lead(status=True, d_rel=9.311, v_lead=0.911, a_lead=-0.263, radar=False, model_prob=0.99), 1.252, ) def test_planner_fcw_keeps_real_low_speed_closing_alerts(): assert should_trigger_planner_fcw( make_lead(status=True, d_rel=1.8, v_lead=0.0, a_lead=0.0, radar=False, model_prob=0.99), 1.6, ) def test_publish_planner_fcw_suppresses_crawl_speed_false_positive(): car_state = SimpleNamespace(vEgo=0.29, standstill=False) radar_state = SimpleNamespace( leadOne=make_lead(status=True, d_rel=7.55, v_lead=0.033, a_lead=0.0, radar=False, model_prob=0.99), ) assert not should_publish_planner_fcw(3, car_state, radar_state) def test_publish_planner_fcw_keeps_real_current_close_closing_alert(): car_state = SimpleNamespace(vEgo=1.6, standstill=False) radar_state = SimpleNamespace( leadOne=make_lead(status=True, d_rel=1.8, v_lead=0.0, a_lead=0.0, radar=False, model_prob=0.99), ) assert should_publish_planner_fcw(3, car_state, radar_state) 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 > -1.2 def test_vision_lead_approach_cap_brakes_harder_when_inside_tight_gap(): v_ego = 26.18 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=39.72, v_lead=22.46, a_lead=-0.15, radar=False, model_prob=0.97) approach_cap = planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.49) assert approach_cap is not None assert approach_cap < -0.5 def test_vision_lead_approach_cap_brakes_harder_for_braking_tracked_lead_inside_tight_gap(): v_ego = 19.50 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=19.7, v_lead=16.25, a_lead=-0.83, radar=False, model_prob=0.98) hard_cap = planner.get_close_lead_brake_cap(lead, v_ego, -3.0) approach_cap = planner.get_vision_lead_approach_cap(lead, v_ego, -3.0, 1.45) assert hard_cap == pytest.approx(-1.01, abs=0.03) assert approach_cap is not None assert approach_cap < -1.35 assert approach_cap < hard_cap 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 def test_vision_untracked_slow_lead_cap_triggers_only_for_meaningful_closing_case(): route_v_ego = 23.23 far_v_ego = 29.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=route_v_ego) route_like_lead = make_lead(status=True, d_rel=66.7, v_lead=18.49, a_lead=0.0, radar=False, model_prob=0.92) far_mild_lead = make_lead(status=True, d_rel=82.0, v_lead=25.0, a_lead=0.0, radar=False, model_prob=0.9) route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, route_v_ego, -1.0) far_cap = planner.get_vision_untracked_slow_lead_cap(far_mild_lead, far_v_ego, -1.0) assert route_cap is not None assert route_cap < -0.1 assert far_cap is None def test_vision_untracked_slow_lead_cap_reaches_high_confidence_far_slower_lead_before_raw_close_lead(): v_ego = 21.48 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) route_like_lead = make_lead(status=True, d_rel=93.0, v_lead=12.84, a_lead=0.0, radar=False, model_prob=0.935) route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, v_ego, -1.0) assert route_cap is not None assert route_cap < -0.5 assert not planner.raw_close_lead_needs_control(route_like_lead, v_ego) def test_vision_untracked_slow_lead_cap_relaxes_confidence_for_near_stopped_high_closure_lead(): v_ego = 20.35 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) route_like_lead = make_lead(status=True, d_rel=115.4, v_lead=3.76, a_lead=0.0, radar=False, model_prob=0.70) route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, v_ego, -1.0) assert route_cap is not None assert route_cap < -0.55 def test_vision_untracked_slow_lead_cap_keeps_low_confidence_floor_for_less_threatening_lead(): v_ego = 20.35 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) less_threatening_lead = make_lead(status=True, d_rel=115.4, v_lead=9.5, a_lead=0.0, radar=False, model_prob=0.75) assert planner.get_vision_untracked_slow_lead_cap(less_threatening_lead, v_ego, -1.0) is None def test_vision_slow_stopped_lead_cap_brakes_earlier_for_confident_stop(): v_ego = 13.207 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=49.131, v_lead=1.837, a_lead=-0.312, radar=False, model_prob=0.942) slow_stop_cap = planner.get_vision_slow_stopped_lead_cap(lead, v_ego, -1.0, 1.75) assert slow_stop_cap is not None assert slow_stop_cap < -0.9 assert slow_stop_cap > -1.25 def test_vision_slow_stopped_lead_cap_ignores_far_high_speed_stop_candidate(): v_ego = 33.5 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=183.0, v_lead=0.0, a_lead=0.0, radar=False, model_prob=0.995) assert planner.get_vision_slow_stopped_lead_cap(lead, v_ego, -1.0, 1.45) is None @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_dynamic_t_follow_increases_modestly_for_closing_lead(model_version): v_ego = 21.535 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( 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["starpilotPlan"].vCruise = v_ego + 8.0 for _ in range(8): planner.update(sm, make_toggles(model_version)) assert planner.effective_t_follow is not None assert planner.effective_t_follow > sm["starpilotPlan"].tFollow + 0.15 assert planner.effective_t_follow < sm["starpilotPlan"].tFollow + 0.45 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_dynamic_t_follow_stays_near_base_for_far_highway_lead(model_version): v_ego = 29.26 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( v_ego, desired_accel=0.2, min_accel=-1.0, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=114.8, v_lead=28.88, a_lead=-0.75, radar=True, model_prob=0.9), ) sm["starpilotPlan"].vCruise = v_ego + 3.0 for _ in range(12): planner.update(sm, make_toggles(model_version)) assert planner.effective_t_follow == pytest.approx(sm["starpilotPlan"].tFollow, abs=0.02) @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_dynamic_t_follow_releases_toward_base_after_lead_opens(model_version): v_ego = 21.535 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( 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), ) for _ in range(8): planner.update(sm, make_toggles(model_version)) boosted_t_follow = planner.effective_t_follow sm["radarState"].leadOne = make_lead(status=True, d_rel=66.168, v_lead=20.751, a_lead=0.261, radar=False, model_prob=0.975) for _ in range(12): planner.update(sm, make_toggles(model_version)) assert boosted_t_follow is not None assert planner.effective_t_follow < boosted_t_follow assert planner.effective_t_follow == pytest.approx(sm["starpilotPlan"].tFollow, abs=0.02) @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) 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=-0.5, 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=-0.5, 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 approach_outputs = [] for _ in range(6): planner_approach.update(sm_approach, make_toggles(model_version)) approach_outputs.append(planner_approach.output_a_target) planner_close.update(sm_close, make_toggles(model_version)) assert planner_approach.mode == "acc" assert planner_close.mode == "acc" assert min(approach_outputs[:2]) > -0.55 assert approach_outputs[-1] < -1.3 assert planner_close.output_a_target < approach_outputs[0] - 0.8 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_tracked_vision_far_mild_closure_does_not_bypass_persistence(model_version): v_ego = 37.45 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=42.8, v_lead=35.31, a_lead=0.18, radar=False, model_prob=0.98) approach_cap = planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.45) assert approach_cap is not None assert approach_cap > -1.0 assert not planner.tracked_vision_lead_approach_needs_immediate_brake(lead, v_ego, approach_cap) @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_tracked_vision_close_or_braking_lead_bypasses_persistence(model_version): v_ego = 19.50 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( v_ego, desired_accel=0.2, min_accel=-1.0, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=19.7, v_lead=16.25, a_lead=-0.83, radar=False, model_prob=0.98), ) sm["starpilotPlan"].vCruise = v_ego + 6.0 planner.update(sm, make_toggles(model_version)) assert planner.output_a_target < -1.3 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_pretracking_vision_slow_lead_blocks_positive_catchup(model_version): v_ego = 23.23 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego) sm_no_lead = make_sm( v_ego, desired_accel=0.2, min_accel=-0.5, experimental_mode=False, tracking_lead=False, ) sm_with_lead = make_sm( v_ego, desired_accel=0.2, min_accel=-0.5, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=66.7, v_lead=18.49, a_lead=0.0, radar=False, model_prob=0.92), ) sm_no_lead["starpilotPlan"].vCruise = v_ego + 6.0 sm_with_lead["starpilotPlan"].vCruise = v_ego + 6.0 for _ in range(6): planner_no_lead.update(sm_no_lead, make_toggles(model_version)) planner_with_lead.update(sm_with_lead, make_toggles(model_version)) assert planner_with_lead.mode == "acc" assert not planner_with_lead.raw_close_lead_needs_control(sm_with_lead["radarState"].leadOne, v_ego) assert planner_with_lead.output_a_target <= planner_no_lead.output_a_target - 0.04 assert planner_with_lead.output_a_target < -0.2 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_keeps_close_slow_radar_lead_active_when_tracking_flaps(model_version): v_ego = 0.96 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego) sm_no_lead = make_sm( v_ego, desired_accel=0.45, min_accel=-0.5, experimental_mode=False, tracking_lead=False, ) sm_with_lead = make_sm( v_ego, desired_accel=0.45, min_accel=-0.5, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=8.15, v_lead=0.09, a_lead=-0.36, radar=True, model_prob=1.0, y_rel=0.1), ) sm_no_lead["starpilotPlan"].vCruise = v_ego + 8.0 sm_with_lead["starpilotPlan"].vCruise = v_ego + 8.0 planner_no_lead.update(sm_no_lead, make_toggles(model_version)) planner_with_lead.update(sm_with_lead, make_toggles(model_version)) assert planner_with_lead.raw_close_lead_needs_control(sm_with_lead["radarState"].leadOne, v_ego) assert planner_with_lead.output_a_target < planner_no_lead.output_a_target - 0.15 assert planner_with_lead.output_a_target <= 0.22 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_low_speed_weak_departure_accel_cap_softens_voacc_follow_pulse(model_version): v_ego = 2.8 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( v_ego, desired_accel=0.45, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=16.6, v_lead=2.85, a_lead=0.05, radar=False, model_prob=0.99, y_rel=0.0), ) sm["starpilotPlan"].vCruise = v_ego + 10.0 planner.update(sm, make_toggles(model_version)) assert planner.output_a_target <= 0.22 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_pretracking_vision_far_slower_lead_starts_braking_before_tracking(model_version): v_ego = 21.48 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego) sm_no_lead = make_sm( v_ego, desired_accel=0.2, min_accel=-1.0, experimental_mode=False, tracking_lead=False, ) sm_with_lead = make_sm( v_ego, desired_accel=0.2, min_accel=-1.0, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=93.0, v_lead=12.84, a_lead=0.0, radar=False, model_prob=0.935), ) sm_no_lead["starpilotPlan"].vCruise = v_ego + 6.0 sm_with_lead["starpilotPlan"].vCruise = v_ego + 6.0 no_lead_outputs = [] lead_outputs = [] for _ in range(8): planner_no_lead.update(sm_no_lead, make_toggles(model_version)) planner_with_lead.update(sm_with_lead, make_toggles(model_version)) no_lead_outputs.append(planner_no_lead.output_a_target) lead_outputs.append(planner_with_lead.output_a_target) assert planner_with_lead.mode == "acc" assert not planner_with_lead.raw_close_lead_needs_control(sm_with_lead["radarState"].leadOne, v_ego) assert all(lead_output <= no_lead_output + 1e-6 for lead_output, no_lead_output in zip(lead_outputs[5:], no_lead_outputs[5:])) assert min(lead_outputs[5:]) < min(no_lead_outputs[5:]) - 0.08 assert lead_outputs[-1] < no_lead_outputs[-1] - 0.15 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_pretracking_vision_far_slower_lead_can_still_brake_immediately(model_version): v_ego = 21.48 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( v_ego, desired_accel=0.2, min_accel=-1.0, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=93.0, v_lead=12.84, a_lead=0.0, radar=False, model_prob=0.935), ) sm["starpilotPlan"].vCruise = v_ego + 6.0 planner.update(sm, make_toggles(model_version)) assert planner.mode == "acc" assert planner.output_a_target < -0.45 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_pretracking_closer_braking_vision_lead_bypasses_far_lead_persistence(model_version): v_ego = 17.46 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( v_ego, desired_accel=0.2, min_accel=-1.0, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=41.9, v_lead=14.86, a_lead=-0.03, radar=False, model_prob=1.0), ) sm["starpilotPlan"].vCruise = v_ego + 6.0 planner.update(sm, make_toggles(model_version)) assert planner.mode == "acc" assert planner.output_a_target < -0.35 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_pretracking_flappy_far_lead_requires_persistence(model_version): v_ego = 26.09 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) planner_flappy = LongitudinalPlanner(CP, init_v=v_ego) sm_no_lead = make_sm( v_ego, desired_accel=0.2, min_accel=-1.0, experimental_mode=False, tracking_lead=False, ) sm_flappy = make_sm( v_ego, desired_accel=0.2, min_accel=-1.0, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=74.75, v_lead=26.63, a_lead=0.01, radar=False, model_prob=0.989), ) sm_no_lead["starpilotPlan"].vCruise = v_ego + 6.0 sm_flappy["starpilotPlan"].vCruise = v_ego + 6.0 flappy_sequence = [ (74.75, 26.63, 0.01, 0.989), (68.17, 20.81, 0.094, 0.971), (69.73, 24.12, 0.057, 0.981), (62.15, 21.38, 0.064, 0.983), (66.29, 23.19, 0.069, 0.985), (70.58, 27.51, 0.036, 0.988), ] no_lead_outputs = [] flappy_outputs = [] for d_rel, v_lead, a_lead, model_prob in flappy_sequence: planner_no_lead.update(sm_no_lead, make_toggles(model_version)) sm_flappy["radarState"].leadOne = make_lead( status=True, d_rel=d_rel, v_lead=v_lead, a_lead=a_lead, radar=False, model_prob=model_prob, ) planner_flappy.update(sm_flappy, make_toggles(model_version)) no_lead_outputs.append(planner_no_lead.output_a_target) flappy_outputs.append(planner_flappy.output_a_target) assert planner_flappy.mode == "acc" assert min(flappy_outputs) > min(no_lead_outputs) - 0.12 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_pretracking_near_stopped_vision_lead_does_not_relax_when_confidence_is_midrange(model_version): v_ego = 20.35 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego) sm_no_lead = make_sm( v_ego, desired_accel=0.2, min_accel=-1.0, experimental_mode=False, tracking_lead=False, ) sm_with_lead = make_sm( v_ego, desired_accel=0.2, min_accel=-1.0, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=115.4, v_lead=3.76, a_lead=0.0, radar=False, model_prob=0.70), ) sm_no_lead["starpilotPlan"].vCruise = v_ego + 6.0 sm_with_lead["starpilotPlan"].vCruise = v_ego + 6.0 for _ in range(8): planner_no_lead.update(sm_no_lead, make_toggles(model_version)) planner_with_lead.update(sm_with_lead, make_toggles(model_version)) assert planner_with_lead.mode == "acc" assert planner_with_lead.output_a_target < planner_no_lead.output_a_target - 0.12 assert planner_with_lead.output_a_target < -0.45 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_tracked_pace_matched_lead_caps_positive_catchup(model_version): v_ego = 28.7 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego) sm_no_lead = make_sm( v_ego, desired_accel=0.5, min_accel=-1.0, experimental_mode=False, tracking_lead=False, ) sm_with_lead = make_sm( v_ego, desired_accel=0.5, min_accel=-1.0, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=22.0, v_lead=29.4, a_lead=0.0, radar=False, model_prob=0.995), ) sm_no_lead["starpilotPlan"].vCruise = v_ego + 4.0 sm_with_lead["starpilotPlan"].vCruise = v_ego + 4.0 for _ in range(8): planner_no_lead.update(sm_no_lead, make_toggles(model_version)) planner_with_lead.update(sm_with_lead, make_toggles(model_version)) assert planner_with_lead.mode == "acc" assert planner_with_lead.output_a_target <= planner_no_lead.output_a_target - 0.15 assert planner_with_lead.output_a_target < 0.08 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_low_speed_vision_stop_buffer_sets_should_stop_before_tiny_gap(model_version): v_ego = 3.8 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( v_ego, desired_accel=0.1, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=5.75, v_lead=0.58, a_lead=-0.1, radar=False, model_prob=0.99), ) sm["starpilotPlan"].vCruise = v_ego + 4.0 planner.update(sm, make_toggles(model_version)) assert planner.mode == "acc" assert planner.output_should_stop assert planner.output_a_target < -1.0 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_low_speed_vision_stop_buffer_brakes_harder_for_close_slow_vision_lead(model_version): v_ego = 6.2 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( v_ego, desired_accel=0.1, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=9.35, v_lead=2.8, a_lead=-0.2, radar=False, model_prob=0.99), ) sm["starpilotPlan"].vCruise = v_ego + 4.0 planner.update(sm, make_toggles(model_version)) assert planner.mode == "acc" assert planner.output_should_stop assert planner.output_a_target <= -2.7 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) 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", "v15"]) def test_acc_mode_close_moving_vision_lead_keeps_negative_output_while_should_stop(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.242) toggles = make_toggles(model_version) stop_sequence = [ (0.242, 2.062, 0.284, -0.081), (0.221, 1.963, 0.338, -0.076), (0.194, 2.100, 0.451, -0.076), (0.180, 2.001, 0.447, -0.066), (0.166, 1.964, 0.451, -0.066), (0.151, 2.075, 0.451, -0.060), ] outputs = [] for v_ego, d_rel, v_lead, desired_accel in stop_sequence: sm = make_sm( v_ego, desired_accel=desired_accel, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=d_rel, v_lead=v_lead, a_lead=0.0, radar=False, model_prob=1.0), ) sm["controlsState"].longControlState = LongCtrlState.stopping sm["starpilotPlan"].vCruise = 10.0 planner.update(sm, toggles) outputs.append(planner.output_a_target) assert all(output <= -0.02 for output in outputs[2:]) @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_close_near_standstill_vision_lead_keeps_meaningful_brake_floor(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.017) sm = make_sm( 0.017, desired_accel=-0.06, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=2.83, v_lead=0.09, a_lead=0.10, radar=False, model_prob=0.9999), ) sm["controlsState"].longControlState = LongCtrlState.stopping sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = True planner.update(sm, make_toggles(model_version)) assert planner.output_should_stop assert planner.output_a_target <= -0.20 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_close_near_standstill_moving_lead_keeps_brake_floor_while_should_stop(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.034) sm = make_sm( 0.034, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=3.93, v_lead=1.61, a_lead=2.18, radar=False, model_prob=1.0), ) sm["controlsState"].longControlState = LongCtrlState.stopping sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = True planner.update(sm, make_toggles(model_version)) assert planner.output_should_stop assert planner.output_a_target <= -0.20 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_close_opening_vision_lead_does_not_drop_to_zero_after_stop_release(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=1.49) toggles = make_toggles(model_version) sm_stop = make_sm( 1.488, desired_accel=-1.64, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=3.433, v_lead=1.469, a_lead=0.58, radar=False, model_prob=0.9996), ) sm_stop["controlsState"].longControlState = LongCtrlState.stopping sm_stop["starpilotPlan"].vCruise = 10.0 sm_stop["modelV2"].action.shouldStop = True planner.update(sm_stop, toggles) sm_release = make_sm( 1.420, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=3.568, v_lead=1.679, a_lead=0.66, radar=False, model_prob=0.9994), ) sm_release["controlsState"].longControlState = LongCtrlState.pid sm_release["starpilotPlan"].vCruise = 10.0 sm_release["modelV2"].action.shouldStop = False planner.update(sm_release, toggles) assert planner.output_a_target <= -0.18 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_close_near_standstill_departing_lead_keeps_small_brake_after_stop_release(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.034) toggles = make_toggles(model_version) sm_stop = make_sm( 0.034, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=3.93, v_lead=1.61, a_lead=2.18, radar=False, model_prob=1.0), ) sm_stop["controlsState"].longControlState = LongCtrlState.stopping sm_stop["starpilotPlan"].vCruise = 10.0 sm_stop["modelV2"].action.shouldStop = True planner.update(sm_stop, toggles) sm_release = make_sm( 0.449, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=4.02, v_lead=2.45, a_lead=2.26, radar=False, model_prob=1.0), ) sm_release["controlsState"].longControlState = LongCtrlState.pid sm_release["starpilotPlan"].vCruise = 10.0 sm_release["modelV2"].action.shouldStop = False planner.update(sm_release, toggles) assert planner.output_a_target <= -0.18 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_tracked_vision_model_brake_floor_prevents_positive_output_on_slower_lead(model_version): v_ego = 19.1 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( v_ego, desired_accel=-1.18, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=36.5, v_lead=17.2, a_lead=-0.53, radar=False, model_prob=0.993), ) for _ in range(8): planner.update(sm, make_toggles(model_version)) assert planner.mode == "acc" assert planner.output_a_target <= -0.35 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_tracked_vision_model_brake_cap_relaxes_mild_model_brake_slam_window(model_version): v_ego = 20.56 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=48.0, v_lead=19.07, a_lead=-0.30, radar=False, model_prob=0.999) cap = planner.get_tracked_vision_model_brake_cap(lead, v_ego, 1.45, -0.35) assert cap is not None assert cap > -1.2 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_tracked_vision_model_brake_cap_does_not_relax_strong_model_brake(model_version): v_ego = 20.56 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=38.1, v_lead=19.07, a_lead=-0.30, radar=False, model_prob=0.999) cap = planner.get_tracked_vision_model_brake_cap(lead, v_ego, 1.45, -1.2) assert cap is None @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_manual_resume_override_clears_no_lead_model_stop_at_standstill(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) sm = make_sm(0.0, desired_accel=0.0, min_accel=-0.5) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["modelV2"].action.shouldStop = True sm["starpilotPlan"].forcingStop = True sm["starpilotCarState"] = SimpleNamespace(accelPressed=True) planner.update(sm, make_toggles(model_version)) assert not planner.output_should_stop assert planner.output_a_target >= 0.2 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_manual_resume_override_does_not_clear_stopped_lead_stop(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) sm = make_sm( 0.0, desired_accel=0.0, min_accel=-0.5, lead_one=make_lead(status=True, d_rel=4.0, v_lead=0.0, radar=False, model_prob=0.99), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["modelV2"].action.shouldStop = True sm["starpilotPlan"].forcingStop = True sm["starpilotCarState"] = SimpleNamespace(accelPressed=True) planner.update(sm, make_toggles(model_version)) assert planner.output_should_stop @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_moving_lead_does_not_force_resume_while_should_stop(model_version): v_ego = 0.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( v_ego, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=7.1, v_lead=2.3, a_lead=1.8, radar=True, model_prob=1.0), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["starpilotPlan"].vCruise = 10.0 planner.update(sm, make_toggles(model_version)) assert planner.output_should_stop assert planner.output_a_target < 0.1 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_accelerating_lead_keeps_small_nudge_while_stop_holds(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) sm = make_sm( 0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=5.55, v_lead=0.02, a_lead=0.40, radar=False, model_prob=1.0), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False planner.update(sm, make_toggles(model_version)) assert planner.output_should_stop assert longitudinal_planner_module.STANDSTILL_LEAD_NUDGE_ACCEL <= planner.output_a_target < 0.1 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_near_standstill_accelerating_lead_keeps_nudge_during_creep_frame(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.03) sm = make_sm( 0.03, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=5.60, v_lead=0.06, a_lead=0.40, radar=False, model_prob=1.0), ) sm["carState"].standstill = False sm["controlsState"].longControlState = LongCtrlState.pid sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False planner.update(sm, make_toggles(model_version)) assert planner.output_should_stop assert longitudinal_planner_module.STANDSTILL_LEAD_NUDGE_ACCEL <= planner.output_a_target < 0.1 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_tiny_opening_lead_without_accel_does_not_get_nudge_floor(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) sm = make_sm( 0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=5.55, v_lead=0.02, a_lead=0.0, radar=False, model_prob=1.0), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False planner.update(sm, make_toggles(model_version)) assert planner.output_should_stop assert planner.output_a_target <= 0.0 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_slow_creep_depart_releases_after_short_confirm(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) sm = make_sm( 0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=7.2, v_lead=0.33, a_lead=0.28, radar=True, model_prob=1.0), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False frames = int(round(longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME / planner.dt)) for _ in range(max(frames, 1)): planner.update(sm, make_toggles(model_version)) assert not planner.output_should_stop assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_slow_creep_depart_releases_near_stop_gap_with_modest_accel(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) sm = make_sm( 0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=5.7, v_lead=1.0, a_lead=0.09, radar=True, model_prob=1.0), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False frames = int(round(longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME / planner.dt)) for _ in range(max(frames, 1)): planner.update(sm, make_toggles(model_version)) assert not planner.output_should_stop assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_slow_creep_depart_does_not_release_on_gap_without_motion_signal(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) sm = make_sm( 0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=7.2, v_lead=0.20, a_lead=0.05, radar=True, model_prob=1.0), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False frames = int(round(longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME / planner.dt)) + 2 for _ in range(max(frames, 1)): planner.update(sm, make_toggles(model_version)) assert planner.output_should_stop @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_moving_lead_applies_resume_floor_once_stop_clears(model_version): v_ego = 0.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( v_ego, desired_accel=0.1, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=15.0, v_lead=2.2, a_lead=0.4, radar=False, model_prob=0.99), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["modelV2"].action.shouldStop = False sm["starpilotPlan"].vCruise = 10.0 for _ in range(12): planner.update(sm, make_toggles(model_version)) assert not planner.output_should_stop assert planner.output_a_target >= 0.2 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_stopped_lead_guard_blocks_false_release_at_longer_gap(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) sm = make_sm( 0.03, desired_accel=1.85, min_accel=-0.5, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=8.15, v_lead=0.04, a_lead=0.0, radar=False, model_prob=0.99), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.pid sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False planner.update(sm, make_toggles(model_version)) assert planner.output_should_stop assert planner.output_a_target <= 0.0 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_stopped_lead_guard_does_not_block_radar_depart_at_longer_gap(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) sm = make_sm( 0.0, desired_accel=0.45, min_accel=-0.5, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=8.2, v_lead=0.72, a_lead=0.32, radar=True, model_prob=0.999, y_rel=0.1), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False planner.update(sm, make_toggles(model_version)) assert not planner.output_should_stop assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_stopped_lead_guard_blocks_false_release_during_creep_frame(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.2) sm = make_sm( 0.2, desired_accel=1.15, min_accel=-0.5, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=7.9, v_lead=0.03, a_lead=0.0, radar=False, model_prob=0.99), ) sm["carState"].standstill = False sm["controlsState"].longControlState = LongCtrlState.pid sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False planner.update(sm, make_toggles(model_version)) assert planner.output_should_stop assert planner.output_a_target <= 0.0 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_confident_departing_lead_clears_stop_without_waiting_for_model_accel(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) sm = make_sm( 0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=3.95, v_lead=0.62, a_lead=1.05, radar=False, model_prob=1.0), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["modelV2"].action.shouldStop = True sm["starpilotPlan"].vCruise = 10.0 for _ in range(6): planner.update(sm, make_toggles(model_version)) assert planner.output_should_stop planner.update(sm, make_toggles(model_version)) assert not planner.output_should_stop assert planner.output_a_target >= 0.2 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_confident_departing_lead_gets_depart_floor_with_zero_model_accel(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) sm = make_sm( 0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=4.10, v_lead=1.05, a_lead=1.20, radar=False, model_prob=1.0), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["modelV2"].action.shouldStop = False sm["starpilotPlan"].vCruise = 10.0 for _ in range(6): planner.update(sm, make_toggles(model_version)) assert planner.output_should_stop planner.update(sm, make_toggles(model_version)) assert not planner.output_should_stop assert planner.output_a_target >= 0.25 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_confident_departing_lead_does_not_release_on_first_creep_frame(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) sm = make_sm( 0.0, desired_accel=0.38, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=4.12, v_lead=0.32, a_lead=1.12, radar=False, model_prob=1.0), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["modelV2"].action.shouldStop = False sm["starpilotPlan"].vCruise = 10.0 planner.update(sm, make_toggles(model_version)) assert planner.output_should_stop assert planner.output_a_target <= 0.2 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_moving_lead_holds_depart_accel_floor_after_stop_release(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) toggles = make_toggles(model_version) sequence = [ (0.0, True, 15.0, 2.2, 0.10), (0.0, True, 15.2, 2.4, 0.12), (0.0, True, 15.5, 2.6, 0.15), (0.10, False, 15.8, 2.8, 0.20), (0.25, False, 16.2, 3.0, 0.25), (0.45, False, 16.8, 3.2, 0.30), ] outputs = [] for v_ego, standstill, d_rel, v_lead, desired_accel in sequence: sm = make_sm( v_ego, desired_accel=desired_accel, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=d_rel, v_lead=v_lead, a_lead=0.0, radar=False, model_prob=0.99), ) sm["carState"].standstill = standstill sm["controlsState"].longControlState = LongCtrlState.starting if standstill else LongCtrlState.pid sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False planner.update(sm, toggles) outputs.append(planner.output_a_target) assert outputs[2] >= 0.25 assert outputs[3] >= 0.25 assert outputs[4] >= 0.25 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_depart_accel_hold_reuses_floor_through_softening_lead_delta(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) toggles = make_toggles(model_version) sm_release = make_sm( 0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=4.1, v_lead=1.05, a_lead=1.2, radar=False, model_prob=1.0), ) sm_release["carState"].standstill = True sm_release["controlsState"].longControlState = LongCtrlState.stopping sm_release["starpilotPlan"].vCruise = 10.0 sm_release["modelV2"].action.shouldStop = False for _ in range(6): planner.update(sm_release, toggles) assert planner.output_should_stop planner.update(sm_release, toggles) assert not planner.output_should_stop assert planner.output_a_target >= longitudinal_planner_module.LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL sm_hold = make_sm( 0.5, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=5.8, v_lead=1.25, a_lead=0.08, radar=False, model_prob=1.0), ) sm_hold["carState"].standstill = False sm_hold["controlsState"].longControlState = LongCtrlState.pid sm_hold["starpilotPlan"].vCruise = 10.0 sm_hold["modelV2"].action.shouldStop = False planner.update(sm_hold, toggles) assert not planner.output_should_stop assert planner.output_a_target >= longitudinal_planner_module.LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_moving_lead_depart_accel_hold_cancels_if_lead_brakes(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) toggles = make_toggles(model_version) sm_release = make_sm( 0.0, desired_accel=0.45, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=4.8, v_lead=1.0, a_lead=0.0, radar=False, model_prob=0.99), ) sm_release["carState"].standstill = True sm_release["controlsState"].longControlState = LongCtrlState.starting sm_release["starpilotPlan"].vCruise = 10.0 sm_release["modelV2"].action.shouldStop = False planner.update(sm_release, toggles) sm_brake = make_sm( 0.18, desired_accel=0.18, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=3.9, v_lead=0.1, a_lead=-0.4, radar=False, model_prob=0.99), ) sm_brake["controlsState"].longControlState = LongCtrlState.pid sm_brake["starpilotPlan"].vCruise = 10.0 sm_brake["modelV2"].action.shouldStop = True planner.update(sm_brake, toggles) assert planner.output_should_stop assert planner.output_a_target < 0.1 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_radar_depart_kept_when_radar_lead_is_centered(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) sm = make_sm( 0.0, desired_accel=0.45, min_accel=-0.5, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=11.2, v_lead=0.63, a_lead=0.36, radar=True, model_prob=0.998, y_rel=0.2), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False set_model_lead(sm["modelV2"], 0, prob=0.999, x0=12.2, y0=0.03, v0=0.4) planner.update(sm, make_toggles(model_version)) assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_radar_depart_blocks_offcenter_radar_conflict(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) sm = make_sm( 0.0, desired_accel=0.45, min_accel=-0.5, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=11.2, v_lead=0.63, a_lead=0.36, radar=True, model_prob=0.998, y_rel=2.3), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False set_model_lead(sm["modelV2"], 0, prob=0.999, x0=12.2, y0=0.03, v0=0.4) planner.update(sm, make_toggles(model_version)) assert planner.output_a_target < longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_low_speed_radar_depart_hold_blocks_offcenter_radar_conflict(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=1.25) sm = make_sm( 1.25, desired_accel=0.20, min_accel=-0.5, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=9.95, v_lead=0.43, a_lead=0.44, radar=True, model_prob=0.999, y_rel=2.2), ) sm["carState"].standstill = False sm["controlsState"].longControlState = LongCtrlState.pid sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False set_model_lead(sm["modelV2"], 0, prob=0.999, x0=11.4, y0=0.0, v0=0.2) planner.update(sm, make_toggles(model_version)) assert planner.output_a_target < longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_radar_takeoffs_toggle_bypasses_offcenter_veto(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0) sm = make_sm( 0.0, desired_accel=0.45, min_accel=-0.5, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=11.2, v_lead=0.63, a_lead=0.36, radar=True, model_prob=0.998, y_rel=2.3), ) sm["carState"].standstill = True sm["controlsState"].longControlState = LongCtrlState.stopping sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False set_model_lead(sm["modelV2"], 0, prob=0.999, x0=12.2, y0=0.03, v0=0.4) planner.update(sm, make_toggles(model_version, radar_takeoffs=True)) assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_DEPART_MIN_ACCEL @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_low_speed_radar_takeoffs_toggle_bypasses_offcenter_veto(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=1.25) sm = make_sm( 1.25, desired_accel=0.20, min_accel=-0.5, experimental_mode=False, tracking_lead=False, lead_one=make_lead(status=True, d_rel=9.95, v_lead=0.43, a_lead=0.44, radar=True, model_prob=0.999, y_rel=2.2), ) sm["carState"].standstill = False sm["controlsState"].longControlState = LongCtrlState.pid sm["starpilotPlan"].vCruise = 10.0 sm["modelV2"].action.shouldStop = False set_model_lead(sm["modelV2"], 0, prob=0.999, x0=11.4, y0=0.0, v0=0.2) planner.update(sm, make_toggles(model_version, radar_takeoffs=True)) assert planner.output_a_target >= 0.0 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_damps_far_radar_mild_lead_brake_more_than_close_brake(model_version): far_v_ego = 29.26 far_v_cruise = 32.22 close_v_ego = 29.38 close_v_cruise = 32.22 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner_far = LongitudinalPlanner(CP, init_v=far_v_ego) planner_close = LongitudinalPlanner(CP, init_v=close_v_ego) sm_far = make_sm( far_v_ego, desired_accel=0.2, min_accel=-1.0, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=114.8, v_lead=28.88, a_lead=-0.75, radar=True, model_prob=0.9), ) sm_close = make_sm( close_v_ego, desired_accel=0.2, min_accel=-1.0, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=68.0, v_lead=26.38, a_lead=-0.76, radar=True, model_prob=0.9), ) sm_far["starpilotPlan"].vCruise = far_v_cruise sm_close["starpilotPlan"].vCruise = close_v_cruise for _ in range(80): planner_far.update(sm_far, make_toggles(model_version)) planner_close.update(sm_close, make_toggles(model_version)) assert planner_far.mode == "acc" assert planner_close.mode == "acc" assert planner_far.output_a_target > -0.4 assert planner_close.output_a_target < planner_far.output_a_target - 0.1 def test_modeld_action_passes_tomb_raider_longitudinal_params(monkeypatch): monkeypatch.setenv("DEBUG", "0") fake_commonmodel = types.ModuleType("openpilot.selfdrive.modeld.models.commonmodel_pyx") fake_commonmodel.DrivingModelFrame = object fake_commonmodel.CLContext = object monkeypatch.setitem(sys.modules, fake_commonmodel.__name__, fake_commonmodel) from openpilot.selfdrive.modeld import modeld captured = {} def fake_get_accel_from_plan(speeds, accels, t_idxs, *, action_t, vEgoStopping): captured["speeds"] = speeds captured["accels"] = accels captured["t_idxs"] = t_idxs captured["action_t"] = action_t captured["vEgoStopping"] = vEgoStopping return 0.4, True monkeypatch.setattr(modeld, "get_accel_from_plan_tomb_raider", fake_get_accel_from_plan) plan = np.zeros((1, ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH), dtype=np.float32) plan[0, :, Plan.VELOCITY] = 3.0 plan[0, :, Plan.ACCELERATION] = -0.1 prev_action = log.ModelDataV2.Action.new_message() toggles = SimpleNamespace(vEgoStopping=0.42) action = modeld.get_action_from_model( {"plan": plan}, prev_action, lat_action_t=0.2, long_action_t=0.73, v_ego=5.0, mlsim=True, is_v9=True, is_v14=False, is_v15=False, starpilot_toggles=toggles, ) assert captured["action_t"] == pytest.approx(0.73) assert captured["vEgoStopping"] == pytest.approx(0.42) assert list(captured["t_idxs"]) == ModelConstants.T_IDXS np.testing.assert_allclose(captured["speeds"], 3.0) np.testing.assert_allclose(captured["accels"], -0.1) assert action.shouldStop def test_modeld_action_uses_direct_action_head_for_v14(monkeypatch): monkeypatch.setenv("DEBUG", "0") fake_commonmodel = types.ModuleType("openpilot.selfdrive.modeld.models.commonmodel_pyx") fake_commonmodel.DrivingModelFrame = object fake_commonmodel.CLContext = object monkeypatch.setitem(sys.modules, fake_commonmodel.__name__, fake_commonmodel) from openpilot.selfdrive.modeld import modeld prev_action = log.ModelDataV2.Action.new_message() prev_action.desiredCurvature = 0.05 prev_action.desiredAcceleration = -0.2 toggles = SimpleNamespace(vEgoStopping=0.42) action = modeld.get_action_from_model( {"action": np.array([[12.0, -0.8]], dtype=np.float32)}, prev_action, lat_action_t=0.2, long_action_t=0.73, v_ego=5.0, mlsim=True, is_v9=False, is_v14=True, is_v15=False, starpilot_toggles=toggles, ) assert action.desiredCurvature == pytest.approx(0.12) assert action.desiredAcceleration < -0.2 assert not action.shouldStop def test_modeld_action_uses_current_action_head_scaling_for_v15(monkeypatch): monkeypatch.setenv("DEBUG", "0") fake_commonmodel = types.ModuleType("openpilot.selfdrive.modeld.models.commonmodel_pyx") fake_commonmodel.DrivingModelFrame = object fake_commonmodel.CLContext = object monkeypatch.setitem(sys.modules, fake_commonmodel.__name__, fake_commonmodel) from openpilot.selfdrive.modeld import modeld prev_action = log.ModelDataV2.Action.new_message() prev_action.desiredCurvature = 0.05 prev_action.desiredAcceleration = -0.2 toggles = SimpleNamespace(vEgoStopping=0.42) action = modeld.get_action_from_model( {"action": np.array([[12.0, -0.8]], dtype=np.float32)}, prev_action, lat_action_t=0.2, long_action_t=0.73, v_ego=5.0, mlsim=True, is_v9=False, is_v14=False, is_v15=True, starpilot_toggles=toggles, ) assert action.desiredCurvature == pytest.approx(0.48) assert action.desiredAcceleration < -0.2 assert not action.shouldStop def test_publish_force_stop_handoff_sets_should_stop_when_vcruise_zero(): class FakePM: def __init__(self): self.sent = {} def send(self, name, msg): self.sent[name] = msg class FakeSM(dict): def all_checks(self, service_list=None): return True logMonoTime = {"modelV2": int(1e9)} v_ego = 5.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) planner.output_a_target = -0.5 planner.output_should_stop = False planner.v_desired_trajectory = np.zeros(CONTROL_N) planner.a_desired_trajectory = np.zeros(CONTROL_N) planner.j_desired_trajectory = np.zeros(CONTROL_N) planner.fcw = False planner.mpc.source = "cruise" planner.mpc.solve_time = 0.0 pm = FakePM() sm = FakeSM(make_sm(v_ego, desired_accel=0.0, min_accel=-1.0, experimental_mode=False)) sm["starpilotPlan"].forcingStop = True sm["starpilotPlan"].forcingStopLength = 5.0 sm["starpilotPlan"].vCruise = 0.0 planner.publish(sm, pm) assert pm.sent["longitudinalPlan"].longitudinalPlan.shouldStop @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_force_stop_handoff_sets_output_should_stop_before_zero_vcruise(model_version): v_ego = 1.25 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm(v_ego, desired_accel=-0.35, min_accel=-1.0, experimental_mode=False) sm["starpilotPlan"].forcingStop = True sm["starpilotPlan"].forcingStopLength = 6.5 sm["starpilotPlan"].vCruise = 0.4 sm["modelV2"].action.shouldStop = False planner.update(sm, make_toggles(model_version)) assert planner.output_should_stop def test_publish_force_stop_handoff_sets_should_stop_when_vcruise_low(): class FakePM: def __init__(self): self.sent = {} def send(self, name, msg): self.sent[name] = msg class FakeSM(dict): def all_checks(self, service_list=None): return True logMonoTime = {"modelV2": int(1e9)} v_ego = 1.25 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) planner.output_a_target = -0.35 planner.output_should_stop = False planner.v_desired_trajectory = np.zeros(CONTROL_N) planner.a_desired_trajectory = np.zeros(CONTROL_N) planner.j_desired_trajectory = np.zeros(CONTROL_N) planner.fcw = False planner.mpc.source = "cruise" planner.mpc.solve_time = 0.0 pm = FakePM() sm = FakeSM(make_sm(v_ego, desired_accel=0.0, min_accel=-1.0, experimental_mode=False)) sm["starpilotPlan"].forcingStop = True sm["starpilotPlan"].forcingStopLength = 6.5 sm["starpilotPlan"].vCruise = 0.4 planner.publish(sm, pm) assert pm.sent["longitudinalPlan"].longitudinalPlan.shouldStop def test_allow_throttle_hysteresis_filters_gas_prob_chatter(): v_ego = 10.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm(v_ego, desired_accel=0.0, min_accel=-1.0, experimental_mode=False, gas_press_prob=0.5) toggles = make_toggles() planner.update(sm, toggles) assert planner.model_allow_throttle assert planner.allow_throttle sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.37) planner.update(sm, toggles) assert planner.model_allow_throttle assert planner.allow_throttle sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.34) planner.update(sm, toggles) assert not planner.model_allow_throttle assert not planner.allow_throttle sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.43) planner.update(sm, toggles) assert not planner.model_allow_throttle assert not planner.allow_throttle sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.46) planner.update(sm, toggles) assert planner.model_allow_throttle assert planner.allow_throttle def test_no_throttle_cap_stays_at_coast_limit_until_throttle_returns(): v_ego = 8.5 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm(v_ego, desired_accel=0.0, min_accel=-3.0, experimental_mode=False, gas_press_prob=0.0) sm["carControl"].orientationNED = [0.0, 0.1, 0.0] toggles = make_toggles() planner.update(sm, toggles) accel_coast = max(get_vehicle_min_accel(CP, v_ego), get_coast_accel(sm["carControl"].orientationNED[1])) assert not planner.allow_throttle assert planner.output_a_target == pytest.approx(accel_coast, abs=1e-3) def test_low_speed_follow_catchup_accel_cap_limits_close_vision_catchup(): v_ego = 7.8 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=18.4, v_lead=8.2, radar=False, model_prob=0.98) cap = planner.get_lead_catchup_accel_cap(lead, v_ego, 1.45) assert cap is not None assert 0.15 <= cap <= 0.45 def test_route_8bc6_post_departure_catchup_cap_skips_accelerating_away_radar_lead(): v_ego = 19.03 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead( status=True, d_rel=33.85, v_lead=19.47, a_lead=0.33, radar=True, model_prob=1.0, y_rel=-0.30, ) cap = planner.get_lead_catchup_accel_cap(lead, v_ego, 1.45) assert cap is None def test_route_8bc6_cruise_tracking_cap_skips_comfortable_accelerating_radar_follow(): v_ego = 18.744474411010742 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead( status=True, d_rel=33.650001525878906, v_lead=19.049156188964844, a_lead=0.4783128798007965, radar=True, model_prob=0.9989967942237854, y_rel=-0.2, ) cap = planner.get_cruise_tracking_lead_accel_cap( lead, v_ego, 1.25, current_source="cruise", tracking_lead_active=True, ) assert cap is None def test_route_687_voacc_catchup_cap_skips_spacious_low_closure_follow_with_flat_lead_accel(): v_ego = 12.2 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead( status=True, d_rel=25.5, v_lead=12.10, a_lead=0.0, radar=False, model_prob=0.999, y_rel=0.10, ) planner.spacious_follow_cap_bypass_until = time.monotonic() + 1.0 cap = planner.get_lead_catchup_accel_cap( lead, v_ego, 1.45, current_source="cruise", tracking_lead_active=True, ) assert cap is None def test_route_687_voacc_cruise_tracking_cap_skips_spacious_low_closure_follow_with_flat_lead_accel(): v_ego = 12.2 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead( status=True, d_rel=25.5, v_lead=12.10, a_lead=0.0, radar=False, model_prob=0.999, y_rel=0.10, ) planner.spacious_follow_cap_bypass_until = time.monotonic() + 1.0 cap = planner.get_cruise_tracking_lead_accel_cap( lead, v_ego, 1.45, current_source="cruise", tracking_lead_active=True, ) assert cap is None def test_low_speed_follow_catchup_uses_raw_vehicle_speed_when_cluster_runs_high(): v_ego = 7.8 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( v_ego, desired_accel=0.6, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=16.0, v_lead=8.4, radar=False, model_prob=0.99), ) sm["carState"].vEgoCluster = 9.2 sm["starpilotPlan"].vCruise = v_ego + 4.0 for _ in range(6): planner.update(sm, make_toggles()) assert planner.output_a_target <= 0.20 def test_low_speed_follow_transition_brake_cap_softens_first_sign_flip(): v_ego = 7.7 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=18.6, v_lead=7.6, radar=False, model_prob=0.98) cap = planner.get_low_speed_follow_transition_brake_cap(lead, v_ego, 1.45, 0.59, -0.24) assert cap is not None assert -0.14 <= cap <= -0.08 def test_low_speed_follow_transition_brake_cap_stays_off_when_gap_is_tight(): v_ego = 7.7 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=13.0, v_lead=7.6, radar=False, model_prob=0.98) cap = planner.get_low_speed_follow_transition_brake_cap(lead, v_ego, 1.45, 0.59, -0.24) assert cap is None def test_far_near_speed_follow_keeps_uncertainty_smoothing_active(): v_ego = 30.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( v_ego, desired_accel=0.0, min_accel=-1.0, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=78.0, v_lead=29.2, radar=False, model_prob=0.96), ) sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=1.0, brake_press_prob=0.52) toggles = make_toggles() for _ in range(12): planner.update(sm, toggles) assert planner.mpc.filter_time_factor > 0.75 def test_near_speed_follow_keeps_some_smoothing_under_high_uncertainty(): v_ego = 31.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=53.0, v_lead=29.8, radar=False, model_prob=0.96) sm = make_sm( v_ego, desired_accel=0.0, min_accel=-1.0, experimental_mode=False, tracking_lead=True, lead_one=lead, brake_press_prob=0.85, ) for _ in range(16): planner.update(sm, make_toggles()) assert planner.mpc.filter_time_factor >= 0.24 def test_near_speed_follow_soft_brake_cap_limits_matched_follow_pulse(): v_ego = 31.4 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=44.0, v_lead=30.1, radar=False, model_prob=0.96) sm = make_sm( v_ego, desired_accel=0.0, min_accel=-1.0, experimental_mode=False, tracking_lead=True, lead_one=lead, brake_press_prob=0.85, ) for _ in range(16): planner.update(sm, make_toggles()) assert planner.mpc.filter_time_factor >= 0.24 assert planner.output_a_target >= -0.33 def test_near_speed_follow_soft_brake_cap_covers_slightly_opening_lead(): v_ego = 29.58 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=44.44, v_lead=30.65, radar=False, model_prob=0.98) cap = planner.get_matched_follow_brake_cap(lead, v_ego, 1.45) assert cap is not None assert cap >= -0.14 assert cap <= -0.06 def test_near_speed_follow_soft_brake_cap_extends_to_spacious_modest_closing(): v_ego = 23.69 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=49.48, v_lead=21.64, a_lead=-0.014, radar=False, model_prob=0.998) cap = planner.get_matched_follow_brake_cap(lead, v_ego, 1.45) assert cap is not None assert cap >= -0.33 assert cap <= -0.22 def test_near_speed_follow_soft_brake_cap_uses_raw_vehicle_speed_when_cluster_runs_high(): v_ego = 23.0073 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( v_ego, desired_accel=0.0, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=47.52, v_lead=21.68, a_lead=-0.0646, radar=False, model_prob=0.998), ) sm["carState"].vEgoCluster = 23.6931 sm["starpilotPlan"].maxAcceleration = 0.61 for _ in range(6): planner.update(sm, make_toggles()) assert not planner.lead_is_matched_follow_window(sm["radarState"].leadOne, sm["carState"].vEgoCluster, 1.45) assert planner.output_a_target > -0.35 assert planner.output_a_target < -0.22 def test_near_speed_follow_soft_brake_cap_rejects_close_gap_even_with_modest_closing(): v_ego = 37.19 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=34.70, v_lead=35.88, radar=False, model_prob=0.99) cap = planner.get_matched_follow_brake_cap(lead, v_ego, 1.0) assert cap is None def test_follow_control_lead_prefers_active_lead1_for_matched_follow(): v_ego = 23.3 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) planner.lead_one = make_lead(status=True, d_rel=80.0, v_lead=20.0, radar=False, model_prob=0.6) planner.lead_two = make_lead(status=True, d_rel=49.9, v_lead=21.9, radar=False, model_prob=0.98) planner.mpc.source = "lead1" follow_lead = planner.get_follow_control_lead(True, v_ego, 1.45) assert follow_lead is planner.lead_two def test_follow_control_lead_disables_optional_matched_follow_override(): v_ego = 23.3 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) planner.lead_one = make_lead(status=True, d_rel=80.0, v_lead=20.0, radar=False, model_prob=0.6) planner.lead_two = make_lead(status=True, d_rel=49.9, v_lead=21.9, radar=False, model_prob=0.98) planner.mpc.source = "lead1" follow_lead = planner.get_follow_control_lead(True, v_ego, 1.45, allow_optional_far_lead_logic=False) assert follow_lead is planner.lead_one def test_follow_control_lead_keeps_matched_follow_lead_without_tracking_latch(): v_ego = 27.5 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) planner.lead_one = make_lead(status=True, d_rel=61.99, v_lead=27.63, radar=False, model_prob=0.99) follow_lead = planner.get_follow_control_lead(False, v_ego, 1.45) assert follow_lead is planner.lead_one def test_follow_control_lead_requires_real_lead_control_when_optional_logic_disabled(): v_ego = 27.5 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) planner.lead_one = make_lead(status=True, d_rel=61.99, v_lead=27.63, radar=False, model_prob=0.99) follow_lead = planner.get_follow_control_lead(False, v_ego, 1.45, allow_optional_far_lead_logic=False) assert follow_lead is None def test_far_lead_soft_brake_cap_limits_high_confidence_distant_vision_lead(): v_ego = 32.37 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=82.07, v_lead=30.63, a_lead=-0.01, radar=False, model_prob=0.99) cap = planner.get_far_lead_brake_cap(lead, v_ego, 1.70) assert cap is not None assert cap > -0.2 assert cap < -0.05 def test_matched_follow_transition_target_damps_large_comfort_sign_flip(): v_ego = 20.3 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=45.6, v_lead=19.19, a_lead=0.0, radar=False, model_prob=0.99) smoothed = planner.get_matched_follow_transition_target( lead, v_ego, 1.45, prev_output_a_target=0.12, output_a_target=-0.40, current_source="cruise", tracking_lead_active=True, ) assert smoothed is not None assert smoothed > -0.05 assert smoothed < 0.12 def test_matched_follow_transition_target_skips_urgent_closure(): v_ego = 31.5 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=35.0, v_lead=28.0, a_lead=0.0, radar=False, model_prob=0.99) smoothed = planner.get_matched_follow_transition_target( lead, v_ego, 1.45, prev_output_a_target=0.10, output_a_target=-0.60, current_source="cruise", tracking_lead_active=True, ) assert smoothed is None def test_matched_follow_transition_target_damps_low_speed_tracking_cruise_throttle_jitter(): v_ego = 14.5 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=31.1, v_lead=14.5, a_lead=0.0, radar=False, model_prob=0.999) smoothed = planner.get_matched_follow_transition_target( lead, v_ego, 1.45, prev_output_a_target=0.08, output_a_target=0.46, current_source="cruise", tracking_lead_active=True, ) assert smoothed is not None assert smoothed == pytest.approx(0.14, abs=1e-6) def test_matched_follow_transition_target_skips_low_speed_without_tracking(): v_ego = 14.5 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=31.1, v_lead=14.5, a_lead=0.0, radar=False, model_prob=0.999) smoothed = planner.get_matched_follow_transition_target( lead, v_ego, 1.45, prev_output_a_target=0.08, output_a_target=0.46, current_source="cruise", tracking_lead_active=False, ) assert smoothed is None def test_matched_follow_transition_target_skips_low_speed_real_braking(): v_ego = 14.5 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=29.0, v_lead=13.6, a_lead=0.0, radar=False, model_prob=0.999) smoothed = planner.get_matched_follow_transition_target( lead, v_ego, 1.45, prev_output_a_target=0.08, output_a_target=-0.30, current_source="cruise", tracking_lead_active=True, ) assert smoothed is None def test_cruise_tracking_lead_accel_cap_limits_mid_speed_follow_nibble(): v_ego = 16.2 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=33.4, v_lead=16.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.12) cap = planner.get_cruise_tracking_lead_accel_cap( lead, v_ego, 1.45, current_source="cruise", tracking_lead_active=True, ) assert cap is not None assert 0.05 <= cap <= 0.10 def test_cruise_tracking_lead_accel_cap_blocks_unresolved_raw_close_lead_burst(): v_ego = 17.6 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=41.9, v_lead=14.2, a_lead=0.0, radar=True, model_prob=0.99, y_rel=-0.97) cap = planner.get_cruise_tracking_lead_accel_cap( lead, v_ego, 1.45, current_source="cruise", tracking_lead_active=False, ) assert cap is not None assert 0.0 <= cap <= 0.05 def test_cruise_tracking_lead_accel_cap_skips_when_lead_clearly_pulls_away(): v_ego = 14.5 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=35.0, v_lead=16.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.1) cap = planner.get_cruise_tracking_lead_accel_cap( lead, v_ego, 1.45, current_source="cruise", tracking_lead_active=True, ) assert cap is None def test_cruise_tracking_lead_accel_cap_skips_accelerating_away_radar_lead(): v_ego = 11.4 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=20.5, v_lead=12.4, a_lead=0.46, radar=True, model_prob=1.0, y_rel=0.1) cap = planner.get_cruise_tracking_lead_accel_cap( lead, v_ego, 1.45, current_source="cruise", tracking_lead_active=True, ) assert cap is None def test_cruise_tracking_lead_accel_cap_skips_spacious_tracking_only_follow(): v_ego = 18.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=45.0, v_lead=17.4, a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.1) cap = planner.get_cruise_tracking_lead_accel_cap( lead, v_ego, 1.45, current_source="cruise", tracking_lead_active=True, ) assert cap is None def test_route_8bc6_post_departure_cruise_cap_skips_accelerating_away_radar_lead(): v_ego = 19.03 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead( status=True, d_rel=33.85, v_lead=19.47, a_lead=0.33, radar=True, model_prob=1.0, y_rel=-0.30, ) cap = planner.get_cruise_tracking_lead_accel_cap( lead, v_ego, 1.45, current_source="cruise", tracking_lead_active=True, ) assert cap is None def test_route_8bc6_catchup_cap_skips_comfortable_accelerating_radar_follow(): v_ego = 24.108949661254883 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead( status=True, d_rel=36.75, v_lead=24.234233856201172, a_lead=0.3050585687160492, radar=True, model_prob=0.9995405077934265, y_rel=-0.2, ) cap = planner.get_lead_catchup_accel_cap( lead, v_ego, 1.160530924797058, current_source="cruise", tracking_lead_active=True, ) assert cap is None def test_route_8bc6_catchup_cap_skips_slightly_negative_delta_when_lead_accelerates_away(): v_ego = 22.293441772460938 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead( status=True, d_rel=36.5, v_lead=22.264942169189453, a_lead=0.2780209183692932, radar=True, model_prob=0.999357283115387, y_rel=0.35, ) cap = planner.get_lead_catchup_accel_cap( lead, v_ego, 1.2014657258987427, current_source="cruise", tracking_lead_active=True, ) assert cap is None def test_route_8bc6_catchup_cap_skips_comfortable_accelerating_lead_when_source_flips_to_lead0(): v_ego = 24.361867904663086 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead( status=True, d_rel=38.5, v_lead=24.46365737915039, a_lead=0.4625629186630249, radar=True, model_prob=1.0, y_rel=0.0, ) cap = planner.get_lead_catchup_accel_cap( lead, v_ego, 1.3549551963806152, current_source="lead0", tracking_lead_active=True, ) assert planner.is_comfortable_accelerating_away_follow(lead, v_ego, 1.3549551963806152) assert cap is None def test_route_8bc6_radar_matched_follow_catchup_cap_skips_mild_pullaway_after_lead_lock(): v_ego = 23.96 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead( status=True, d_rel=33.6, v_lead=24.23, a_lead=0.16, radar=True, model_prob=1.0, y_rel=0.0, ) cap = planner.get_lead_catchup_accel_cap( lead, v_ego, 1.16, current_source="lead0", tracking_lead_active=True, ) assert planner.lead_is_matched_follow_window(lead, v_ego, 1.16) assert cap is None def test_route_8bc6_radar_matched_follow_catchup_cap_keeps_cap_when_pullaway_is_not_confirmed(): v_ego = 23.96 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead( status=True, d_rel=33.6, v_lead=24.23, a_lead=0.08, radar=True, model_prob=1.0, y_rel=0.0, ) cap = planner.get_lead_catchup_accel_cap( lead, v_ego, 1.16, current_source="lead0", tracking_lead_active=True, ) assert planner.lead_is_matched_follow_window(lead, v_ego, 1.16) assert cap is not None def test_route_8bc6_radar_matched_follow_catchup_cap_skips_buffer_edge_square_wave(): v_ego = 22.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead( status=True, d_rel=41.558, v_lead=22.3, a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.0, ) cap = planner.get_lead_catchup_accel_cap( lead, v_ego, 1.45, current_source="cruise", tracking_lead_active=True, ) assert planner.lead_is_matched_follow_window(lead, v_ego, 1.45) assert cap is None def test_route_8bc6_radar_matched_follow_catchup_cap_holds_small_cap_for_slower_lead_on_cruise(): v_ego = 22.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead( status=True, d_rel=39.108, v_lead=21.2, a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.0, ) cap = planner.get_lead_catchup_accel_cap( lead, v_ego, 1.45, current_source="cruise", tracking_lead_active=True, ) assert planner.lead_is_matched_follow_window(lead, v_ego, 1.45) assert cap == pytest.approx(0.04, abs=1e-6) def test_route_8bc6_post_departure_settle_latch_bypasses_mild_closure_catchup_cap(): v_ego = 16.4023914337 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead( status=True, d_rel=25.75, v_lead=15.9339208603, a_lead=0.1164954603, radar=True, model_prob=1.0, y_rel=0.0, ) cap_without_latch = planner.get_lead_catchup_accel_cap( lead, v_ego, 1.25, current_source="lead0", tracking_lead_active=True, ) planner.post_departure_follow_settle_until = time.monotonic() + 5.0 cap_with_latch = planner.get_lead_catchup_accel_cap( lead, v_ego, 1.25, current_source="lead0", tracking_lead_active=True, ) assert cap_without_latch == pytest.approx(0.03, abs=1e-6) assert cap_with_latch is None def test_route_8bc6_post_departure_settle_latch_bypasses_mild_closure_cruise_cap(): v_ego = 19.2975330353 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead( status=True, d_rel=28.9, v_lead=19.6019687653, a_lead=0.1241656989, radar=True, model_prob=1.0, y_rel=0.0, ) cap_without_latch = planner.get_cruise_tracking_lead_accel_cap( lead, v_ego, 1.25, current_source="cruise", tracking_lead_active=True, ) planner.post_departure_follow_settle_until = time.monotonic() + 5.0 cap_with_latch = planner.get_cruise_tracking_lead_accel_cap( lead, v_ego, 1.25, current_source="cruise", tracking_lead_active=True, ) assert cap_without_latch == pytest.approx(0.10831585854957321, abs=1e-6) assert cap_with_latch is None def test_post_departure_settle_latch_does_not_bypass_when_lead_brakes_again(): v_ego = 19.192998192 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) planner.post_departure_follow_settle_until = time.monotonic() + 5.0 lead = make_lead( status=True, d_rel=30.5, v_lead=19.147550216, a_lead=-0.16, radar=True, model_prob=1.0, y_rel=0.2, ) catchup_cap = planner.get_lead_catchup_accel_cap( lead, v_ego, 1.25, current_source="cruise", tracking_lead_active=True, ) cruise_cap = planner.get_cruise_tracking_lead_accel_cap( lead, v_ego, 1.25, current_source="cruise", tracking_lead_active=True, ) assert catchup_cap is not None assert cruise_cap is not None assert planner.post_departure_follow_settle_until == 0.0 def test_post_departure_settle_latch_clears_once_follow_has_settled_near_target(): v_ego = 16.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) planner.post_departure_follow_settle_until = time.monotonic() + 5.0 lead = make_lead( status=True, d_rel=20.48, v_lead=16.25, a_lead=0.18, radar=True, model_prob=1.0, y_rel=0.0, ) cap = planner.get_lead_catchup_accel_cap( lead, v_ego, 1.25, current_source="lead0", tracking_lead_active=True, ) assert cap is not None assert planner.post_departure_follow_settle_until == 0.0 def test_post_departure_pullaway_bypass_does_not_skip_when_lead_brakes_again(): v_ego = 19.03 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead( status=True, d_rel=33.85, v_lead=18.90, a_lead=-0.35, radar=True, model_prob=1.0, y_rel=-0.30, ) catchup_cap = planner.get_lead_catchup_accel_cap(lead, v_ego, 1.45) cruise_cap = planner.get_cruise_tracking_lead_accel_cap( lead, v_ego, 1.45, current_source="cruise", tracking_lead_active=True, ) assert catchup_cap is not None assert cruise_cap is not None def test_cruise_tracking_lead_accel_transition_target_damps_mid_speed_reacquisition_burst(): v_ego = 16.2 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=20.0, v_lead=17.4, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.18) smoothed = planner.get_cruise_tracking_lead_accel_transition_target( lead, v_ego, 1.45, prev_output_a_target=0.10, output_a_target=1.10, current_source="cruise", ) assert smoothed is not None assert smoothed == pytest.approx(0.23, abs=1e-2) def test_cruise_tracking_lead_accel_transition_target_skips_clear_pullaway(): v_ego = 16.2 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=34.0, v_lead=19.2, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.12) smoothed = planner.get_cruise_tracking_lead_accel_transition_target( lead, v_ego, 1.45, prev_output_a_target=0.10, output_a_target=1.10, current_source="cruise", ) assert smoothed is None def test_mild_follow_zero_cross_guard_coasts_on_cruise_sign_flip(): v_ego = 19.8 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=42.0, v_lead=19.0, a_lead=-0.15, radar=False, model_prob=0.99, y_rel=0.10) guarded = planner.get_mild_follow_zero_cross_guard_target( lead, v_ego, 1.45, prev_output_a_target=0.18, output_a_target=-0.12, current_source="cruise", tracking_lead_active=True, ) assert guarded == pytest.approx(0.0, abs=1e-6) def test_mild_follow_zero_cross_guard_coasts_on_near_duplicate_sign_flip(): v_ego = 24.5 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=51.0, v_lead=21.7, a_lead=-0.25, radar=False, model_prob=0.99, y_rel=0.08) guarded = planner.get_mild_follow_zero_cross_guard_target( lead, v_ego, 1.45, prev_output_a_target=0.33, output_a_target=-0.13, current_source="lead1", tracking_lead_active=True, ) assert guarded == pytest.approx(0.0, abs=1e-6) def test_mild_follow_zero_cross_guard_skips_urgent_close_follow(): v_ego = 17.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=16.0, v_lead=12.0, a_lead=-0.45, radar=False, model_prob=0.99, y_rel=0.05) guarded = planner.get_mild_follow_zero_cross_guard_target( lead, v_ego, 1.45, prev_output_a_target=0.22, output_a_target=-0.28, current_source="cruise", tracking_lead_active=True, ) assert guarded is None def test_prioritize_smooth_following_skips_post_097_follow_nudges(): v_ego = 16.2 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) lead = make_lead(status=True, d_rel=33.4, v_lead=16.0, a_lead=0.0, radar=False, model_prob=0.99, y_rel=0.12) def run_once(prioritize_smooth_following: bool) -> list[str]: planner = LongitudinalPlanner(CP, init_v=v_ego) sm = make_sm( v_ego, desired_accel=0.8, min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=lead, ) sm["starpilotPlan"].vCruise = v_ego + 8.0 calls = [] def record(name, return_value=None): def _inner(self, *args, **kwargs): calls.append(name) return return_value return types.MethodType(_inner, planner) planner.get_follow_control_lead = record("get_follow_control_lead", lead) planner.get_lead_catchup_accel_cap = record("get_lead_catchup_accel_cap", None) planner.get_tracked_vision_model_brake_floor = record("get_tracked_vision_model_brake_floor", None) planner.get_low_speed_follow_transition_brake_cap = record("get_low_speed_follow_transition_brake_cap", None) planner.get_tracked_vision_model_brake_cap = record("get_tracked_vision_model_brake_cap", None) planner.get_cruise_tracking_lead_accel_cap = record("get_cruise_tracking_lead_accel_cap", None) planner.get_cruise_tracking_lead_accel_transition_target = record("get_cruise_tracking_lead_accel_transition_target", None) planner.update(sm, make_toggles(prioritize_smooth_following=prioritize_smooth_following)) return calls calls_default = run_once(False) calls_smooth = run_once(True) expected_calls = { "get_lead_catchup_accel_cap", "get_follow_control_lead", "get_tracked_vision_model_brake_floor", "get_low_speed_follow_transition_brake_cap", "get_tracked_vision_model_brake_cap", "get_cruise_tracking_lead_accel_cap", "get_cruise_tracking_lead_accel_transition_target", } assert expected_calls.issubset(set(calls_default)) assert not expected_calls.intersection(set(calls_smooth)) def test_near_duplicate_lead_source_hysteresis_prefers_previous_source(): v_ego = 27.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=46.2, v_lead=25.5, a_lead=-0.05, radar=False, model_prob=0.99) lead_two = make_lead(status=True, d_rel=46.8, v_lead=25.55, a_lead=-0.03, radar=False, model_prob=0.99) lead_0_bias, lead_1_bias = planner.mpc.get_near_duplicate_lead_source_hysteresis("lead0", lead_one, lead_two, v_ego) assert lead_0_bias == 0.0 assert lead_1_bias > 0.0 def test_stable_follow_cruise_hysteresis_applies_for_radar_lead(): v_ego = 27.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=40.5, v_lead=26.3, a_lead=-0.02, radar=True, model_prob=1.0) hysteresis = planner.mpc.get_stable_follow_cruise_hysteresis(lead, v_ego, 1.45) assert hysteresis > 0.0 def test_stable_follow_cruise_hysteresis_skips_fast_closing_radar_lead(): v_ego = 27.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead = make_lead(status=True, d_rel=40.5, v_lead=21.5, a_lead=-0.02, radar=True, model_prob=1.0) hysteresis = planner.mpc.get_stable_follow_cruise_hysteresis(lead, v_ego, 1.45) assert hysteresis == 0.0 def test_near_duplicate_lead_source_hysteresis_prefers_previous_source_for_identical_radar_track(): v_ego = 27.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=34.6, v_lead=24.2, a_lead=-0.04, radar=True, model_prob=1.0) lead_two = make_lead(status=True, d_rel=34.7, v_lead=24.2, a_lead=-0.04, radar=True, model_prob=1.0) lead_one.vRel = -0.8 lead_two.vRel = -0.78 lead_one.radarTrackId = 123 lead_two.radarTrackId = 123 lead_0_bias, lead_1_bias = planner.mpc.get_near_duplicate_lead_source_hysteresis("lead0", lead_one, lead_two, v_ego) assert lead_0_bias == 0.0 assert lead_1_bias > 0.0 def test_near_duplicate_leads_detect_identical_radar_track_below_45_mph(): v_ego = 14.31 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=25.7, v_lead=14.61, a_lead=0.0, radar=True, model_prob=1.0) lead_two = make_lead(status=True, d_rel=25.7, v_lead=14.61, a_lead=0.0, radar=True, model_prob=1.0) lead_one.vRel = lead_one.vLead - v_ego lead_two.vRel = lead_two.vLead - v_ego lead_one.radarTrackId = 2493 lead_two.radarTrackId = 2493 assert planner.mpc.leads_are_near_duplicates(lead_one, lead_two, v_ego) def test_identical_radar_duplicate_source_hold_keeps_previous_label(): v_ego = 21.6 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=33.5, v_lead=20.7, a_lead=-0.03, radar=True, model_prob=1.0) lead_two = make_lead(status=True, d_rel=33.5, v_lead=20.7, a_lead=-0.03, radar=True, model_prob=1.0) lead_one.radarTrackId = 2493 lead_two.radarTrackId = 2493 sticky = planner.mpc.get_identical_radar_duplicate_source_hold("lead1", lead_one, lead_two, 33.52, 33.50) assert sticky == "lead1" def test_identical_radar_duplicate_cruise_hold_keeps_previous_lead_through_route_like_crossover(): v_ego = 22.57 t_follow = 1.20 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=32.3, v_lead=23.74, a_lead=0.67, radar=True, model_prob=1.0) lead_two = make_lead(status=True, d_rel=32.3, v_lead=23.74, a_lead=0.67, radar=True, model_prob=1.0) lead_one.radarTrackId = 2493 lead_two.radarTrackId = 2493 sticky = planner.mpc.get_identical_radar_duplicate_cruise_hold( "lead0", lead_one, lead_two, 144.92, 144.91, 135.10, v_ego, t_follow, ) assert sticky == "lead0" def test_identical_radar_duplicate_cruise_hold_skips_clear_pullaway(): v_ego = 22.57 t_follow = 1.20 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=48.0, v_lead=25.5, a_lead=0.45, radar=True, model_prob=1.0) lead_two = make_lead(status=True, d_rel=48.0, v_lead=25.5, a_lead=0.45, radar=True, model_prob=1.0) lead_one.radarTrackId = 2493 lead_two.radarTrackId = 2493 sticky = planner.mpc.get_identical_radar_duplicate_cruise_hold( "lead0", lead_one, lead_two, 180.0, 180.0, 150.0, v_ego, t_follow, ) assert sticky is None def test_identical_radar_duplicate_cruise_bias_penalizes_near_target_follow(): v_ego = 23.8 t_follow = 1.15 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=35.8, v_lead=23.2, a_lead=0.02, radar=True, model_prob=1.0) lead_two = make_lead(status=True, d_rel=35.8, v_lead=23.2, a_lead=0.02, radar=True, model_prob=1.0) lead_one.radarTrackId = 2493 lead_two.radarTrackId = 2493 bias = planner.mpc.get_identical_radar_duplicate_cruise_bias(lead_one, lead_two, v_ego, t_follow) assert bias > 0.0 def test_identical_radar_duplicate_cruise_bias_skips_far_pullaway_follow(): v_ego = 23.8 t_follow = 1.15 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=52.0, v_lead=25.5, a_lead=0.08, radar=True, model_prob=1.0) lead_two = make_lead(status=True, d_rel=52.0, v_lead=25.5, a_lead=0.08, radar=True, model_prob=1.0) lead_one.radarTrackId = 2493 lead_two.radarTrackId = 2493 bias = planner.mpc.get_identical_radar_duplicate_cruise_bias(lead_one, lead_two, v_ego, t_follow) assert bias == 0.0 def test_near_duplicate_lead_source_hysteresis_skips_distinct_leads(): v_ego = 27.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=41.0, v_lead=23.8, a_lead=0.0, radar=False, model_prob=0.99) lead_two = make_lead(status=True, d_rel=48.0, v_lead=25.4, a_lead=0.0, radar=False, model_prob=0.99) lead_0_bias, lead_1_bias = planner.mpc.get_near_duplicate_lead_source_hysteresis("lead0", lead_one, lead_two, v_ego) assert lead_0_bias == 0.0 assert lead_1_bias == 0.0 def test_duplicate_vision_comfort_lead_prefers_centered_candidate_before_closer_offset_candidate(): v_ego = 25.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10) lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05) lead_one.vRel = lead_one.vLead - v_ego lead_two.vRel = lead_two.vLead - v_ego planner.lead_one = lead_one planner.lead_two = lead_two selected = planner.get_duplicate_vision_comfort_lead(v_ego) assert selected is lead_one assert planner.duplicate_vision_comfort_lead_source == "lead0" def test_duplicate_vision_comfort_lead_latches_source_until_duplicate_cluster_resolves(): v_ego = 25.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10) lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05) lead_one.vRel = lead_one.vLead - v_ego lead_two.vRel = lead_two.vLead - v_ego planner.lead_one = lead_one planner.lead_two = lead_two initial = planner.get_duplicate_vision_comfort_lead(v_ego) lead_one.yRel = 1.25 lead_two.yRel = 0.05 lead_one.dRel = 44.4 lead_two.dRel = 44.5 latched = planner.get_duplicate_vision_comfort_lead(v_ego) assert initial is lead_one assert latched is lead_one assert planner.duplicate_vision_comfort_lead_source == "lead0" def test_duplicate_vision_comfort_lead_resets_when_duplicate_cluster_clears(): v_ego = 25.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=44.8, v_lead=24.2, a_lead=-0.03, radar=False, model_prob=0.99, y_rel=0.10) lead_two = make_lead(status=True, d_rel=44.2, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99, y_rel=1.05) lead_one.vRel = lead_one.vLead - v_ego lead_two.vRel = lead_two.vLead - v_ego planner.lead_one = lead_one planner.lead_two = lead_two assert planner.get_duplicate_vision_comfort_lead(v_ego) is lead_one lead_two.dRel = 49.5 assert planner.get_duplicate_vision_comfort_lead(v_ego) is None assert planner.duplicate_vision_comfort_lead_source is None def test_near_duplicate_lead_transition_target_damps_same_source_sign_flip(): v_ego = 25.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=44.6, v_lead=24.05, a_lead=-0.03, radar=False, model_prob=0.99) lead_two = make_lead(status=True, d_rel=45.1, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99) lead_one.vRel = -0.95 lead_two.vRel = -1.00 planner.lead_one = lead_one planner.lead_two = lead_two smoothed = planner.get_near_duplicate_lead_transition_target( lead_two, v_ego, 1.45, prev_output_a_target=-1.10, output_a_target=0.13, current_source="lead1", tracking_lead_active=True, ) assert smoothed is not None assert smoothed == pytest.approx(-0.92, abs=1e-6) def test_near_duplicate_lead_transition_target_damps_tracking_cruise_sign_flip(): v_ego = 25.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=44.6, v_lead=24.05, a_lead=-0.03, radar=False, model_prob=0.99) lead_two = make_lead(status=True, d_rel=45.1, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99) lead_one.vRel = -0.95 lead_two.vRel = -1.00 planner.lead_one = lead_one planner.lead_two = lead_two smoothed = planner.get_near_duplicate_lead_transition_target( lead_two, v_ego, 1.45, prev_output_a_target=-1.10, output_a_target=0.13, current_source="cruise", tracking_lead_active=True, ) assert smoothed is not None assert smoothed == pytest.approx(-0.92, abs=1e-6) def test_near_duplicate_lead_transition_target_damps_low_speed_duplicate_radar_handoff(): v_ego = 17.61 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=41.9, v_lead=16.85, a_lead=0.0, radar=True, model_prob=1.0) lead_two = make_lead(status=True, d_rel=41.9, v_lead=16.85, a_lead=0.0, radar=True, model_prob=1.0) lead_one.vRel = lead_one.vLead - v_ego lead_two.vRel = lead_two.vLead - v_ego lead_one.radarTrackId = 2493 lead_two.radarTrackId = 2493 planner.lead_one = lead_one planner.lead_two = lead_two smoothed = planner.get_near_duplicate_lead_transition_target( lead_one, v_ego, 1.45, prev_output_a_target=0.89, output_a_target=0.05, current_source="cruise", tracking_lead_active=True, ) assert smoothed is not None assert smoothed == pytest.approx(0.57, abs=1e-6) def test_near_duplicate_lead_transition_target_damps_generous_headway_duplicate_vision_sign_flip(): v_ego = 25.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=54.0, v_lead=20.8, a_lead=-0.02, radar=False, model_prob=0.99) lead_two = make_lead(status=True, d_rel=54.3, v_lead=20.82, a_lead=-0.01, radar=False, model_prob=0.99) lead_one.vRel = lead_one.vLead - v_ego lead_two.vRel = lead_two.vLead - v_ego planner.lead_one = lead_one planner.lead_two = lead_two smoothed = planner.get_near_duplicate_lead_transition_target( lead_two, v_ego, 1.45, prev_output_a_target=0.70, output_a_target=-0.61, current_source="lead1", tracking_lead_active=True, ) assert smoothed is not None assert smoothed == pytest.approx(0.52, abs=1e-6) def test_near_duplicate_lead_transition_target_skips_fast_duplicate_vision_sign_flip_when_headway_is_not_generous(): v_ego = 25.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=43.0, v_lead=20.8, a_lead=-0.02, radar=False, model_prob=0.99) lead_two = make_lead(status=True, d_rel=43.3, v_lead=20.82, a_lead=-0.01, radar=False, model_prob=0.99) lead_one.vRel = lead_one.vLead - v_ego lead_two.vRel = lead_two.vLead - v_ego planner.lead_one = lead_one planner.lead_two = lead_two smoothed = planner.get_near_duplicate_lead_transition_target( lead_two, v_ego, 1.45, prev_output_a_target=0.70, output_a_target=-0.61, current_source="lead1", tracking_lead_active=True, ) assert smoothed is None def test_duplicate_slow_lead_brake_hold_prevents_zero_cross_from_duplicate_voacc_leads(): v_ego = 24.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=19.2, v_lead=20.0, a_lead=-0.38, radar=False, model_prob=0.998) lead_two = make_lead(status=True, d_rel=19.25, v_lead=20.02, a_lead=-0.41, radar=False, model_prob=0.996) lead_one.vRel = lead_one.vLead - v_ego lead_two.vRel = lead_two.vLead - v_ego planner.lead_one = lead_one planner.lead_two = lead_two smoothed = planner.get_duplicate_slow_lead_brake_hold_target( lead_one, v_ego, 1.0, prev_output_a_target=-3.50, output_a_target=0.0, current_source="lead0", tracking_lead_active=True, ) assert smoothed is not None assert smoothed == pytest.approx(-3.28, abs=1e-6) def test_duplicate_slow_lead_brake_hold_skips_distinct_leads(): v_ego = 24.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=19.2, v_lead=20.0, a_lead=-0.38, radar=False, model_prob=0.998) lead_two = make_lead(status=True, d_rel=24.0, v_lead=21.5, a_lead=-0.10, radar=False, model_prob=0.996) lead_one.vRel = lead_one.vLead - v_ego lead_two.vRel = lead_two.vLead - v_ego planner.lead_one = lead_one planner.lead_two = lead_two smoothed = planner.get_duplicate_slow_lead_brake_hold_target( lead_one, v_ego, 1.0, prev_output_a_target=-3.50, output_a_target=0.0, current_source="lead0", tracking_lead_active=True, ) assert smoothed is None def test_near_duplicate_lead_transition_target_skips_plain_cruise_without_tracking(): v_ego = 25.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=v_ego) lead_one = make_lead(status=True, d_rel=44.6, v_lead=24.05, a_lead=-0.03, radar=False, model_prob=0.99) lead_two = make_lead(status=True, d_rel=45.1, v_lead=24.0, a_lead=-0.04, radar=False, model_prob=0.99) lead_one.vRel = -0.95 lead_two.vRel = -1.00 planner.lead_one = lead_one planner.lead_two = lead_two smoothed = planner.get_near_duplicate_lead_transition_target( lead_two, v_ego, 1.45, prev_output_a_target=-1.10, output_a_target=0.13, current_source="cruise", tracking_lead_active=False, ) assert smoothed is None