IQ.Pilot Release Commit @ bec7652

iqpilot/selfdrive/controls/tests/test_drive_helpers.py

@@ -0,0 +1,20 @@
+import pytest
+
+from iqpilot.selfdrive.controls.lib.drive_helpers import DEFAULT_STOPPING_SPEED, should_stop
+
+
+class TestShouldStop:
+  @pytest.mark.parametrize("v_ego, expected", [
+    (DEFAULT_STOPPING_SPEED - 0.01, True),
+    (DEFAULT_STOPPING_SPEED, False),
+  ])
+  def test_upstream_default(self, v_ego, expected):
+    assert should_stop(v_ego, -0.1) == expected
+
+  @pytest.mark.parametrize("stopping_speed", [0.55 / 3.6, 1.5 / 3.6])
+  def test_car_override(self, stopping_speed):
+    assert should_stop(stopping_speed - 0.01, -0.1, stopping_speed)
+    assert not should_stop(stopping_speed, -0.1, stopping_speed)
+
+  def test_requires_deceleration(self):
+    assert not should_stop(0.0, 0.1, 1.0)

iqpilot/selfdrive/controls/tests/test_following_distance.py

@@ -0,0 +1,46 @@
+import pytest
+import itertools
+from parameterized import parameterized_class
+
+from iqpilot.cereal import log
+
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
+from iqpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
+
+
+def desired_follow_distance(v_ego, v_lead, t_follow=None):
+  if t_follow is None:
+    t_follow = get_T_FOLLOW()
+  return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
+
+def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personality=0):
+  man = Maneuver(
+    '',
+    duration=t_end,
+    initial_speed=float(v_lead),
+    lead_relevancy=True,
+    initial_distance_lead=100,
+    speed_lead_values=[v_lead],
+    breakpoints=[0.],
+    e2e=e2e,
+    personality=personality,
+  )
+  valid, output = man.evaluate()
+  assert valid
+  return output[-1,2] - output[-1,1]
+
+
+@parameterized_class(("e2e", "personality", "speed"), itertools.product(
+                      [True, False], # e2e
+                      [log.LongitudinalPersonality.relaxed, # personality
+                       log.LongitudinalPersonality.standard,
+                       log.LongitudinalPersonality.aggressive],
+                      [0,10,35])) # speed
+class TestFollowingDistance:
+  def test_following_distance(self):
+    v_lead = float(self.speed)
+    simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e, personality=self.personality)
+    correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
+    err_ratio = 0.2 if self.e2e else 0.1
+    abs_err_margin = 0.5 if v_lead > 0.0 else 1.15
+    assert simulation_steady_state == pytest.approx(correct_steady_state, abs=err_ratio * correct_steady_state + abs_err_margin)

iqpilot/selfdrive/controls/tests/test_latcontrol.py

