Fix cruise MPC convergence bug and add test (#22295)

* fix tests

* fix cruise convergence bug

* update refs

* Update selfdrive/controls/tests/test_cruise_speed.py

Co-authored-by: Willem Melching <willem.melching@gmail.com>

* Update selfdrive/controls/tests/test_cruise_speed.py

* add msgh

Co-authored-by: Willem Melching <willem.melching@gmail.com>

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -115,7 +115,7 @@ class LongitudinalMpc():
     self.v_solution = [0.0 for i in range(len(T_IDXS))]
     self.a_solution = [0.0 for i in range(len(T_IDXS))]
     self.j_solution = [0.0 for i in range(len(T_IDXS)-1)]
-    self.yref = np.ones((N+1, 4))
+    self.yref = np.zeros((N+1, 4))
     self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
     self.solver.cost_set(N, "yref", self.yref[N][:3])
     self.T_IDXS = np.array(T_IDXS[:N+1])

selfdrive/controls/tests/test_cruise_speed.py

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+import unittest
+import numpy as np
+from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
+
+def run_cruise_simulation(cruise, t_end=100.):
+  man = Maneuver(
+    '',
+    duration=t_end,
+    initial_speed=float(0.),
+    lead_relevancy=True,
+    initial_distance_lead=100,
+    cruise_values=[cruise],
+    prob_lead_values=[0.0],
+    breakpoints=[0.],
+  )
+  valid, output = man.evaluate()
+  assert valid
+  return output[-1,3]
+
+
+class TestCruiseSpeed(unittest.TestCase):
+  def test_cruise_speed(self):
+    for speed in np.arange(5, 40, 5):
+      print(f'Testing {speed} m/s')
+      cruise_speed = float(speed)
+
+      simulation_steady_state = run_cruise_simulation(cruise_speed)
+      self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {speed} m/s')
+
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/controls/tests/test_following_distance.py

@@ -2,70 +2,29 @@
 import unittest
 import numpy as np
 
-from cereal import log
-import cereal.messaging as messaging
-from selfdrive.config import Conversions as CV
-from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import desired_follow_distance, LeadMpc
-
-
-class FakePubMaster():
-  def send(self, s, data):
-    assert data
-
-
-def run_following_distance_simulation(v_lead, t_end=200.0):
-  dt = 0.2
-  t = 0.
-
-  x_lead = 200.0
-
-  x_ego = 0.0
-  v_ego = v_lead
-  a_ego = 0.0
-
-  mpc = LeadMpc(0)
-
-  first = True
-  while t < t_end:
-
-    # Setup CarState
-    CS = messaging.new_message('carState')
-    CS.carState.vEgo = float(v_ego)
-    CS.carState.aEgo = float(a_ego)
-
-    # Setup model packet
-    radarstate = messaging.new_message('radarState')
-    lead = log.RadarState.LeadData.new_message()
-    lead.modelProb = .75
-    lead.dRel = x_lead - x_ego
-    lead.vLead = v_lead
-    lead.aLeadK = 0.0
-    lead.status = True
-    radarstate.radarState.leadOne = lead
-
-    # Run MPC
-    mpc.set_cur_state(v_ego, a_ego)
-    if first:  # Make sure MPC is converged on first timestep
-      for _ in range(20):
-        mpc.update(CS.carState, radarstate.radarState, 0)
-    mpc.update(CS.carState, radarstate.radarState, 0)
-
-    # Choose slowest of two solutions
-    v_ego, a_ego = float(mpc.v_solution[5]), float(mpc.a_solution[5])
-
-    # Update state
-    x_lead += v_lead * dt
-    x_ego += v_ego * dt
-    t += dt
-    first = False
-
-  return x_lead - x_ego
+from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import desired_follow_distance
+from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
+
+def run_following_distance_simulation(v_lead, t_end=150.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.],
+  )
+  valid, output = man.evaluate()
+  assert valid
+  return output[-1,2] - output[-1,1]
 
 
 class TestFollowingDistance(unittest.TestCase):
   def test_following_distanc(self):
-    for speed_mph in np.linspace(10, 100, num=10):
-      v_lead = float(speed_mph * CV.MPH_TO_MS)
+    for speed in np.arange(0, 40, 5):
+      print(f'Testing {speed} m/s')
+      v_lead = float(speed)
 
       simulation_steady_state = run_following_distance_simulation(v_lead)
       correct_steady_state = desired_follow_distance(v_lead, v_lead)

selfdrive/test/longitudinal_maneuvers/maneuver.py

@@ -9,9 +9,10 @@ class Maneuver():
     self.speed = kwargs.get("initial_speed", 0.0)
     self.lead_relevancy = kwargs.get("lead_relevancy", 0)
 
