mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-20 15:54:13 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
6bd48c28e8feea277cfdd3916c313dfec6236a93
StarPilot/selfdrive/controls/tests/test_longitudinal_planner.py
T
firestar5683 6bd48c28e8 The smallest promotion
2026-08-10 23:17:21 -05:00

3441 lines
121 KiB
Python
Raw Blame History

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
from opendbc.car.toyota.interface import CarInterface as ToyotaCarInterface
from opendbc.car.toyota.values import CAR as TOYOTA_CAR
import openpilot.selfdrive.controls.lib.longitudinal_planner as longitudinal_planner_module
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
from openpilot.selfdrive.controls.lib.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 LongitudinalMpc, soften_far_radar_lead_accel, should_trigger_planner_fcw
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
get_follow_prebrake_min_headway,
get_toyota_sienna_post_departure_restop_cap,
is_gm_silverado_early_follow_lead,
is_toyota_rav4_tss2_2023,
)
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
from openpilot.selfdrive.modeld import modeld
class _SmoothParams:
def __init__(self, value, developer=True, safe=False):
self.value = value
self.developer = developer
self.safe = safe
def get_bool(self, key):
return self.developer if key == "DeveloperUI" else self.safe
def get_float(self, key, **kwargs):
return self.value
def test_model_smoothing_is_developer_gated_and_quantized():
assert modeld._model_smooth_seconds(_SmoothParams(0.126), "LatSmoothSeconds", 0.1) == pytest.approx(0.125)
assert modeld._model_smooth_seconds(_SmoothParams(0.126, developer=False), "LatSmoothSeconds", 0.1) == pytest.approx(0.1)
assert modeld._model_smooth_seconds(_SmoothParams(0.126, safe=True), "LatSmoothSeconds", 0.1) == pytest.approx(0.1)
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 test_mpc_duplicate_lead_filters_do_not_cross_contaminate_tracks():
mpc = LongitudinalMpc()
mpc.set_cur_state(20.0, 0.0)
mpc.current_filter_time = 0.5
lead_one = make_lead(status=True, d_rel=35.0, v_lead=12.0, model_prob=1.0)
lead_two = make_lead(status=True, d_rel=35.0, v_lead=28.0, model_prob=1.0)
mpc.process_lead(lead_one, lead_index=0, smooth_duplicate_vision=True)
mpc.process_lead(lead_two, lead_index=1, smooth_duplicate_vision=True)
assert mpc.duplicate_lead_v_filters[0].x == pytest.approx(12.0)
assert mpc.duplicate_lead_v_filters[1].x == pytest.approx(28.0)
lead_one.vLead = 14.0
mpc.process_lead(lead_one, lead_index=0, smooth_duplicate_vision=True)
assert 12.0 < mpc.duplicate_lead_v_filters[0].x < 14.0
assert mpc.duplicate_lead_v_filters[1].x == pytest.approx(28.0)
def test_mpc_duplicate_vision_filter_smooths_distance_jumps_per_track():
mpc = LongitudinalMpc()
mpc.set_cur_state(27.0, 0.0)
mpc.current_filter_time = 1.2
lead_one = make_lead(status=True, d_rel=52.0, v_lead=25.0, model_prob=1.0)
lead_two = make_lead(status=True, d_rel=70.0, v_lead=25.0, model_prob=1.0)
first_one = mpc.process_lead(lead_one, lead_index=0, smooth_duplicate_vision=True)
first_two = mpc.process_lead(lead_two, lead_index=1, smooth_duplicate_vision=True)
assert first_one[0, 0] == pytest.approx(52.0)
assert first_two[0, 0] == pytest.approx(70.0)
lead_one.dRel = 60.0
filtered_one = mpc.process_lead(lead_one, lead_index=0, smooth_duplicate_vision=True)
assert 52.0 < filtered_one[0, 0] < 54.0
assert mpc.duplicate_lead_x_filters[1].x == pytest.approx(70.0)
def test_mpc_duplicate_vision_distance_filter_bypasses_urgent_path():
mpc = LongitudinalMpc()
mpc.set_cur_state(27.0, 0.0)
mpc.current_filter_time = 1.2
lead = make_lead(status=True, d_rel=60.0, v_lead=25.0, model_prob=1.0)
mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=True)
lead.dRel = 35.0
raw = mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=False)
assert raw[0, 0] == pytest.approx(35.0)
assert not mpc.duplicate_lead_x_filters[0].initialized
def test_mpc_duplicate_vision_distance_filter_never_delays_closer_lead():
mpc = LongitudinalMpc()
mpc.set_cur_state(27.0, 0.0)
mpc.current_filter_time = 1.2
lead = make_lead(status=True, d_rel=60.0, v_lead=25.0, model_prob=1.0)
mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=True)
lead.dRel = 42.0
closer = mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=True)
assert closer[0, 0] == pytest.approx(42.0)
def test_mpc_duplicate_vision_filter_damps_low_speed_velocity_noise():
mpc = LongitudinalMpc()
mpc.set_cur_state(18.0, 0.0)
mpc.current_filter_time = 0.0
lead = make_lead(status=True, d_rel=38.0, v_lead=17.0, model_prob=1.0)
mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=True)
lead.vLead = 20.0
mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=True)
assert 17.0 < mpc.duplicate_lead_v_filters[0].x < 20.0
def test_mpc_distinct_vision_lead_uses_unchanged_baseline_filter():
mpc = LongitudinalMpc()
baseline_mpc = LongitudinalMpc()
mpc.set_cur_state(18.0, 0.0)
baseline_mpc.set_cur_state(18.0, 0.0)
mpc.current_filter_time = 0.0
baseline_mpc.current_filter_time = 0.0
lead = make_lead(status=True, d_rel=38.0, v_lead=17.0, model_prob=1.0)
mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=False)
baseline_mpc.process_lead(lead)
lead.vLead = 20.0
mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=False)
baseline_mpc.process_lead(lead)
assert mpc.lead_v_filter.x == pytest.approx(baseline_mpc.lead_v_filter.x)
def test_mpc_panic_bypass_immediately_removes_duplicate_vision_filter():
mpc = LongitudinalMpc()
mpc.set_cur_state(18.0, 0.0)
mpc.current_filter_time = 0.0
lead = make_lead(status=True, d_rel=25.0, v_lead=17.0, model_prob=1.0)
mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=True)
mpc.set_weights(v_ego=18.0, panic_bypass=True)
lead.vLead = 10.0
mpc.process_lead(lead, lead_index=0, smooth_duplicate_vision=False)
assert mpc.filter_time_factor == 0.0
assert mpc.lead_v_filter.x == pytest.approx(10.0)
def test_hrv_far_follow_output_slew_damps_only_continuous_safe_follow():
v_ego = 24.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_HRV_3G)
planner = LongitudinalPlanner(CP, init_v=v_ego)
planner.lead_one = make_lead(status=True, d_rel=58.0, v_lead=20.0, model_prob=0.99)
planner.lead_two = make_lead(status=False)
initial = planner.get_vehicle_far_follow_slew_target(
v_ego, prev_target=0.0, target=-0.6, output_should_stop=False, panic_bypass=False,
)
assert initial == pytest.approx(-0.6)
smoothed = planner.get_vehicle_far_follow_slew_target(
v_ego, prev_target=initial, target=0.4, output_should_stop=False, panic_bypass=False,
)
assert smoothed == pytest.approx(-0.5)
@pytest.mark.parametrize("d_rel,v_lead,output_should_stop,panic_bypass", [
(20.0, 20.0, False, False),
(35.0, 18.0, False, False),
(58.0, 20.0, True, False),
(58.0, 20.0, False, True),
])
def test_hrv_far_follow_output_slew_bypasses_urgent_scenes(d_rel, v_lead, output_should_stop, panic_bypass):
v_ego = 24.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_HRV_3G)
planner = LongitudinalPlanner(CP, init_v=v_ego)
planner.lead_one = make_lead(status=True, d_rel=58.0, v_lead=20.0, model_prob=0.99)
planner.lead_two = make_lead(status=False)
planner.far_follow_output_slew_active = True
planner.lead_one.dRel = d_rel
planner.lead_one.vLead = v_lead
target = planner.get_vehicle_far_follow_slew_target(
v_ego, prev_target=0.4, target=-1.0,
output_should_stop=output_should_stop, panic_bypass=panic_bypass,
)
assert target == pytest.approx(-1.0)
assert not planner.far_follow_output_slew_active
def test_non_hrv_has_no_vehicle_far_follow_output_slew():
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=24.0)
planner.lead_one = make_lead(status=True, d_rel=58.0, v_lead=20.0, model_prob=0.99)
planner.lead_two = make_lead(status=False)
target = planner.get_vehicle_far_follow_slew_target(
24.0, prev_target=0.4, target=-1.0, output_should_stop=False, panic_bypass=False,
)
assert target == pytest.approx(-1.0)
def test_depart_release_hold_rejects_nearby_stopped_lead_conflict():
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP)
planner.lead_one = make_lead(status=True, d_rel=4.0, v_lead=0.8, a_lead=0.2, model_prob=1.0)
planner.lead_two = make_lead(status=True, d_rel=5.5, v_lead=0.0, model_prob=1.0)
assert planner.get_safe_depart_release_hold_lead(0.0) is None
def test_depart_release_hold_survives_stale_stop_then_cancels_for_braking_lead():
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP)
lead_one = make_lead(status=True, d_rel=4.0, v_lead=0.8, a_lead=0.2, model_prob=1.0)
lead_two = make_lead(status=False)
sm = make_sm(
0.0,
desired_accel=0.4,
min_accel=-1.0,
experimental_mode=True,
tracking_lead=True,
lead_one=lead_one,
lead_two=lead_two,
)
sm["carState"].standstill = True
sm["controlsState"].longControlState = LongCtrlState.stopping
sm["modelV2"].action.shouldStop = True
sm["modelV2"].position.x = [0.0] * len(ModelConstants.T_IDXS)
sm["modelV2"].velocity.x = [0.0] * len(ModelConstants.T_IDXS)
sm["modelV2"].acceleration.x = [0.0] * len(ModelConstants.T_IDXS)
planner.lead_depart_release_candidate_elapsed = longitudinal_planner_module.LEAD_DEPART_RELEASE_HOLD_CONFIRM_TIME
planner.lead_depart_release_hold_remaining = 1.0
planner.update(sm, make_toggles())
assert not planner.output_should_stop
assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL
lead_one.vLead = 0.1
lead_one.aLeadK = -0.5
planner.update(sm, make_toggles())
assert planner.lead_depart_release_hold_remaining == 0.0
assert planner.output_should_stop
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 set_model_launch_trajectory(model, *, wait_time: float = 0.6, accel: float = 1.0):
times = np.asarray(ModelConstants.T_IDXS, dtype=float)
moving_time = np.maximum(times - wait_time, 0.0)
model.position.x = (0.5 * accel * moving_time ** 2).tolist()
model.velocity.x = (accel * moving_time).tolist()
model.acceleration.x = np.where(times >= wait_time, accel, 0.0).tolist()
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):
return SimpleNamespace(
taco_tune=False,
classic_model=False,
tinygrad_model=True,
model_version=model_version,
vEgoStopping=0.5,
radar_takeoffs=radar_takeoffs,
)
@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_gm_silverado_admits_credible_far_vision_lead_for_acc_follow():
CP = SimpleNamespace(brand="gm", carFingerprint=GM_CAR.CHEVROLET_SILVERADO)
lead = make_lead(status=True, d_rel=80.0, v_lead=30.0, model_prob=0.90, y_rel=0.2)
assert is_gm_silverado_early_follow_lead(CP, lead, 30.0)
@pytest.mark.parametrize("fingerprint", [GM_CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL, CAR.HONDA_CIVIC])
def test_gm_silverado_early_follow_does_not_apply_to_other_vehicles(fingerprint):
brand = "gm" if fingerprint == GM_CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL else "honda"
CP = SimpleNamespace(brand=brand, carFingerprint=fingerprint)
lead = make_lead(status=True, d_rel=80.0, v_lead=30.0, model_prob=0.99)
assert not is_gm_silverado_early_follow_lead(CP, lead, 30.0)
@pytest.mark.parametrize("kwargs", [
{"radar": True},
{"model_prob": 0.80},
{"y_rel": 1.3},
{"d_rel": 131.0},
])
def test_gm_silverado_early_follow_requires_a_credible_centered_vision_lead(kwargs):
CP = SimpleNamespace(brand="gm", carFingerprint=GM_CAR.CHEVROLET_SILVERADO)
lead_kwargs = {"d_rel": 80.0, "v_lead": 30.0, "model_prob": 0.90}
lead_kwargs.update(kwargs)
lead = make_lead(status=True, **lead_kwargs)
assert not is_gm_silverado_early_follow_lead(CP, lead, 30.0)
def test_silverado_prebrake_floor_is_vehicle_specific():
silverado = SimpleNamespace(brand="gm", carFingerprint=GM_CAR.CHEVROLET_SILVERADO)
honda = SimpleNamespace(brand="honda", carFingerprint=CAR.HONDA_CIVIC)
assert get_follow_prebrake_min_headway(silverado, 1.0) == pytest.approx(1.25)
assert get_follow_prebrake_min_headway(honda, 1.0) == pytest.approx(1.6)
def test_silverado_vision_follow_hold_survives_nonurgent_far_lead_crossover():
v_ego = 32.0
t_follow = 1.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=70.0, v_lead=31.2, a_lead=-0.02, radar=False, model_prob=1.0, y_rel=0.1)
lead_two = make_lead(status=False)
assert planner.mpc.get_vision_follow_cruise_hold(
"lead0", lead_one, lead_two, 101.0, 200.0, 100.0, v_ego, t_follow, True,
) is None
assert planner.mpc.get_vision_follow_cruise_hold(
"lead0", lead_one, lead_two, 101.0, 200.0, 100.0, v_ego, t_follow, True,
early_follow=True,
) == "lead0"
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
def test_acc_mode_uses_far_near_stopped_radar_lead_before_tracking(model_version):
v_ego = 24.6
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=False,
lead_one=make_lead(status=True, d_rel=116.0, v_lead=0.0, a_lead=-0.2, radar=True, model_prob=0.85),
)
sm["starpilotPlan"].vCruise = v_ego + 6.0
planner.update(sm, make_toggles(model_version))
assert planner.raw_close_lead_needs_control(sm["radarState"].leadOne, v_ego)
assert planner.output_a_target < -0.1
def test_cruise_accel_cap_does_not_manufacture_braking_after_set_speed_drop_with_lead():
v_ego = 20.115
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead_one = make_lead(
status=True,
d_rel=68.95,
v_lead=18.74,
a_lead=-0.32,
radar=True,
model_prob=0.991,
y_rel=-0.15,
)
lead_two = make_lead(
status=True,
d_rel=68.95,
v_lead=18.74,
a_lead=-0.32,
radar=True,
model_prob=0.993,
y_rel=-0.15,
)
sm = make_sm(
v_ego,
desired_accel=-0.05,
min_accel=-1.0,
experimental_mode=True,
tracking_lead=True,
lead_one=lead_one,
lead_two=lead_two,
)
sm["starpilotPlan"].vCruise = 15.646
sm["starpilotPlan"].tFollow = 1.0
planner.update(sm, make_toggles())
assert planner.output_a_target > -1.0
assert planner.output_a_target <= 0.0
def test_cruise_accel_cap_preserves_close_lead_braking_after_set_speed_drop():
v_ego = 20.115
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
sm = make_sm(
v_ego,
desired_accel=-0.05,
min_accel=-1.0,
experimental_mode=True,
tracking_lead=True,
lead_one=make_lead(
status=True,
d_rel=20.0,
v_lead=0.0,
a_lead=-1.0,
radar=True,
model_prob=0.99,
),
)
sm["starpilotPlan"].vCruise = 15.646
sm["starpilotPlan"].tFollow = 1.0
planner.update(sm, make_toggles())
assert planner.output_a_target <= -3.0
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_starts_earlier_for_high_confidence_rav4_approach():
v_ego = 21.4
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=68.3, v_lead=18.3, radar=False, model_prob=0.95)
route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, v_ego, -1.0)
assert route_cap is not None
assert -0.6 < route_cap < -0.2
def test_vision_untracked_slow_lead_cap_catches_near_rav4_lead_before_tracking():
v_ego = 21.3
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=41.1, v_lead=19.2, radar=False, model_prob=0.90)
route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, v_ego, -1.0)
assert route_cap is not None
assert -0.35 < route_cap <= -0.1
def test_vision_untracked_slow_lead_relaxed_entry_requires_centered_lead():
v_ego = 21.4
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
off_path_lead = make_lead(status=True, d_rel=68.3, v_lead=18.3, radar=False, model_prob=0.99, y_rel=1.5)
assert planner.get_vision_untracked_slow_lead_cap(off_path_lead, v_ego, -1.0) 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_hrv_untracked_slow_lead_cap_prepares_more_for_high_speed_stop():
v_ego = 21.8
lead = make_lead(status=True, d_rel=86.5, v_lead=8.3, a_lead=0.0, radar=False, model_prob=0.93, y_rel=-0.14)
civic_planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.HONDA_CIVIC), init_v=v_ego)
hrv_planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.HONDA_HRV_3G), init_v=v_ego)
civic_cap = civic_planner.get_vision_untracked_slow_lead_cap(lead, v_ego, -3.5)
hrv_cap = hrv_planner.get_vision_untracked_slow_lead_cap(lead, v_ego, -3.5)
assert civic_cap is not None
assert hrv_cap is not None
assert hrv_cap < civic_cap - 0.15
assert -1.2 < hrv_cap < -0.8
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_untracked_approach_lift_eases_throttle_without_braking():
v_ego = 30.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(status=True, d_rel=110.0, v_lead=27.0, radar=False, model_prob=0.98)
aggressive_cap = planner.get_vision_untracked_approach_lift_cap(lead, v_ego, 1.2)
standard_cap = planner.get_vision_untracked_approach_lift_cap(lead, v_ego, 1.4)
assert aggressive_cap is not None
assert standard_cap is not None
assert 0.0 <= standard_cap <= aggressive_cap < 0.22
@pytest.mark.parametrize("lead", [
make_lead(status=True, d_rel=110.0, v_lead=27.0, radar=True, model_prob=0.98),
make_lead(status=True, d_rel=110.0, v_lead=27.0, radar=False, model_prob=0.90),
make_lead(status=True, d_rel=110.0, v_lead=27.0, radar=False, model_prob=0.98, y_rel=1.5),
make_lead(status=True, d_rel=110.0, v_lead=30.0, radar=False, model_prob=0.98),
])
def test_vision_untracked_approach_lift_ignores_unqualified_leads(lead):
v_ego = 30.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
assert planner.get_vision_untracked_approach_lift_cap(lead, v_ego, 1.4) is None
def test_vision_untracked_approach_lift_is_rate_limited_and_held():
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=25.0, init_a=0.5)
now = 0.0
cap = None
for _ in range(6):
now += planner.dt
cap = planner.update_vision_untracked_approach_lift_cap(0.0, 0.5, 0.5, now, True)
assert cap is not None
assert 0.45 < cap < 0.5
held_cap = planner.update_vision_untracked_approach_lift_cap(None, 0.5, 0.5, now + planner.dt, True)
assert held_cap is not None
assert held_cap < cap
def test_vision_untracked_approach_lift_releases_after_hold_or_tracking():
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=25.0, init_a=0.5)
now = 0.0
for _ in range(6):
now += planner.dt
planner.update_vision_untracked_approach_lift_cap(0.0, 0.5, 0.5, now, True)
previous_cap = planner.untracked_vision_approach_lift_cap
now += planner.dt
releasing_cap = planner.update_vision_untracked_approach_lift_cap(None, 0.5, 0.5, now, False)
assert releasing_cap is not None
assert releasing_cap > previous_cap
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
def test_model_launch_accel_skips_hesitant_start_of_trajectory():
model_v = np.maximum(T_IDXS_MPC - 0.6, 0.0)
model_a = np.where(T_IDXS_MPC >= 0.6, 1.0, 0.0)
launch_accel = LongitudinalPlanner.get_model_launch_accel(model_v, model_a, action_t=0.2, v_ego=0.0)
assert launch_accel is not None
assert launch_accel >= 0.8
def test_green_light_model_launch_boosts_no_lead_experimental_takeoff():
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=True)
sm["carState"].standstill = True
sm["controlsState"].longControlState = LongCtrlState.stopping
set_model_launch_trajectory(sm["modelV2"])
planner.update(sm, make_toggles())
assert not planner.output_should_stop
assert planner.output_a_target >= 0.8
def test_green_light_model_launch_survives_cem_switch_back_to_chill():
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=0.0)
sm_red = make_sm(0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=True)
sm_red["carState"].standstill = True
sm_red["controlsState"].longControlState = LongCtrlState.stopping
sm_red["modelV2"].action.shouldStop = True
sm_red["starpilotPlan"].redLight = True
planner.update(sm_red, make_toggles())
sm_green = make_sm(0.0, desired_accel=0.0, min_accel=-0.5, experimental_mode=False)
sm_green["carState"].standstill = True
sm_green["controlsState"].longControlState = LongCtrlState.stopping
set_model_launch_trajectory(sm_green["modelV2"])
planner.update(sm_green, make_toggles())
assert planner.model_launch_stop_seen
assert not planner.output_should_stop
assert planner.output_a_target >= 0.8
@pytest.mark.parametrize(("veto", "brake_pressed"), [("redLight", False), ("forcingStop", False), (None, True)])
def test_green_light_model_launch_respects_stop_and_driver_vetoes(veto, brake_pressed):
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=True)
sm["carState"].standstill = True
sm["carState"].brakePressed = brake_pressed
sm["controlsState"].longControlState = LongCtrlState.stopping
if veto is not None:
setattr(sm["starpilotPlan"], veto, True)
set_model_launch_trajectory(sm["modelV2"])
planner.update(sm, make_toggles())
assert planner.output_a_target < 0.3
def test_model_launch_does_not_override_stationary_lead_guard():
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=True,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=4.0, v_lead=0.0, a_lead=0.0, radar=True, model_prob=1.0),
)
sm["carState"].standstill = True
sm["controlsState"].longControlState = LongCtrlState.stopping
set_model_launch_trajectory(sm["modelV2"])
planner.update(sm, make_toggles())
assert planner.output_should_stop
assert planner.output_a_target <= 0.0
def test_model_launch_boosts_only_after_lead_departure_is_confirmed():
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=0.0)
sm = make_sm(
0.0,
desired_accel=0.1,
min_accel=-0.5,
experimental_mode=True,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=7.0, v_lead=1.5, a_lead=0.8, radar=True, model_prob=1.0),
)
sm["carState"].standstill = True
sm["controlsState"].longControlState = LongCtrlState.stopping
set_model_launch_trajectory(sm["modelV2"])
planner.update(sm, make_toggles())
assert not planner.output_should_stop
assert planner.output_a_target >= 0.8
def test_model_launch_is_cancelled_when_departing_lead_stops_again():
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=0.0)
sm_depart = make_sm(
0.0,
desired_accel=0.1,
min_accel=-0.5,
experimental_mode=True,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=7.0, v_lead=1.5, a_lead=0.8, radar=True, model_prob=1.0),
)
sm_depart["carState"].standstill = True
sm_depart["controlsState"].longControlState = LongCtrlState.stopping
set_model_launch_trajectory(sm_depart["modelV2"])
planner.update(sm_depart, make_toggles())
sm_stop = make_sm(
0.2,
desired_accel=0.1,
min_accel=-0.5,
experimental_mode=True,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=3.8, v_lead=0.0, a_lead=-0.6, radar=True, model_prob=1.0),
)
sm_stop["controlsState"].longControlState = LongCtrlState.pid
set_model_launch_trajectory(sm_stop["modelV2"])
planner.update(sm_stop, make_toggles())
assert planner.output_should_stop
assert planner.output_a_target <= 0.0
@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_stationary_radar_lead_settles_excess_stop_gap_after_confirmation(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.12,
min_accel=-0.5,
experimental_mode=True,
tracking_lead=False,
lead_one=make_lead(status=True, d_rel=6.4, v_lead=0.0, a_lead=0.0, radar=True, model_prob=1.0),
)
sm["carState"].standstill = True
sm["controlsState"].longControlState = LongCtrlState.stopping
sm["modelV2"].action.shouldStop = True
frames = int(round(longitudinal_planner_module.RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME / planner.dt))
for _ in range(max(frames - 1, 1)):
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 == pytest.approx(longitudinal_planner_module.RADAR_STANDSTILL_GAP_SETTLE_ACCEL)
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
def test_stationary_radar_gap_settle_reholds_at_target_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.12,
min_accel=-0.5,
experimental_mode=True,
tracking_lead=False,
lead_one=make_lead(status=True, d_rel=6.4, v_lead=0.0, a_lead=0.0, radar=True, model_prob=1.0),
)
sm["carState"].standstill = True
sm["controlsState"].longControlState = LongCtrlState.stopping
sm["modelV2"].action.shouldStop = True
frames = int(round(longitudinal_planner_module.RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME / planner.dt))
for _ in range(frames):
planner.update(sm, make_toggles(model_version))
assert planner.radar_standstill_gap_settle_active
sm["carState"].vEgo = 0.15
sm["carState"].standstill = False
sm["radarState"].leadOne.dRel = 5.6
planner.update(sm, make_toggles(model_version))
assert not planner.radar_standstill_gap_settle_active
assert planner.output_should_stop
assert planner.output_a_target <= 0.0
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
def test_stationary_gap_settle_never_uses_vision_only_lead(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.12,
min_accel=-0.5,
experimental_mode=True,
tracking_lead=False,
lead_one=make_lead(status=True, d_rel=6.4, v_lead=0.0, a_lead=0.0, radar=False, model_prob=1.0),
)
sm["carState"].standstill = True
sm["controlsState"].longControlState = LongCtrlState.stopping
sm["modelV2"].action.shouldStop = True
frames = int(round(longitudinal_planner_module.RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME / planner.dt)) + 2
for _ in range(frames):
planner.update(sm, make_toggles(model_version))
assert not planner.radar_standstill_gap_settle_active
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_holds_marginal_creep_release(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.8,
min_accel=-0.5,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=4.2, v_lead=0.38, a_lead=0.31, 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
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_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
def test_toyota_sienna_post_departure_restop_blocks_lead_that_stops_again():
CP = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
now_t = time.monotonic()
lead = make_lead(status=True, d_rel=5.0, v_lead=0.10, a_lead=0.0, model_prob=0.99)
cap = get_toyota_sienna_post_departure_restop_cap(
CP, lead, v_ego=1.2, accel_min=-1.0, stop_distance=4.0,
now_t=now_t, departure_latch_until=now_t + 5.0,
)
assert cap is not None
assert cap < -0.18
def test_toyota_sienna_post_departure_restop_allows_confirmed_departure():
CP = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
now_t = time.monotonic()
lead = make_lead(status=True, d_rel=6.5, v_lead=1.0, a_lead=0.45, model_prob=0.99)
cap = get_toyota_sienna_post_departure_restop_cap(
CP, lead, v_ego=0.6, accel_min=-1.0, stop_distance=4.0,
now_t=now_t, departure_latch_until=now_t + 5.0,
)
assert cap is None
def test_toyota_sienna_post_departure_restop_reasserts_should_stop_in_planner():
CP = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
planner = LongitudinalPlanner(CP, init_v=1.2)
sm = make_sm(
1.2,
desired_accel=1.0,
min_accel=-1.0,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=5.0, v_lead=0.10, a_lead=0.0, model_prob=0.99),
)
planner.post_departure_follow_settle_until = time.monotonic() + 5.0
planner.update(sm, make_toggles())
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
def test_route_251682_rav4_confirmed_depart_adds_bounded_accel_assist():
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=0.0)
lead = make_lead(
status=True,
d_rel=7.7,
v_lead=2.0,
a_lead=1.79,
radar=False,
model_prob=1.0,
)
floor = planner.get_lead_depart_accel_floor(lead, v_ego=0.0, model_desired_accel=0.44)
assert 0.52 <= floor <= 0.54
assert floor <= longitudinal_planner_module.LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL
@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(modeld.smooth_value(0.12, prev_action.desiredCurvature, modeld.LAT_SMOOTH_SECONDS))
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(modeld.smooth_value(0.48, prev_action.desiredCurvature, modeld.LAT_SMOOTH_SECONDS))
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
def test_publish_has_lead_includes_second_mpc_lead():
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)}
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=5.0)
planner.output_a_target = 0.0
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 = "lead1"
planner.mpc.solve_time = 0.0
sm = FakeSM(make_sm(5.0, desired_accel=0.0, min_accel=-1.0, experimental_mode=False))
sm["radarState"].leadOne = make_lead(status=False)
sm["radarState"].leadTwo = make_lead(status=True, d_rel=8.0, v_lead=4.0, radar=True)
pm = FakePM()
planner.publish(sm, pm)
assert pm.sent["longitudinalPlan"].longitudinalPlan.hasLead
def test_rav4_tss2_2023_is_the_car_specific_post_departure_tune():
rav4_cp = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023)
other_cp = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2022)
assert is_toyota_rav4_tss2_2023(rav4_cp)
assert not is_toyota_rav4_tss2_2023(other_cp)
@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)
for _ in range(4):
planner.update(sm, toggles)
assert planner.model_allow_throttle
assert planner.allow_throttle
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)
for _ in range(4):
planner.update(sm, toggles)
assert not planner.model_allow_throttle
assert not planner.allow_throttle
planner.update(sm, toggles)
assert planner.model_allow_throttle
assert planner.allow_throttle
def test_allow_throttle_confirmation_filters_route_length_model_pulses():
v_ego = 25.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.6)
toggles = make_toggles()
planner.update(sm, toggles)
# Representative gasPressProb runs from route b85c25a4c6f99d83/0000000c:
# repeated 0.05-0.20 second threshold crossings must not change the coast cap.
for probability, frames in ((0.30, 2), (0.50, 1), (0.32, 3), (0.48, 4), (0.29, 4)):
sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=probability)
for _ in range(frames):
planner.update(sm, toggles)
assert planner.model_allow_throttle
assert planner.allow_throttle
# A sustained model request still applies the physical coast cap.
sm["modelV2"] = make_model(v_ego, desired_accel=0.0, gas_press_prob=0.2)
for _ in range(5):
planner.update(sm, toggles)
assert not planner.model_allow_throttle
assert not 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()
for _ in range(5):
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_experimental_release_state_arms_only_on_falling_edge():
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP)
planner.update_experimental_release_accel_state(True, 10.0, v_ego=23.96)
assert planner.experimental_release_accel_until == 0.0
planner.update_experimental_release_accel_state(False, 10.1, v_ego=23.96)
assert planner.experimental_release_accel_until == pytest.approx(
10.1 + longitudinal_planner_module.EXPERIMENTAL_RELEASE_ACCEL_HOLD_TIME
)
planner.update_experimental_release_accel_state(True, 10.2)
assert planner.experimental_release_accel_until == 0.0
def test_experimental_release_state_holds_longer_at_low_speed():
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP)
planner.update_experimental_release_accel_state(True, 10.0, v_ego=6.75)
planner.update_experimental_release_accel_state(False, 10.1, v_ego=6.75)
assert planner.experimental_release_accel_until == pytest.approx(
10.1 + longitudinal_planner_module.EXPERIMENTAL_RELEASE_ACCEL_LOW_SPEED_HOLD_TIME
)
def test_experimental_release_accel_transition_damps_low_speed_slow_lead_handoff():
v_ego = 6.75
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(status=True, d_rel=11.0, v_lead=6.8, a_lead=0.0, radar=False, model_prob=0.99)
target = planner.get_experimental_release_accel_target(
lead,
v_ego,
1.13,
prev_output_a_target=-0.25,
output_a_target=-0.03,
release_active=True,
)
assert target == pytest.approx(-0.19)
def test_experimental_release_accel_transition_damps_moving_lead_handoff():
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=55.15, v_lead=23.73, a_lead=0.40, radar=True, model_prob=0.997)
target = planner.get_experimental_release_accel_target(
lead,
v_ego,
1.13,
prev_output_a_target=0.03,
output_a_target=0.44,
release_active=True,
)
assert target == pytest.approx(0.09)
def test_experimental_release_accel_transition_does_not_mask_stopped_lead():
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=35.0, v_lead=0.0, a_lead=-1.0, radar=True, model_prob=0.997)
target = planner.get_experimental_release_accel_target(
lead,
v_ego,
1.13,
prev_output_a_target=-0.4,
output_a_target=0.4,
release_active=True,
)
assert target is None
def test_planner_arms_experimental_release_accel_only_on_mode_exit():
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=55.15, v_lead=23.73, a_lead=0.40, radar=True, model_prob=0.997)
sm = make_sm(
v_ego,
desired_accel=0.0,
min_accel=-1.0,
experimental_mode=True,
tracking_lead=True,
lead_one=lead,
)
release_states = []
original = planner.get_experimental_release_accel_target
def record_release_state(self, *args, **kwargs):
release_states.append(bool(args[-1]))
return original(*args, **kwargs)
planner.get_experimental_release_accel_target = types.MethodType(record_release_state, planner)
planner.update(sm, make_toggles())
sm["selfdriveState"].experimentalMode = False
planner.update(sm, make_toggles())
assert release_states == [False, True]
def test_inside_gap_closing_lead_cap_blocks_route_accel_burst():
v_ego = 17.1
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(status=True, d_rel=26.3, v_lead=16.5, a_lead=0.0, radar=True, model_prob=1.0)
cap = planner.get_inside_gap_closing_lead_accel_cap(lead, v_ego, -1.0, 1.25)
assert cap is not None
assert cap == pytest.approx(0.0)
def test_inside_gap_closing_lead_cap_strengthens_with_route_closure():
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=23.3, v_lead=17.3, a_lead=0.0, radar=True, model_prob=1.0)
cap = planner.get_inside_gap_closing_lead_accel_cap(lead, v_ego, -1.0, 1.25)
assert cap is not None
assert -0.7 <= cap <= -0.5
@pytest.mark.parametrize("lead", [
make_lead(status=True, d_rel=36.0, v_lead=16.5, radar=True, model_prob=1.0),
make_lead(status=True, d_rel=26.3, v_lead=17.8, radar=True, model_prob=1.0),
make_lead(status=True, d_rel=26.3, v_lead=16.5, radar=False, model_prob=0.8),
make_lead(status=True, d_rel=26.3, v_lead=16.5, radar=True, model_prob=1.0, y_rel=2.0),
])
def test_inside_gap_closing_lead_cap_ignores_normal_or_ambiguous_follow(lead):
v_ego = 17.1
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
assert planner.get_inside_gap_closing_lead_accel_cap(lead, v_ego, -1.0, 1.25) is None
def test_inside_gap_closing_lead_cap_does_not_touch_standstill_departure():
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=0.0)
lead = make_lead(status=True, d_rel=5.0, v_lead=1.0, a_lead=0.5, radar=True, model_prob=1.0)
assert planner.get_inside_gap_closing_lead_accel_cap(lead, 0.0, -1.0, 1.25) is None
def test_rolling_departure_settle_latch_stays_active_through_headway_hysteresis():
v_ego = 10.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
departing_lead = make_lead(
status=True, d_rel=13.4, v_lead=10.3,
a_lead=0.3, radar=True, model_prob=1.0, y_rel=0.0,
)
settling_lead = make_lead(
status=True, d_rel=13.2, v_lead=10.2,
a_lead=0.1, radar=True, model_prob=1.0, y_rel=0.0,
)
assert planner.post_departure_follow_settle_active(departing_lead, v_ego, 1.25)
assert planner.post_departure_follow_settle_active(settling_lead, v_ego, 1.25)
def test_rolling_departure_settle_latch_supports_confident_vision_lead():
v_ego = 10.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(
status=True, d_rel=13.4, v_lead=10.3,
a_lead=0.3, radar=False, model_prob=0.95, y_rel=0.0,
)
assert planner.post_departure_follow_settle_active(lead, v_ego, 1.25)
def test_rolling_departure_settle_latch_clears_for_stopped_lead():
v_ego = 10.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
departing_lead = make_lead(
status=True, d_rel=13.4, v_lead=10.3,
a_lead=0.3, radar=True, model_prob=1.0, y_rel=0.0,
)
stopped_lead = make_lead(
status=True, d_rel=12.0, v_lead=0.0,
a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.0,
)
assert planner.post_departure_follow_settle_active(departing_lead, v_ego, 1.25)
assert not planner.post_departure_follow_settle_active(stopped_lead, v_ego, 1.25)
assert planner.post_departure_follow_settle_until == 0.0
@pytest.mark.parametrize("v_ego,d_rel,v_lead,a_lead", [
(10.0, 14.0, 10.5, -0.2),
(10.0, 12.5, 11.0, 0.5),
(20.0, 29.0, 20.5, 0.5),
])
def test_rolling_departure_settle_latch_does_not_arm_without_safe_departure(v_ego, d_rel, v_lead, a_lead):
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(
status=True, d_rel=d_rel, v_lead=v_lead,
a_lead=a_lead, radar=True, model_prob=1.0, y_rel=0.0,
)
assert not planner.post_departure_follow_settle_active(lead, v_ego, 1.25)
assert planner.post_departure_follow_settle_until == 0.0
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_near_duplicate_vision_source_hysteresis_applies_at_tesla_city_speed():
v_ego = 11.5
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead_one = make_lead(status=True, d_rel=22.0, v_lead=10.8, a_lead=-0.04, radar=False, model_prob=1.0)
lead_two = make_lead(status=True, d_rel=22.1, v_lead=10.82, a_lead=-0.03, radar=False, model_prob=1.0)
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_vision_source_hysteresis_holds_through_low_speed_stop_approach():
v_ego = 5.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=9.8, v_lead=3.0, a_lead=-0.02, radar=False, model_prob=1.0)
lead_two = make_lead(status=True, d_rel=9.9, v_lead=3.02, a_lead=-0.01, radar=False, model_prob=1.0)
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_applies_to_radarless_lead_below_highway_speed():
v_ego = 10.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(status=True, d_rel=18.0, v_lead=9.9, a_lead=-0.02, radar=False, 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_holds_pullaway_lead_longer_near_target_gap():
v_ego = 15.0
t_follow = 1.45
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead_matched = make_lead(status=True, d_rel=22.0, v_lead=15.0, a_lead=0.02, radar=True, model_prob=1.0)
lead_pullaway = make_lead(status=True, d_rel=22.0, v_lead=16.4, a_lead=0.02, radar=True, model_prob=1.0)
matched_hysteresis = planner.mpc.get_stable_follow_cruise_hysteresis(lead_matched, v_ego, t_follow)
pullaway_hysteresis = planner.mpc.get_stable_follow_cruise_hysteresis(lead_pullaway, v_ego, t_follow)
assert pullaway_hysteresis > matched_hysteresis
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_vision_follow_cruise_hold_keeps_high_confidence_matched_lead_through_small_crossover():
v_ego = 22.5
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.0, v_lead=22.0, a_lead=-0.02, radar=False, model_prob=0.99)
lead_two = make_lead(status=False)
sticky = planner.mpc.get_vision_follow_cruise_hold(
"lead0",
lead_one,
lead_two,
101.0,
200.0,
100.0,
v_ego,
t_follow,
True,
)
assert sticky == "lead0"
@pytest.mark.parametrize("tracking_lead, radar, model_prob, cruise_obstacle", [
(False, False, 0.99, 100.0),
(True, True, 1.0, 100.0),
(True, False, 0.80, 100.0),
(True, False, 0.99, 98.0),
])
def test_vision_follow_cruise_hold_skips_nonmatching_or_clear_cruise_cases(
tracking_lead, radar, model_prob, cruise_obstacle,
):
v_ego = 22.5
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.0, v_lead=22.0, a_lead=-0.02, radar=radar, model_prob=model_prob)
lead_two = make_lead(status=False)
sticky = planner.mpc.get_vision_follow_cruise_hold(
"lead0",
lead_one,
lead_two,
101.0,
200.0,
cruise_obstacle,
v_ego,
t_follow,
tracking_lead,
)
assert sticky is None
@pytest.mark.parametrize("prev_source, lead_accel", [
("cruise", -0.02),
("lead0", -0.60),
])
def test_vision_follow_cruise_hold_never_delays_restrictive_transition(prev_source, lead_accel):
v_ego = 22.5
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.0, v_lead=22.0, a_lead=lead_accel, radar=False, model_prob=0.99)
lead_two = make_lead(status=False)
sticky = planner.mpc.get_vision_follow_cruise_hold(
prev_source,
lead_one,
lead_two,
101.0,
200.0,
100.0,
v_ego,
t_follow,
True,
)
assert sticky is None
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
Reference in New Issue View Git Blame Copy Permalink