@@ -0,0 +1,106 @@
+from parameterized import parameterized
+
+from iqpilot.cereal import car, log
+from iqdbc.car.car_helpers import interfaces
+from iqdbc.car.honda.values import CAR as HONDA
+from iqdbc.car.toyota.values import CAR as TOYOTA
+from iqdbc.car.nissan.values import CAR as NISSAN
+from iqdbc.car.gm.values import CAR as GM
+from iqdbc.car.vehicle_model import VehicleModel
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.car.helpers import convert_to_capnp
+from iqpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
+from iqpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from iqpilot.selfdrive.controls.lib.latcontrol_torque_pq import LatControlTorquePQ
+import iqpilot.selfdrive.controls.lib.latcontrol_torque_pq as latcontrol_torque_pq
+from iqpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+from iqpilot.selfdrive.locationd.helpers import Pose
+from iqpilot.common.mock.generators import generate_deviceMotion
+from iqpilot.selfdrive.car import interfaces as iqpilot_interfaces
+
+
+class TestLatControl:
+
+  @staticmethod
+  def build_pq_controller():
+    car_name = TOYOTA.TOYOTA_RAV4
+    CarInterface = interfaces[car_name]
+    CP = CarInterface.get_non_essential_params(car_name)
+    CP_IQ = CarInterface.get_non_essential_params_iq(CP, car_name)
+    CI = CarInterface(CP, CP_IQ)
+    iqpilot_interfaces.apply_iq_car_config(CI)
+    return CP, LatControlTorquePQ(CP.as_reader(), convert_to_capnp(CP_IQ).as_reader(), CI, DT_CTRL)
+
+  @parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque),
+                         (NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_EUV, LatControlTorque)])
+  def test_saturation(self, car_name, controller):
+    CarInterface = interfaces[car_name]
+    CP = CarInterface.get_non_essential_params(car_name)
+    CP_IQ = CarInterface.get_non_essential_params_iq(CP, car_name)
+    CI = CarInterface(CP, CP_IQ)
+    iqpilot_interfaces.apply_iq_car_config(CI)
+    CP_IQ = convert_to_capnp(CP_IQ)
+    VM = VehicleModel(CP)
+
+    controller = controller(CP.as_reader(), CP_IQ.as_reader(), CI, DT_CTRL)
+
+    CS = car.CarState.new_message()
+    CS.vEgo = 30
+    CS.steeringPressed = False
+
+    params = log.VehicleParameters.new_message()
+
+    lp = generate_deviceMotion()
+    pose = Pose.from_live_pose(lp.deviceMotion)
+
+    # Saturate for curvature limited and controller limited
+    for _ in range(1000):
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True, 0.2)
+    assert lac_log.saturated
+
+    for _ in range(1000):
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False, 0.2)
+    assert not lac_log.saturated
+
+    for _ in range(1000):
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False, 0.2)
+    assert lac_log.saturated
+
+  def test_pq_controller_update(self):
+    CP, controller = self.build_pq_controller()
+    VM = VehicleModel(CP)
+
+    CS = car.CarState.new_message()
+    CS.vEgo = 30
+    params = log.VehicleParameters.new_message()
+    pose = Pose.from_live_pose(generate_deviceMotion().deviceMotion)
+
+    _, _, lac_log = controller.update(True, CS, VM, params, False, 0.001, pose, False, 0.2)
+    assert lac_log.active
+
+  def test_pq_controller_inactive_lookahead_and_slew_reset(self):
+    CP, controller = self.build_pq_controller()
+    controller.curvature_lookahead_enabled = True
+    controller.lateral_acceleration_slew_limiter.enabled = True
+    VM = VehicleModel(CP)
+    CS = car.CarState.new_message(vEgo=30)
+    params = log.VehicleParameters.new_message()
+    pose = Pose.from_live_pose(generate_deviceMotion().deviceMotion)
+
+    torque, angle, lac_log = controller.update(False, CS, VM, params, False, 0.001, pose, False, 0.2, lookahead_curvature=0.002)
+    assert torque == 0.0
+    assert angle == 0.0
+    assert not lac_log.active
+    assert controller.lateral_acceleration_slew_limiter.a_lim == 1.8
+
+  def test_pq_live_torque_update_freeze_and_unfreeze(self, monkeypatch):
+    _, controller = self.build_pq_controller()
+    initial = (controller.torque_params.latAccelFactor, controller.torque_params.latAccelOffset, controller.torque_params.friction)
+    controller.update_live_torque_params(3.0, 0.2, 0.4)
+    assert (controller.torque_params.latAccelFactor, controller.torque_params.latAccelOffset, controller.torque_params.friction) == initial
+
+    monkeypatch.setattr(latcontrol_torque_pq, "FREEZE_LIVE_TORQUE_PARAMS", False)
+    controller.update_live_torque_params(3.0, 0.2, 0.4)
+    assert controller.torque_params.latAccelFactor == 3.0
+    assert abs(controller.torque_params.latAccelOffset - 0.2) < 1e-6
+    assert abs(controller.torque_params.friction - 0.4) < 1e-6

iqpilot/selfdrive/controls/tests/test_latcontrol_torque_buffer.py

