From 9ea72b655035ee9f96c60c3268049bba6b8b9e1a Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 5 Dec 2022 17:34:57 -0800 Subject: [PATCH] Longitudinal tests: clean up (#26708) * refactor test_cruise_speed.py * clean up long tests * and here --- selfdrive/controls/tests/test_cruise_speed.py | 8 +- .../controls/tests/test_following_distance.py | 9 +- .../test/longitudinal_maneuvers/maneuver.py | 4 +- .../test/longitudinal_maneuvers/plant.py | 7 +- .../test_longitudinal.py | 278 +++++++++--------- 5 files changed, 149 insertions(+), 157 deletions(-) diff --git a/selfdrive/controls/tests/test_cruise_speed.py b/selfdrive/controls/tests/test_cruise_speed.py index a635198ceb..cd1d31cf07 100755 --- a/selfdrive/controls/tests/test_cruise_speed.py +++ b/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') diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 0535caab84..5185867d2d 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/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)) diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 071eaada12..65935f8979 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/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 diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 0d3336134f..8e150d800c 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/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 diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index e859952445..686b35e456 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/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__":