-    self.speed_lead_breakpoints = kwargs.get("speed_lead_breakpoints", [0.0, duration])
-    self.speed_lead_values = kwargs.get("speed_lead_values", [0.0, 0.0])
-    self.prob_lead_values = kwargs.get("prob_lead_values", [1.0 for i in range(len(self.speed_lead_breakpoints))])
+    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.cruise_values = kwargs.get("cruise_values", [50.0 for i in range(len(self.breakpoints))])
 
     self.only_lead2 = kwargs.get("only_lead2", False)
     self.only_radar = kwargs.get("only_radar", False)
@@ -31,9 +32,10 @@ class Maneuver():
     valid = True
     logs = []
     while plant.current_time() < self.duration:
-      speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
-      prob = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.prob_lead_values)
-      log = plant.step(speed_lead, prob)
+      speed_lead = np.interp(plant.current_time(), self.breakpoints, self.speed_lead_values)
+      prob = np.interp(plant.current_time(), self.breakpoints, self.prob_lead_values)
+      cruise = np.interp(plant.current_time(), self.breakpoints, self.cruise_values)
+      log = plant.step(speed_lead, prob, cruise)
 
       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.
@@ -46,7 +48,7 @@ class Maneuver():
                             speed_lead,
                             log['acceleration']]))
 
-      if d_rel < 1.0:
+      if d_rel < 1.0 and (self.only_radar or prob > 0.5):
         print("Crashed!!!!")
         valid = False
 

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -48,7 +48,7 @@ class Plant():
   def current_time(self):
     return float(self.rk.frame) / self.rate
 
-  def step(self, v_lead=0.0, prob=1.0):
+  def step(self, v_lead=0.0, prob=1.0, v_cruise=50.):
     # ******** 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')
@@ -89,7 +89,7 @@ class Plant():
 
 
     control.controlsState.longControlState = LongCtrlState.pid
-    control.controlsState.vCruise = 130
+    control.controlsState.vCruise = float(v_cruise * 3.6)
     car_state.carState.vEgo = float(self.speed)
 
 

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -15,7 +15,7 @@ maneuvers = [
     lead_relevancy=True,
     initial_distance_lead=120.,
     speed_lead_values=[30., 0.],
-    speed_lead_breakpoints=[0., 1.],
+    breakpoints=[0., 1.],
   ),
   Maneuver(
     'approach stopped car at 20m/s',
@@ -24,7 +24,7 @@ maneuvers = [
     lead_relevancy=True,
     initial_distance_lead=60.,
     speed_lead_values=[20., 0.],
-    speed_lead_breakpoints=[0., 1.],
+    breakpoints=[0., 1.],
   ),
   Maneuver(
     'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2',
@@ -33,7 +33,7 @@ maneuvers = [
     lead_relevancy=True,
     initial_distance_lead=35.,
     speed_lead_values=[20., 20., 0.],
-    speed_lead_breakpoints=[0., 15., 35.0],
+    breakpoints=[0., 15., 35.0],
   ),
   Maneuver(
     'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2',
@@ -42,7 +42,7 @@ maneuvers = [
     lead_relevancy=True,
     initial_distance_lead=35.,
     speed_lead_values=[20., 20., 0.],
-    speed_lead_breakpoints=[0., 15., 25.0],
+    breakpoints=[0., 15., 25.0],
   ),
   Maneuver(
     'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2',
@@ -51,7 +51,7 @@ maneuvers = [
     lead_relevancy=True,
     initial_distance_lead=35.,
     speed_lead_values=[20., 20., 0.],
-    speed_lead_breakpoints=[0., 15., 21.66],
+    breakpoints=[0., 15., 21.66],
   ),
   Maneuver(
     'steady state following a car at 20m/s, then lead decel to 0mph at 5m/s^2',
@@ -60,7 +60,7 @@ maneuvers = [
     lead_relevancy=True,
     initial_distance_lead=35.,
     speed_lead_values=[20., 20., 0.],
-    speed_lead_breakpoints=[0., 15., 19.],
+    breakpoints=[0., 15., 19.],
   ),
   Maneuver(
     "approach stopped car at 20m/s",
@@ -69,7 +69,7 @@ maneuvers = [
     lead_relevancy=True,
     initial_distance_lead=50.,
     speed_lead_values=[0., 0.],
-    speed_lead_breakpoints=[1., 11.],
+    breakpoints=[1., 11.],
   ),
   Maneuver(
     "approach slower cut-in car at 20m/s",
@@ -78,7 +78,7 @@ maneuvers = [
     lead_relevancy=True,
     initial_distance_lead=50.,
     speed_lead_values=[15., 15.],
-    speed_lead_breakpoints=[1., 11.],
+    breakpoints=[1., 11.],
     only_lead2=True,
   ),
   Maneuver(
@@ -89,7 +89,7 @@ maneuvers = [
     initial_distance_lead=10.,
     speed_lead_values=[0., 0.],
     prob_lead_values=[0., 0.],
-    speed_lead_breakpoints=[1., 11.],
+    breakpoints=[1., 11.],
     only_radar=True,
   ),
 ]

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-03585a6e067ff58f1030450efc55cd225dc2ffc3
+1e029954854211b6277f98e6b0c3438fd11e6667