@@ -0,0 +1,47 @@
+from parameterized import parameterized
+
+from iqpilot.cereal import car, log
+from iqdbc.car.car_helpers import interfaces
+from iqdbc.car.toyota.values import CAR as TOYOTA
+from iqdbc.car.vehicle_model import VehicleModel
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque, LAT_ACCEL_REQUEST_BUFFER_SECONDS
+
+from iqpilot.selfdrive.car.helpers import convert_to_capnp
+from iqpilot.selfdrive.locationd.helpers import Pose
+from iqpilot.common.mock.generators import generate_deviceMotion
+from iqpilot.selfdrive.car import interfaces as iqpilot_interfaces
+
+def get_controller(car_name):
+  CarInterface = interfaces[car_name]
+  CP = CarInterface.get_non_essential_params(car_name)
+  CP_IQ = CarInterface.get_non_essential_params_iq(CP, car_name)
+  CI = CarInterface(CP, CP_IQ)
+  iqpilot_interfaces.apply_iq_car_config(CI)
+  CP_IQ = convert_to_capnp(CP_IQ)
+  VM = VehicleModel(CP)
+  controller = LatControlTorque(CP.as_reader(), CP_IQ.as_reader(), CI, DT_CTRL)
+  return controller, VM
+
+class TestLatControlTorqueBuffer:
+
+  @parameterized.expand([(TOYOTA.TOYOTA_COROLLA_TSS2,)])
+  def test_request_buffer_consistency(self, car_name):
+    buffer_steps = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / DT_CTRL)
+    controller, VM = get_controller(car_name)
+
+    CS = car.CarState.new_message()
+    CS.vEgo = 30
+    CS.steeringPressed = False
+    params = log.VehicleParameters.new_message()
+
+    lp = generate_deviceMotion()
+    pose = Pose.from_live_pose(lp.deviceMotion)
+
+    for _ in range(buffer_steps):
+      controller.update(True, CS, VM, params, False, 0.001, pose, False, 0.2)
+    assert all(val != 0 for val in controller.lat_accel_request_buffer)
+
+    for _ in range(buffer_steps):
+      controller.update(False, CS, VM, params, False, 0.0, pose, False, 0.2)
+    assert all(val == 0 for val in controller.lat_accel_request_buffer)

iqpilot/selfdrive/controls/tests/test_lateral_mpc.py

@@ -0,0 +1,85 @@
+import pytest
+import numpy as np
+from iqpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
+from iqpilot.selfdrive.controls.lib.drive_helpers import CAR_ROTATION_RADIUS
+from iqpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
+
+
+def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0.,
+            lane_width=3.6, poly_shift=0.):
+
+  if lat_mpc is None:
+    lat_mpc = LateralMpc()
+  lat_mpc.set_weights(1., .1, 0.0, .05, 800)
+
+  y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
+  heading_pts = np.zeros(LAT_MPC_N + 1)
+  curv_rate_pts = np.zeros(LAT_MPC_N + 1)
+
+  x0 = np.array([x_init, y_init, psi_init, curvature_init])
+  p = np.column_stack([v_ref * np.ones(LAT_MPC_N + 1),
+                      CAR_ROTATION_RADIUS * np.ones(LAT_MPC_N + 1)])
+
+  # converge in no more than 10 iterations
+  for _ in range(10):
+    lat_mpc.run(x0, p,
+                y_pts, heading_pts, curv_rate_pts)
+  return lat_mpc.x_sol
+
+
+class TestLateralMpc:
+
+  def _assert_null(self, sol, curvature=1e-6):
+    for i in range(len(sol)):
+      assert sol[0,i,1] == pytest.approx(0, abs=curvature)
+      assert sol[0,i,2] == pytest.approx(0, abs=curvature)
+      assert sol[0,i,3] == pytest.approx(0, abs=curvature)
+
+  def _assert_simmetry(self, sol, curvature=1e-6):
+    for i in range(len(sol)):
+      assert sol[0,i,1] == pytest.approx(-sol[1,i,1], abs=curvature)
+      assert sol[0,i,2] == pytest.approx(-sol[1,i,2], abs=curvature)
+      assert sol[0,i,3] == pytest.approx(-sol[1,i,3], abs=curvature)
+      assert sol[0,i,0] == pytest.approx(sol[1,i,0], abs=curvature)
+
+  def test_straight(self):
+    sol = run_mpc()
+    self._assert_null(np.array([sol]))
+
+  def test_y_symmetry(self):
+    sol = []
+    for y_init in [-0.5, 0.5]:
+      sol.append(run_mpc(y_init=y_init))
+    self._assert_simmetry(np.array(sol))
+
+  def test_poly_symmetry(self):
+    sol = []
+    for poly_shift in [-1., 1.]:
+      sol.append(run_mpc(poly_shift=poly_shift))
+    self._assert_simmetry(np.array(sol))
+
+  def test_curvature_symmetry(self):
+    sol = []
+    for curvature_init in [-0.1, 0.1]:
+      sol.append(run_mpc(curvature_init=curvature_init))
+    self._assert_simmetry(np.array(sol))
+
+  def test_psi_symmetry(self):
+    sol = []
+    for psi_init in [-0.1, 0.1]:
+      sol.append(run_mpc(psi_init=psi_init))
+    self._assert_simmetry(np.array(sol))
+
+  def test_no_overshoot(self):
+    y_init = 1.
+    sol = run_mpc(y_init=y_init)
+    for y in list(sol[:,1]):
+      assert y_init >= abs(y)
+
+  def test_switch_convergence(self):
+    lat_mpc = LateralMpc()
+    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=3.0, v_ref=7.0)
+    right_psi_deg = np.degrees(sol[:,2])
+    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-3.0, v_ref=7.0)
+    left_psi_deg = np.degrees(sol[:,2])
+    np.testing.assert_almost_equal(right_psi_deg, -left_psi_deg, decimal=3)

