Longitudinal tests: clean up (#26708)

* refactor test_cruise_speed.py

* clean up long tests

* and here

selfdrive/controls/tests/test_cruise_speed.py

@@ -6,14 +6,13 @@ import unittest
 from selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_ENABLE_MIN, IMPERIAL_INCREMENT
 from cereal import car
 from common.conversions import Conversions as CV
-from common.params import Params
 from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 
 ButtonEvent = car.CarState.ButtonEvent
 ButtonType = car.CarState.ButtonEvent.Type
 
 
-def run_cruise_simulation(cruise, t_end=20.):
+def run_cruise_simulation(cruise, e2e, t_end=20.):
   man = Maneuver(
     '',
     duration=t_end,
@@ -23,6 +22,7 @@ def run_cruise_simulation(cruise, t_end=20.):
     cruise_values=[cruise],
     prob_lead_values=[0.0],
     breakpoints=[0.],
+    e2e=e2e,
   )
   valid, output = man.evaluate()
   assert valid
@@ -31,14 +31,12 @@ def run_cruise_simulation(cruise, t_end=20.):
 
 class TestCruiseSpeed(unittest.TestCase):
   def test_cruise_speed(self):
-    params = Params()
     for e2e in [False, True]:
-      params.put_bool("ExperimentalMode", e2e)
       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)
+        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')
 
 

selfdrive/controls/tests/test_following_distance.py

@@ -1,12 +1,11 @@
 #!/usr/bin/env python3
 import unittest
 import numpy as np
-from common.params import Params
-
 
 from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance
 from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 
+
 def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False):
   man = Maneuver(
     '',
@@ -16,7 +15,7 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False):
     initial_distance_lead=100,
     speed_lead_values=[v_lead],
     breakpoints=[0.],
-    e2e=e2e
+    e2e=e2e,
   )
   valid, output = man.evaluate()
   assert valid
@@ -25,13 +24,11 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False):
 
 class TestFollowingDistance(unittest.TestCase):
   def test_following_distance(self):
-    params = Params()
     for e2e in [False, True]:
-      params.put_bool("ExperimentalMode", e2e)
       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)
+        simulation_steady_state = run_following_distance_simulation(v_lead, e2e=e2e)
         correct_steady_state = desired_follow_distance(v_lead, v_lead)
         err_ratio = 0.2 if e2e else 0.1
         self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(err_ratio * correct_steady_state + .5))

selfdrive/test/longitudinal_maneuvers/maneuver.py

@@ -2,7 +2,7 @@ import numpy as np
 from selfdrive.test.longitudinal_maneuvers.plant import Plant
 
 
-class Maneuver():
+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)
@@ -18,6 +18,7 @@ class Maneuver():
     self.only_radar = kwargs.get("only_radar", False)
     self.ensure_start = kwargs.get("ensure_start", False)
     self.enabled = kwargs.get("enabled", True)
+    self.e2e = kwargs.get("e2e", False)
 
     self.duration = duration
     self.title = title
@@ -30,6 +31,7 @@ class Maneuver():
       enabled=self.enabled,
       only_lead2=self.only_lead2,
       only_radar=self.only_radar,
+      e2e=self.e2e,
     )
 
     valid = True

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -4,7 +4,6 @@ import numpy as np
 
 from cereal import log
 import cereal.messaging as messaging
-from common.params import Params
 from common.realtime import Ratekeeper, DT_MDL
 from selfdrive.controls.lib.longcontrol import LongCtrlState
 from selfdrive.modeld.constants import T_IDXS
@@ -16,9 +15,8 @@ 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):
+               enabled=True, only_lead2=False, only_radar=False, e2e=False):
     self.rate = 1. / DT_MDL
-    self.params = Params()
 
     if not Plant.messaging_initialized:
       Plant.radar = messaging.pub_sock('radarState')
@@ -40,6 +38,7 @@ class Plant:
     self.enabled = enabled
     self.only_lead2 = only_lead2
     self.only_radar = only_radar
+    self.e2e = e2e
 
     self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
     self.ts = 1. / self.rate
@@ -111,7 +110,7 @@ class Plant:
 
     control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off
     control.controlsState.vCruise = float(v_cruise * 3.6)
-    control.controlsState.experimentalMode = self.params.get_bool("ExperimentalMode")
+    control.controlsState.experimentalMode = self.e2e
     car_state.carState.vEgo = float(self.speed)
     car_state.carState.standstill = self.speed < 0.01
 

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 import os
+from parameterized import parameterized
 import unittest
 
 from common.params import Params
@@ -8,124 +9,137 @@ from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 
 
 # TODO: make new FCW tests
-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.],
-  ),
-  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.],
-  ),
-  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],
-  ),
-  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],
-  ),
-  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],
-  ),
-  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],
-  ),
-  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],
-  ),
-  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,
-  ),
-  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,
-  ),
-  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.],
-  ),
-  # controls relies on planner commanding to move for stock-ACC resume spamming
-  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,
-  ),
-  Maneuver(
-    'cruising at 25 m/s while disabled',
-    duration=20.,
-    initial_speed=25.,
-    lead_relevancy=False,
-    enabled=False,
-  ),
-]
+def create_maneuvers(e2e):
+  return [
+    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.],
+      e2e=e2e,
+    ),
+    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.],
+      e2e=e2e,
+    ),
+    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],
+      e2e=e2e,
+    ),
+    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],
+      e2e=e2e,
+    ),
+    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],
+      e2e=e2e,
+    ),
+    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],
+      e2e=e2e,
+    ),
+    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],
+      e2e=e2e,
+    ),
+    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,
+      e2e=e2e,
+    ),
+    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,
+      e2e=e2e,
+    ),
+    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.],
+      e2e=e2e,
+    ),
+    # controls relies on planner commanding to move for stock-ACC resume spamming
+    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,
+      e2e=e2e,
+    ),
+    Maneuver(
+      'cruising at 25 m/s while disabled',
+      duration=20.,
+      initial_speed=25.,
+      lead_relevancy=False,
+      enabled=False,
+      e2e=e2e,
+    ),
+  ]
 
 
 class LongitudinalControl(unittest.TestCase):
@@ -140,29 +154,11 @@ class LongitudinalControl(unittest.TestCase):
     params.put_bool("Passive", bool(os.getenv("PASSIVE")))
     params.put_bool("OpenpilotEnabledToggle", True)
 
-  # hack
-  def test_longitudinal_setup(self):
-    pass
-
-
-def run_maneuver_worker(k):
-  def run(self):
-    params = Params()
-
-    man = maneuvers[k]
-    params.put_bool("ExperimentalMode", True)
-    print(man.title, ' in e2e mode')
-    valid, _ = man.evaluate()
-    self.assertTrue(valid, msg=man.title)
-    params.put_bool("ExperimentalMode", False)
-    print(man.title, ' in acc mode')
-    valid, _ = man.evaluate()
-    self.assertTrue(valid, msg=man.title)
-  return run
-
-for k in range(len(maneuvers)):
-  setattr(LongitudinalControl, f"test_longitudinal_maneuvers_{k+1}",
-          run_maneuver_worker(k))
+  @parameterized.expand([(man,) for e2e in [True, False] for man in create_maneuvers(e2e)])
+  def test_maneuver(self, maneuver):
+    print(maneuver.title, f'in {"e2e" if maneuver.e2e else "acc"} mode')
+    valid, _ = maneuver.evaluate()
+    self.assertTrue(valid, msg=maneuver.title)
 
 
 if __name__ == "__main__":