IQ.Pilot Prebuilt Release @ 27f668a

iqpilot/selfdrive/test/longitudinal_maneuvers/.gitignore

@@ -0,0 +1 @@
+out/*

iqpilot/selfdrive/test/longitudinal_maneuvers/maneuver.py

@@ -0,0 +1,88 @@
+import numpy as np
+from iqpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
+
+
+class Maneuver:
+  def __init__(self, title, duration, **kwargs):
+    # Was tempted to make a builder class
+    self.distance_lead = kwargs.get("initial_distance_lead", 200.0)
+    self.speed = kwargs.get("initial_speed", 0.0)
+    self.lead_relevancy = kwargs.get("lead_relevancy", 0)
+
+    self.breakpoints = kwargs.get("breakpoints", [0.0, duration])
+    self.speed_lead_values = kwargs.get("speed_lead_values", [0.0 for i in range(len(self.breakpoints))])
+    self.prob_lead_values = kwargs.get("prob_lead_values", [1.0 for i in range(len(self.breakpoints))])
+    self.prob_throttle_values = kwargs.get("prob_throttle_values", [1.0 for i in range(len(self.breakpoints))])
+    self.cruise_values = kwargs.get("cruise_values", [50.0 for i in range(len(self.breakpoints))])
+    self.pitch_values = kwargs.get("pitch_values", [0.0 for i in range(len(self.breakpoints))])
+
+    self.only_lead2 = kwargs.get("only_lead2", False)
+    self.only_radar = kwargs.get("only_radar", False)
+    self.ensure_start = kwargs.get("ensure_start", False)
+    self.ensure_slowdown = kwargs.get("ensure_slowdown", False)
+    self.enabled = kwargs.get("enabled", True)
+    self.e2e = kwargs.get("e2e", False)
+    self.personality = kwargs.get("personality", 0)
+    self.force_decel = kwargs.get("force_decel", False)
+
+    self.duration = duration
+    self.title = title
+
+  def evaluate(self):
+    plant = Plant(
+      lead_relevancy=self.lead_relevancy,
+      speed=self.speed,
+      distance_lead=self.distance_lead,
+      enabled=self.enabled,
+      only_lead2=self.only_lead2,
+      only_radar=self.only_radar,
+      e2e=self.e2e,
+      personality=self.personality,
+      force_decel=self.force_decel,
+    )
+
+    valid = True
+    started = not self.ensure_start
+    logs = []
+    while plant.current_time < self.duration:
+      speed_lead = np.interp(plant.current_time, self.breakpoints, self.speed_lead_values)
+      prob_lead = np.interp(plant.current_time, self.breakpoints, self.prob_lead_values)
+      cruise = np.interp(plant.current_time, self.breakpoints, self.cruise_values)
+      pitch = np.interp(plant.current_time, self.breakpoints, self.pitch_values)
+      prob_throttle = np.interp(plant.current_time, self.breakpoints, self.prob_throttle_values)
+      log = plant.step(speed_lead, prob_lead, cruise, pitch, prob_throttle)
+
+      d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200.
+      v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0.
+      log['d_rel'] = d_rel
+      log['v_rel'] = v_rel
+      logs.append(np.array([plant.current_time,
+                            log['distance'],
+                            log['distance_lead'],
+                            log['speed'],
+                            speed_lead,
+                            log['acceleration'],
+                            log['d_rel']]))
+
+      if d_rel < .4 and (self.only_radar or prob_lead > 0.5):
+        print("Crashed!!!!")
+        valid = False
+
+      if self.ensure_start and log['v_rel'] > 0 and log['acceleration'] >= 1e-3:
+        started = True
+
+    if self.ensure_slowdown and log['speed'] > 5.5:
+      print('LongitudinalPlanner not slowing down!')
+      valid = False
+
+    if not started:
+      print('LongitudinalPlanner not starting!')
+      valid = False
+
+    if self.force_decel and log['speed'] > 1e-1 and log['acceleration'] > -0.04:
+      print('Not stopping with force decel')
+      valid = False
+
+
+    print("maneuver end", valid)
+    return valid, np.array(logs)

iqpilot/selfdrive/test/longitudinal_maneuvers/plant.py

@@ -0,0 +1,207 @@
+#!/usr/bin/env python3
+import time
+import numpy as np
+
+from iqpilot.cereal import log
+import iqpilot.cereal.messaging as messaging
+from iqpilot.common.realtime import Ratekeeper, DT_MDL
+from iqpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from iqpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+from iqpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
+
+
+class Plant:
+  messaging_initialized = False
+
+  def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0,
+               enabled=True, only_lead2=False, only_radar=False, e2e=False, personality=0, force_decel=False):
+    self.rate = 1. / DT_MDL
+
+    if not Plant.messaging_initialized:
+      Plant.radar = messaging.pub_sock('radarState')
+      Plant.controls_state = messaging.pub_sock('controlsState')
+      Plant.selfdrive_state = messaging.pub_sock('selfdriveState')
+      Plant.car_state = messaging.pub_sock('carState')
+      Plant.plan = messaging.sub_sock('longitudinalPlan')
+      Plant.messaging_initialized = True
+
+    self.v_lead_prev = 0.0
+
+    self.distance = 0.
+    self.speed = speed
+    self.should_stop = False
+    self.acceleration = 0.0
+
+    # lead car
+    self.lead_relevancy = lead_relevancy
+    self.distance_lead = distance_lead
+    self.enabled = enabled
+    self.only_lead2 = only_lead2
+    self.only_radar = only_radar
+    self.e2e = e2e
+    self.personality = personality
+    self.force_decel = force_decel
+
+    self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
+    self.ts = 1. / self.rate
+    time.sleep(0.1)
+    self.sm = messaging.SubMaster(['longitudinalPlan'])
+
+    from iqdbc.car.honda.values import CAR
+    from iqdbc.car.honda.interface import CarInterface
+
+    CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+    CP_IQ = CarInterface.get_non_essential_params_iq(CP, CAR.HONDA_CIVIC)
+    self.planner = LongitudinalPlanner(CP, CP_IQ, init_v=self.speed)
+
+  @property
+  def current_time(self):
+    return float(self.rk.frame) / self.rate
+
+  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle=1.0):
+    # ******** publish a fake model going straight and fake calibration ********
+    # note that this is worst case for MPC, since model will delay long mpc by one time step
+    radar = messaging.new_message('radarState')
+    control = messaging.new_message('controlsState')
+    ss = messaging.new_message('selfdriveState')
+    car_state = messaging.new_message('carState')
+    lp = messaging.new_message('vehicleParameters')
+    car_control = messaging.new_message('carControl')
+    model = messaging.new_message('modelV2')
+    iq_car_state = messaging.new_message('iqCarState')
+    iq_nav_state = messaging.new_message('iqNavState')
+    live_map_data_iq = messaging.new_message('iqLiveData')
+    gps_data = messaging.new_message('gpsLocation')
+    a_lead = (v_lead - self.v_lead_prev)/self.ts
+    self.v_lead_prev = v_lead
+
+    if self.lead_relevancy:
+      d_rel = np.maximum(0., self.distance_lead - self.distance)
+      v_rel = v_lead - self.speed
+      if self.only_radar:
+        status = True
+      elif prob_lead > .5:
+        status = True
+      else:
+        status = False
+    else:
+      d_rel = 200.
+      v_rel = 0.
+      prob_lead = 0.0
+      status = False
+
+    lead = log.RadarState.LeadData.new_message()
+    lead.dRel = float(d_rel)
+    lead.yRel = 0.0
+    lead.vRel = float(v_rel)
+    lead.aRel = float(a_lead - self.acceleration)
+    lead.vLead = float(v_lead)
+    lead.vLeadK = float(v_lead)
+    lead.aLeadK = float(a_lead)
+    # TODO use real radard logic for this
+    lead.aLeadTau = float(_LEAD_ACCEL_TAU)
+    lead.status = status
+    lead.modelProb = float(prob_lead)
+    if not self.only_lead2:
+      radar.radarState.leadOne = lead
+    radar.radarState.leadTwo = lead
+
+    # Simulate model predicting slightly faster speed
+    # this is to ensure lead policy is effective when model
+    # does not predict slowdown in e2e mode
+    position = log.XYZTData.new_message()
+    position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
+    model.modelV2.position = position
+    model.modelV2.action.desiredAcceleration = float(self.acceleration + 0.5)
+    velocity = log.XYZTData.new_message()
+    velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
+    velocity.x[0] = float(self.speed) # always start at current speed
+    model.modelV2.velocity = velocity
+    acceleration = log.XYZTData.new_message()
+    acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
+    model.modelV2.acceleration = acceleration
+    model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)]
+    lead_times = np.asarray(ModelConstants.LEAD_T_IDXS)
+    lead_accel = np.clip(a_lead, -10.0, 5.0)
+    stop_time = -v_lead / lead_accel if lead_accel < 0.0 else np.inf
+    motion_times = np.minimum(lead_times, stop_time)
+    lead_positions = d_rel + v_lead * motion_times + 0.5 * lead_accel * motion_times**2
+    lead_velocities = np.maximum(v_lead + lead_accel * lead_times, 0.0)
+    for lead_prediction in model.modelV2.leadsV3:
+      lead_prediction.prob = float(prob_lead)
+      lead_prediction.x = [float(x) for x in lead_positions]
+      lead_prediction.v = [float(v) for v in lead_velocities]
+
+    control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off
+    ss.selfdriveState.experimentalMode = self.e2e
+    ss.selfdriveState.personality = self.personality
+    control.controlsState.forceDecel = self.force_decel
+    car_state.carState.vEgo = float(self.speed)
+    car_state.carState.standstill = bool(self.speed < 0.01)
+    car_state.carState.vCruise = float(v_cruise * 3.6)
+    car_control.carControl.orientationNED = [0., float(pitch), 0.]
+
+    # ******** get controlsState messages for plotting ***
+    sm = {'radarState': radar.radarState,
+          'carState': car_state.carState,
+          'carControl': car_control.carControl,
+          'controlsState': control.controlsState,
+          'selfdriveState': ss.selfdriveState,
+          'vehicleParameters': lp.vehicleParameters,
+          'modelV2': model.modelV2,
+          'iqCarState': iq_car_state.iqCarState,
+          'iqNavState': iq_nav_state.iqNavState,
+          'iqLiveData': live_map_data_iq.iqLiveData,
+          'gpsLocation': gps_data.gpsLocation}
+    self.planner.update(sm)
+    self.acceleration = self.planner.output_a_target
+    if self.planner.output_should_stop:
+      self.acceleration = min(-0.5, self.acceleration)
+    self.speed = self.speed + self.acceleration * self.ts
+    self.should_stop = self.planner.output_should_stop
+    fcw = self.planner.fcw
+    self.distance_lead = self.distance_lead + v_lead * self.ts
+
+    # ******** run the car ********
+    #print(self.distance, speed)
+    if self.speed <= 0:
+      self.speed = 0
+      self.acceleration = 0
+    self.distance = self.distance + self.speed * self.ts
+
+    # *** radar model ***
+    if self.lead_relevancy:
+      d_rel = np.maximum(0., self.distance_lead - self.distance)
+      v_rel = v_lead - self.speed
+    else:
+      d_rel = 200.
+      v_rel = 0.
+
+    # print at 5hz
+    # if (self.rk.frame % (self.rate // 5)) == 0:
+    #   print("%2.2f sec   %6.2f m  %6.2f m/s  %6.2f m/s2   lead_rel: %6.2f m  %6.2f m/s"
+    #         % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel))
+
+
+    # ******** update prevs ********
+    self.rk.monitor_time()
+
+    return {
+      "distance": self.distance,
+      "speed": self.speed,
+      "acceleration": self.acceleration,
+      "should_stop": self.should_stop,
+      "distance_lead": self.distance_lead,
+      "fcw": fcw,
+    }
+
+# simple engage in standalone mode
+def plant_thread():
+  plant = Plant()
+  while 1:
+    plant.step()
+
+
+if __name__ == "__main__":
+  plant_thread()

iqpilot/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -0,0 +1,191 @@
+import itertools
+from parameterized import parameterized_class
+
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
+from iqpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
+
+
+# TODO: make new FCW tests
+def create_maneuvers(kwargs):
+  maneuvers = [
+    Maneuver(
+      'approach stopped car at 25m/s, initial distance: 120m',
+      duration=20.,
+      initial_speed=25.,
+      lead_relevancy=True,
+      initial_distance_lead=120.,
+      speed_lead_values=[30., 0.],
+      breakpoints=[0., 1.],
+      **kwargs,
+    ),
+    Maneuver(
+      'approach stopped car at 20m/s, initial distance 90m',
+      duration=20.,
+      initial_speed=20.,
+      lead_relevancy=True,
+      initial_distance_lead=90.,
+      speed_lead_values=[20., 0.],
+      breakpoints=[0., 1.],
+      **kwargs,
+    ),
+    Maneuver(
+      'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2',
+      duration=50.,
+      initial_speed=20.,
+      lead_relevancy=True,
+      initial_distance_lead=35.,
+      speed_lead_values=[20., 20., 0.],
+      breakpoints=[0., 15., 35.0],
+      **kwargs,
+    ),
+    Maneuver(
+      'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2',
+      duration=50.,
+      initial_speed=20.,
+      lead_relevancy=True,
+      initial_distance_lead=35.,
+      speed_lead_values=[20., 20., 0.],
+      breakpoints=[0., 15., 25.0],
+      **kwargs,
+    ),
+    Maneuver(
+      'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2',
+      duration=50.,
+      initial_speed=20.,
+      lead_relevancy=True,
+      initial_distance_lead=35.,
+      speed_lead_values=[20., 20., 0.],
+      breakpoints=[0., 15., 21.66],
+      **kwargs,
+    ),
+    Maneuver(
+      'steady state following a car at 20m/s, then lead decel to 0mph at 3+m/s^2',
+      duration=40.,
+      initial_speed=20.,
+      lead_relevancy=True,
+      initial_distance_lead=35.,
+      speed_lead_values=[20., 20., 0.],
+      prob_lead_values=[0., 1., 1.],
+      cruise_values=[20., 20., 20.],
+      breakpoints=[2., 2.01, 8.8],
+      **kwargs,
+    ),
+    Maneuver(
+      "approach stopped car at 20m/s, with prob_lead_values",
+      duration=30.,
+      initial_speed=20.,
+      lead_relevancy=True,
+      initial_distance_lead=120.,
+      speed_lead_values=[0.0, 0., 0.],
+      prob_lead_values=[0.0, 0., 1.],
+      cruise_values=[20., 20., 20.],
+      breakpoints=[0.0, 2., 2.01],
+      **kwargs,
+    ),
+    Maneuver(
+      "approach stopped car at 20m/s, with prob_throttle_values and pitch = -0.1",
+      duration=30.,
+      initial_speed=20.,
+      lead_relevancy=True,
+      initial_distance_lead=120.,
+      speed_lead_values=[0.0, 0., 0.],
+      prob_throttle_values=[1., 0., 0.],
+      cruise_values=[20., 20., 20.],
+      pitch_values=[0., -0.1, -0.1],
+      breakpoints=[0.0, 2., 2.01],
+      **kwargs,
+    ),
+    Maneuver(
+      "approach stopped car at 20m/s, with prob_throttle_values and pitch = +0.1",
+      duration=30.,
+      initial_speed=20.,
+      lead_relevancy=True,
+      initial_distance_lead=120.,
+      speed_lead_values=[0.0, 0., 0.],
+      prob_throttle_values=[1., 0., 0.],
+      cruise_values=[20., 20., 20.],
+      pitch_values=[0., 0.1, 0.1],
+      breakpoints=[0.0, 2., 2.01],
+      **kwargs,
+    ),
+    Maneuver(
+      "approach slower cut-in car at 20m/s",
+      duration=20.,
+      initial_speed=20.,
+      lead_relevancy=True,
+      initial_distance_lead=50.,
+      speed_lead_values=[15., 15.],
+      breakpoints=[1., 11.],
+      only_lead2=True,
+      **kwargs,
+    ),
+    Maneuver(
+      "stay stopped behind radar override lead",
+      duration=20.,
+      initial_speed=0.,
+      lead_relevancy=True,
+      initial_distance_lead=10.,
+      speed_lead_values=[0., 0.],
+      prob_lead_values=[0., 0.],
+      breakpoints=[1., 11.],
+      only_radar=True,
+      **kwargs,
+    ),
+    Maneuver(
+      "NaN recovery",
+      duration=30.,
+      initial_speed=15.,
+      lead_relevancy=True,
+      initial_distance_lead=60.,
+      speed_lead_values=[0., 0., 0.0],
+      breakpoints=[1., 1.01, 11.],
+      cruise_values=[float("nan"), 15., 15.],
+      **kwargs,
+    ),
+    Maneuver(
+      'cruising at 25 m/s while disabled',
+      duration=20.,
+      initial_speed=25.,
+      lead_relevancy=False,
+      enabled=False,
+      **kwargs,
+    ),
+    Maneuver(
+      "slow to 5m/s with allow_throttle = False and pitch = +0.1",
+      duration=30.,
+      initial_speed=20.,
+      lead_relevancy=False,
+      prob_throttle_values=[1., 0., 0.],
+      cruise_values=[20., 20., 20.],
+      pitch_values=[0., 0.1, 0.1],
+      breakpoints=[0.0, 2., 2.01],
+      ensure_slowdown=True,
+      **kwargs,
+    )]
+  if not kwargs['force_decel']:
+    # controls relies on planner commanding to move for stock-ACC resume spamming
+    maneuvers.append(Maneuver(
+      "resume from a stop",
+      duration=20.,
+      initial_speed=0.,
+      lead_relevancy=True,
+      initial_distance_lead=STOP_DISTANCE,
+      speed_lead_values=[0., 0., 2.],
+      breakpoints=[1., 10., 15.],
+      ensure_start=True,
+      **kwargs,
+    ))
+  return maneuvers
+
+
+@parameterized_class(("e2e", "force_decel"), itertools.product([True, False], repeat=2))
+class TestLongitudinalControl:
+  e2e: bool
+  force_decel: bool
+
+  def test_maneuver(self, subtests):
+    for maneuver in create_maneuvers({"e2e": self.e2e, "force_decel": self.force_decel}):
+      with subtests.test(title=maneuver.title, e2e=maneuver.e2e, force_decel=maneuver.force_decel):
+        print(maneuver.title, f'in {"e2e" if maneuver.e2e else "acc"} mode')
+        valid, _ = maneuver.evaluate()
+        assert valid