iqpilot/selfdrive/controls/tests/test_leads.py

@@ -0,0 +1,33 @@
+import iqpilot.cereal.messaging as messaging
+import pytest
+
+from iqdbc.car.toyota.values import CAR as TOYOTA
+from iqpilot.selfdrive.test.process_replay import replay_process_with_name
+
+
+class TestLeads:
+  @pytest.mark.linux
+  def test_radar_fault(self):
+    # if there's no radar-related can traffic, radard should either not respond or respond with an error
+    # this is tightly coupled with underlying car radar_interface implementation, but it's a good sanity check
+    def single_iter_pkg():
+      # single iter package, with meaningless cans and empty carState/modelV2
+      msgs = []
+      for _ in range(500):
+        can = messaging.new_message("can", 1)
+        cs = messaging.new_message("carState")
+        cp = messaging.new_message("carParams")
+        msgs.append(can.as_reader())
+        msgs.append(cs.as_reader())
+        msgs.append(cp.as_reader())
+      model = messaging.new_message("modelV2")
+      msgs.append(model.as_reader())
+
+      return msgs
+
+    msgs = [m for _ in range(3) for m in single_iter_pkg()]
+    out = replay_process_with_name("card", msgs, fingerprint=TOYOTA.TOYOTA_COROLLA_TSS2)
+    states = [m for m in out if m.which() == "radarTracks"]
+    failures = [not state.valid for state in states]
+
+    assert len(states) == 0 or all(failures)

iqpilot/selfdrive/controls/tests/test_longcontrol.py

@@ -0,0 +1,72 @@
+from types import SimpleNamespace
+
+from iqpilot.cereal import custom
+from iqpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState, long_control_state_trans
+
+
+class TestLongControlStateTransition:
+
+  def test_stay_stopped(self):
+    CP_IQ = custom.IQCarParams.new_message()
+    active = True
+    current_state = LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=True, brake_pressed=False, cruise_standstill=False)
+    assert next_state == LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=True, cruise_standstill=False)
+    assert next_state == LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=True)
+    assert next_state == LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+    assert next_state == LongCtrlState.pid
+    active = False
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+    assert next_state == LongCtrlState.off
+
+def test_engage():
+  CP_IQ = custom.IQCarParams.new_message()
+  active = True
+  current_state = LongCtrlState.off
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=True, brake_pressed=False, cruise_standstill=False)
+  assert next_state == LongCtrlState.stopping
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=True, cruise_standstill=False)
+  assert next_state == LongCtrlState.stopping
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=True)
+  assert next_state == LongCtrlState.stopping
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+  assert next_state == LongCtrlState.pid
+
+
+def test_gas_override_preserves_negative_accel_command():
+  pid_calls = []
+  control = object.__new__(LongControl)
+  control.CP = SimpleNamespace(stopAccel=-0.55)
+  control.CP_IQ = SimpleNamespace(enableGasInterceptor=False)
+  control.long_control_state = LongCtrlState.pid
+  control.pid = SimpleNamespace(
+    update=lambda error, **kwargs: pid_calls.append((error, kwargs)) or -0.5,
+    reset=lambda: None,
+  )
+  control.last_output_accel = -0.4
+  control.stopping_decel_rate = 1.0
+  control.smooth = SimpleNamespace(enabled=False, update=lambda: None, reset=lambda: None)
+  car_state = SimpleNamespace(
+    vEgo=15.0,
+    aEgo=0.0,
+    brakePressed=False,
+    standstill=False,
+    cruiseState=SimpleNamespace(standstill=False),
+  )
+
+  output = control.update(True, car_state, -0.5, False, (-3.5, 2.0), gas_override=True)
+
+  assert output == -0.5
+  assert pid_calls == [(-0.5, {"speed": 15.0, "feedforward": -0.5, "freeze_integrator": True})]

