Test and fix cruise speed all personalities (#28658)

* Fix cruise speed in non-standard mode

* Test all personalities

* Test only critical speeds

* 35ms max

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -357,7 +357,7 @@ class LongitudinalMpc:
       v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
                                  v_lower,
                                  v_upper)
-      cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, get_T_FOLLOW())
+      cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, get_T_FOLLOW(personality))
       x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
       self.source = SOURCES[np.argmin(x_obstacles[0])]
 

selfdrive/controls/tests/test_cruise_speed.py

@@ -1,8 +1,10 @@
 #!/usr/bin/env python3
 import numpy as np
-from parameterized import parameterized_class
 import unittest
 
+from parameterized import parameterized_class
+from cereal import log
+from common.params import Params
 from selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT
 from cereal import car
 from common.conversions import Conversions as CV
@@ -31,13 +33,19 @@ def run_cruise_simulation(cruise, e2e, t_end=20.):
 
 class TestCruiseSpeed(unittest.TestCase):
   def test_cruise_speed(self):
-    for e2e in [False, True]:
+    params = Params()
-      for speed in np.arange(5, 40, 5):
+    personalities = [log.LongitudinalPersonality.relaxed,
-        print(f'Testing {speed} m/s')
+                     log.LongitudinalPersonality.standard,
-        cruise_speed = float(speed)
+                     log.LongitudinalPersonality.aggressive]
-
+    for personality in personalities:
-        simulation_steady_state = run_cruise_simulation(cruise_speed, e2e)
+      params.put("LongitudinalPersonality", str(personality))
-        self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {speed} m/s')
+      for e2e in [False, True]:
+        for speed in [5,35]:
+          print(f'Testing {speed} m/s')
+          cruise_speed = float(speed)
+
+          simulation_steady_state = run_cruise_simulation(cruise_speed, e2e)
+          self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {speed} m/s')
 
 
 # TODO: test pcmCruise

selfdrive/controls/tests/test_following_distance.py

@@ -1,8 +1,9 @@
 #!/usr/bin/env python3
 import unittest
-import numpy as np
+from common.params import Params
+from cereal import log
 
-from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance
+from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance, get_T_FOLLOW
 from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 
 
@@ -24,14 +25,20 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False):
 
 class TestFollowingDistance(unittest.TestCase):
   def test_following_distance(self):
-    for e2e in [False, True]:
+    params = Params()
-      for speed in np.arange(0, 40, 5):
+    personalities = [log.LongitudinalPersonality.relaxed,
-        print(f'Testing {speed} m/s')
+                     log.LongitudinalPersonality.standard,
-        v_lead = float(speed)
+                     log.LongitudinalPersonality.aggressive]
-        simulation_steady_state = run_following_distance_simulation(v_lead, e2e=e2e)
+    for personality in personalities:
-        correct_steady_state = desired_follow_distance(v_lead, v_lead)
+      params.put("LongitudinalPersonality", str(personality))
-        err_ratio = 0.2 if e2e else 0.1
+      for e2e in [False, True]:
-        self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(err_ratio * correct_steady_state + .5))
+        for speed in [0,10,35]:
+          print(f'Testing {speed} m/s')
+          v_lead = float(speed)
+          simulation_steady_state = run_following_distance_simulation(v_lead, e2e=e2e)
+          correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(personality))
+          err_ratio = 0.2 if e2e else 0.1
+          self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(err_ratio * correct_steady_state + .5))
 
 
 if __name__ == "__main__":