iqpilot/selfdrive/controls/tests/test_longitudinal_planner.py

@@ -0,0 +1,136 @@
+import numpy as np
+import pytest
+from types import SimpleNamespace
+
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LEAD_T_IDXS_MODEL, T_IDXS
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource
+from iqpilot.selfdrive.controls.lib.longitudinal_planner import J_CRUISE, get_accel_candidates, get_cruise_accel, get_e2e_accel
+
+
+def model_velocity(v_ego, v_future):
+  return np.interp(T_IDXS, [T_IDXS[0], T_IDXS[-1]], [v_ego, v_future])
+
+
+class TestCruiseAccel:
+  @pytest.mark.parametrize("v_cruise,v_ego,a_cruise_prev,direction", [
+    (10.0, 30.0, 0.4, -1.0),
+    (40.0, 30.0, -0.4, 1.0),
+  ])
+  def test_e2e_rate_limit(self, v_cruise, v_ego, a_cruise_prev, direction):
+    dt = 0.05
+    accel, _ = get_cruise_accel(True, v_cruise, v_ego, a_cruise_prev, 0.0, SimpleNamespace(), dt, 0.0, True)
+
+    assert accel == pytest.approx(a_cruise_prev + direction * J_CRUISE * dt)
+
+
+class TestE2eCruiseConvergence:
+  def test_converges_when_model_wants_to_accelerate(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 25.0), 0.1, False) == pytest.approx(0.5)
+
+  def test_scales_down_near_cruise_speed(self):
+    assert get_e2e_accel(28.5, 30.0, model_velocity(28.5, 30.0), 0.0, False) == pytest.approx(0.05)
+
+  def test_preserves_active_model_deceleration(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 25.0), -0.05, False) == pytest.approx(-0.05)
+
+  def test_preserves_future_model_slowdown(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 18.0), 0.1, False) == pytest.approx(0.1)
+
+  @pytest.mark.parametrize("v_ego, v_cruise, should_stop", [
+    (30.0, 30.0, False),
+    (31.0, 30.0, False),
+    (20.0, 30.0, True),
+  ])
+  def test_never_overrides_cruise_or_stop(self, v_ego, v_cruise, should_stop):
+    assert get_e2e_accel(v_ego, v_cruise, model_velocity(v_ego, v_ego + 5.0), -0.2, should_stop) == pytest.approx(-0.2)
+
+
+class TestAccelCandidates:
+  MPC = (-0.2, LongitudinalPlanSource.lead0, True)
+  CRUISE = (0.5, LongitudinalPlanSource.cruise, False)
+  E2E = (0.1, LongitudinalPlanSource.e2e, False)
+
+  def test_e2e_without_lead_frees_model_from_mpc(self):
+    candidates = get_accel_candidates(True, False, self.MPC, self.CRUISE, self.E2E)
+    assert candidates == [self.CRUISE, self.E2E]
+    assert min(candidates, key=lambda c: c[0])[1] == LongitudinalPlanSource.e2e
+    assert not any(should_stop for _, _, should_stop in candidates)
+
+  def test_e2e_with_lead_keeps_mpc_safety_constraint(self):
+    candidates = get_accel_candidates(True, True, self.MPC, self.CRUISE, self.E2E)
+    assert candidates == [self.MPC, self.CRUISE, self.E2E]
+    assert min(candidates, key=lambda c: c[0])[1] == LongitudinalPlanSource.lead0
+    assert any(should_stop for _, _, should_stop in candidates)
+
+  def test_acc_without_lead_keeps_mpc_policy(self):
+    candidates = get_accel_candidates(False, False, self.MPC, self.CRUISE, self.E2E)
+    assert candidates == [self.MPC, self.CRUISE]
+
+
+class TestExperimentalLeadMpc:
+  @staticmethod
+  def mpc(v_ego=20.0):
+    mpc = object.__new__(LongitudinalMpc)
+    mpc.x0 = np.array([0.0, v_ego, 0.0])
+    return mpc
+
+  @staticmethod
+  def model_lead(prob=0.9, x=None, v=None):
+    return SimpleNamespace(
+      prob=prob,
+      x=np.asarray(x if x is not None else [30.0, 66.0, 98.0, 126.0, 150.0, 170.0]),
+      v=np.asarray(v if v is not None else [20.0, 18.0, 16.0, 14.0, 12.0, 10.0]),
+    )
+
+  @staticmethod
+  def radar_lead(status=True, model_prob=0.9, radar=True):
+    return SimpleNamespace(
+      status=status,
+      dRel=28.0,
+      vLead=19.0,
+      aLeadK=-0.5,
+      aLeadTau=1.5,
+      vRel=-1.0,
+      modelProb=model_prob,
+      radar=radar,
+    )
+
+  def test_uses_valid_trajectory_with_radar_anchor(self):
+    lead_xv = self.mpc().process_lead(self.model_lead(), self.radar_lead())
+
+    assert lead_xv[0, 0] == pytest.approx(28.0)
+    assert lead_xv[0, 1] == pytest.approx(19.0)
+    assert lead_xv[-1, 0] == pytest.approx(168.0)
+    assert lead_xv[-1, 1] == pytest.approx(9.0)
+    assert np.all(np.diff(lead_xv[:, 0]) >= 0.0)
+
+  def test_uses_valid_vision_only_trajectory(self):
+    radar_lead = self.radar_lead(radar=False)
+    lead_xv = self.mpc().process_lead(self.model_lead(), radar_lead)
+
+    assert lead_xv[-1, 0] == pytest.approx(168.0)
+    assert lead_xv[-1, 1] == pytest.approx(9.0)
+
+  @pytest.mark.parametrize("model_lead, radar_lead", [
+    (model_lead.__func__(prob=0.5), radar_lead.__func__()),
+    (model_lead.__func__(x=[30.0, 66.0]), radar_lead.__func__()),
+    (model_lead.__func__(v=[20.0, 18.0]), radar_lead.__func__()),
+    (model_lead.__func__(x=[30.0, 66.0, 98.0, np.nan, 150.0, 170.0]), radar_lead.__func__()),
+    (model_lead.__func__(v=[20.0, 18.0, 16.0, np.inf, 12.0, 10.0]), radar_lead.__func__()),
+    (model_lead.__func__(), radar_lead.__func__(model_prob=0.0)),
+    (None, radar_lead.__func__()),
+  ])
+  def test_falls_back_to_radar_extrapolation(self, model_lead, radar_lead):
+    mpc = self.mpc()
+
+    assert np.array_equal(mpc.process_lead(model_lead, radar_lead), mpc.process_lead_legacy(radar_lead))
+
+  def test_prevents_backward_position_trajectory(self):
+    model_lead = self.model_lead(x=[30.0, 40.0, 38.0, 60.0, 80.0, 100.0])
+    lead_xv = self.mpc().process_lead(model_lead, self.radar_lead())
+
+    assert np.all(np.diff(lead_xv[:, 0]) >= 0.0)
+
+  def test_model_time_shape_matches_expected_horizon(self):
+    assert LEAD_T_IDXS_MODEL.shape == (6,)

iqpilot/selfdrive/controls/tests/test_torqued_lat_accel_offset.py

@@ -0,0 +1,70 @@
+import numpy as np
+from iqpilot.cereal import car, messaging
+from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY
+from iqdbc.car import structs
+from iqdbc.car.lateral import get_friction, FRICTION_THRESHOLD
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.locationd.torqued import TorqueEstimator, MIN_BUCKET_POINTS, POINTS_PER_BUCKET, STEER_BUCKET_BOUNDS
+
+np.random.seed(0)
+
+LA_ERR_STD = 1.0
+INPUT_NOISE_STD = 0.08
+V_EGO = 30.0
+
+WARMUP_BUCKET_POINTS = (1.5*MIN_BUCKET_POINTS).astype(int)
+STRAIGHT_ROAD_LA_BOUNDS = (0.02, 0.03)
+
+ROLL_BIAS_DEG = 2.0
+ROLL_COMPENSATION_BIAS = ACCELERATION_DUE_TO_GRAVITY*float(np.sin(np.deg2rad(ROLL_BIAS_DEG)))
+TORQUE_TUNE = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=0.0, friction=0.2)
+TORQUE_TUNE_BIASED = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=-ROLL_COMPENSATION_BIAS, friction=0.2)
+
+def generate_inputs(torque_tune, la_err_std, input_noise_std=None):
+  rng = np.random.default_rng(0)
+  steer_torques = np.concat([rng.uniform(bnd[0], bnd[1], pts) for bnd, pts in zip(STEER_BUCKET_BOUNDS, WARMUP_BUCKET_POINTS, strict=True)])
+  la_errs = rng.normal(scale=la_err_std, size=steer_torques.size)
+  frictions = np.array([get_friction(la_err, 0.0, FRICTION_THRESHOLD, torque_tune) for la_err in la_errs])
+  lat_accels = torque_tune.latAccelFactor*steer_torques + torque_tune.latAccelOffset + frictions
+  if input_noise_std is not None:
+    steer_torques += rng.normal(scale=input_noise_std, size=steer_torques.size)
+    lat_accels += rng.normal(scale=input_noise_std, size=steer_torques.size)
+  return steer_torques, lat_accels
+
+def get_warmed_up_estimator(steer_torques, lat_accels):
+  est = TorqueEstimator(car.CarParams())
+  for steer_torque, lat_accel in zip(steer_torques, lat_accels, strict=True):
+    est.filtered_points.add_point(steer_torque, lat_accel)
+  return est
+
+def simulate_straight_road_msgs(est):
+  carControl = messaging.new_message('carControl').carControl
+  carOutput = messaging.new_message('carOutput').carOutput
+  carState = messaging.new_message('carState').carState
+  deviceMotion = messaging.new_message('deviceMotion').deviceMotion
+  carControl.latActive = True
+  carState.vEgo = V_EGO
+  carState.steeringPressed = False
+  ts = DT_MDL*np.arange(2*POINTS_PER_BUCKET)
+  steer_torques = np.concat((np.linspace(-0.03, -0.02, POINTS_PER_BUCKET), np.linspace(0.02, 0.03, POINTS_PER_BUCKET)))
+  lat_accels = TORQUE_TUNE.latAccelFactor * steer_torques
+  for t, steer_torque, lat_accel in zip(ts, steer_torques, lat_accels, strict=True):
+    carOutput.actuatorsOutput.torque = float(-steer_torque)
+    deviceMotion.orientationNED.x = float(np.deg2rad(ROLL_BIAS_DEG))
+    deviceMotion.angularVelocityDevice.z = float(lat_accel / V_EGO)
+    for which, msg in (('carControl', carControl), ('carOutput', carOutput), ('carState', carState), ('deviceMotion', deviceMotion)):
+      est.handle_log(t, which, msg)
+
+def test_estimated_offset():
+  steer_torques, lat_accels = generate_inputs(TORQUE_TUNE_BIASED, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD)
+  est = get_warmed_up_estimator(steer_torques, lat_accels)
+  msg = est.get_msg()
+  # TODO add lataccelfactor and friction check when we have more accurate estimates
+  assert abs(msg.lateralTorqueParameters.latAccelOffsetRaw - TORQUE_TUNE_BIASED.latAccelOffset) < 0.1
+
+def test_straight_road_roll_bias():
+  steer_torques, lat_accels = generate_inputs(TORQUE_TUNE, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD)
+  est = get_warmed_up_estimator(steer_torques, lat_accels)
+  simulate_straight_road_msgs(est)
+  msg = est.get_msg()
+  assert (msg.lateralTorqueParameters.latAccelOffsetRaw < -0.05) and np.isfinite(msg.lateralTorqueParameters.latAccelOffsetRaw)