sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Longitudinal tests: clean up (#26708)

Browse Source 
* refactor test_cruise_speed.py

* clean up long tests

* and here
pull/26712/head
Shane Smiskol 2 years ago committed by GitHub
parent e598438a97
commit 9ea72b6550
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 149 additions and 157 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      selfdrive/controls/tests/test_cruise_speed.py
  2. 9
      selfdrive/controls/tests/test_following_distance.py
  3. 4
      selfdrive/test/longitudinal_maneuvers/maneuver.py
  4. 7
      selfdrive/test/longitudinal_maneuvers/plant.py
  5. 278
      selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

8
selfdrive/controls/tests/test_cruise_speed.py
Unescape View File

@ -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 selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_ENABLE_MIN, IMPERIAL_INCREMENT
from cereal import car from cereal import car
from common.conversions import Conversions as CV from common.conversions import Conversions as CV
from common.params import Params
from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
ButtonEvent = car.CarState.ButtonEvent ButtonEvent = car.CarState.ButtonEvent
ButtonType = car.CarState.ButtonEvent.Type ButtonType = car.CarState.ButtonEvent.Type
def run_cruise_simulation(cruise, t_end=20.): def run_cruise_simulation(cruise, e2e, t_end=20.):
man = Maneuver( man = Maneuver(
'', '',
duration=t_end, duration=t_end,
@ -23,6 +22,7 @@ def run_cruise_simulation(cruise, t_end=20.):
cruise_values=[cruise], cruise_values=[cruise],
prob_lead_values=[0.0], prob_lead_values=[0.0],
breakpoints=[0.], breakpoints=[0.],
e2e=e2e,
) )
valid, output = man.evaluate() valid, output = man.evaluate()
assert valid assert valid
@ -31,14 +31,12 @@ def run_cruise_simulation(cruise, t_end=20.):
class TestCruiseSpeed(unittest.TestCase): class TestCruiseSpeed(unittest.TestCase):
def test_cruise_speed(self): def test_cruise_speed(self):
params = Params()
for e2e in [False, True]: for e2e in [False, True]:
params.put_bool("ExperimentalMode", e2e)
for speed in np.arange(5, 40, 5): for speed in np.arange(5, 40, 5):
print(f'Testing {speed} m/s') print(f'Testing {speed} m/s')
cruise_speed = float(speed) 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') self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {speed} m/s')

9
selfdrive/controls/tests/test_following_distance.py
Unescape View File

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

4
selfdrive/test/longitudinal_maneuvers/maneuver.py
Unescape View File

@ -2,7 +2,7 @@ import numpy as np
from selfdrive.test.longitudinal_maneuvers.plant import Plant from selfdrive.test.longitudinal_maneuvers.plant import Plant
class Maneuver(): class Maneuver:
def __init__(self, title, duration, **kwargs): def __init__(self, title, duration, **kwargs):
# Was tempted to make a builder class # Was tempted to make a builder class
self.distance_lead = kwargs.get("initial_distance_lead", 200.0) 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.only_radar = kwargs.get("only_radar", False)
self.ensure_start = kwargs.get("ensure_start", False) self.ensure_start = kwargs.get("ensure_start", False)
self.enabled = kwargs.get("enabled", True) self.enabled = kwargs.get("enabled", True)
self.e2e = kwargs.get("e2e", False)
self.duration = duration self.duration = duration
self.title = title self.title = title
@ -30,6 +31,7 @@ class Maneuver():
enabled=self.enabled, enabled=self.enabled,
only_lead2=self.only_lead2, only_lead2=self.only_lead2,
only_radar=self.only_radar, only_radar=self.only_radar,
e2e=self.e2e,
) )
valid = True valid = True

7
selfdrive/test/longitudinal_maneuvers/plant.py
Unescape View File

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

278
selfdrive/test/longitudinal_maneuvers/test_longitudinal.py
Unescape View File

@ -1,5 +1,6 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
import os import os
from parameterized import parameterized
import unittest import unittest
from common.params import Params from common.params import Params
@ -8,124 +9,137 @@ from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
# TODO: make new FCW tests # TODO: make new FCW tests
maneuvers = [ def create_maneuvers(e2e):
Maneuver( return [
'approach stopped car at 25m/s, initial distance: 120m', Maneuver(
duration=20., 'approach stopped car at 25m/s, initial distance: 120m',
initial_speed=25., duration=20.,
lead_relevancy=True, initial_speed=25.,
initial_distance_lead=120., lead_relevancy=True,
speed_lead_values=[30., 0.], initial_distance_lead=120.,
breakpoints=[0., 1.], speed_lead_values=[30., 0.],
), breakpoints=[0., 1.],
Maneuver( e2e=e2e,
'approach stopped car at 20m/s, initial distance 90m', ),
duration=20., Maneuver(
initial_speed=20., 'approach stopped car at 20m/s, initial distance 90m',
lead_relevancy=True, duration=20.,
initial_distance_lead=90., initial_speed=20.,
speed_lead_values=[20., 0.], lead_relevancy=True,
breakpoints=[0., 1.], initial_distance_lead=90.,
), speed_lead_values=[20., 0.],
Maneuver( breakpoints=[0., 1.],
'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', e2e=e2e,
duration=50., ),
initial_speed=20., Maneuver(
lead_relevancy=True, 'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2',
initial_distance_lead=35., duration=50.,
speed_lead_values=[20., 20., 0.], initial_speed=20.,
breakpoints=[0., 15., 35.0], lead_relevancy=True,
), initial_distance_lead=35.,
Maneuver( speed_lead_values=[20., 20., 0.],
'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', breakpoints=[0., 15., 35.0],
duration=50., e2e=e2e,
initial_speed=20., ),
lead_relevancy=True, Maneuver(
initial_distance_lead=35., 'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2',
speed_lead_values=[20., 20., 0.], duration=50.,
breakpoints=[0., 15., 25.0], initial_speed=20.,
), lead_relevancy=True,
Maneuver( initial_distance_lead=35.,
'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2', speed_lead_values=[20., 20., 0.],
duration=50., breakpoints=[0., 15., 25.0],
initial_speed=20., e2e=e2e,
lead_relevancy=True, ),
initial_distance_lead=35., Maneuver(
speed_lead_values=[20., 20., 0.], 'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2',
breakpoints=[0., 15., 21.66], duration=50.,
), initial_speed=20.,
Maneuver( lead_relevancy=True,
'steady state following a car at 20m/s, then lead decel to 0mph at 3+m/s^2', initial_distance_lead=35.,
duration=40., speed_lead_values=[20., 20., 0.],
initial_speed=20., breakpoints=[0., 15., 21.66],
lead_relevancy=True, e2e=e2e,
initial_distance_lead=35., ),
speed_lead_values=[20., 20., 0.], Maneuver(
prob_lead_values=[0., 1., 1.], 'steady state following a car at 20m/s, then lead decel to 0mph at 3+m/s^2',
cruise_values=[20., 20., 20.], duration=40.,
breakpoints=[2., 2.01, 8.8], initial_speed=20.,
), lead_relevancy=True,
Maneuver( initial_distance_lead=35.,
"approach stopped car at 20m/s, with prob_lead_values", speed_lead_values=[20., 20., 0.],
duration=30., prob_lead_values=[0., 1., 1.],
initial_speed=20., cruise_values=[20., 20., 20.],
lead_relevancy=True, breakpoints=[2., 2.01, 8.8],
initial_distance_lead=120., e2e=e2e,
speed_lead_values=[0.0, 0., 0.], ),
prob_lead_values=[0.0, 0., 1.], Maneuver(
cruise_values=[20., 20., 20.], "approach stopped car at 20m/s, with prob_lead_values",
breakpoints=[0.0, 2., 2.01], duration=30.,
), initial_speed=20.,
Maneuver( lead_relevancy=True,
"approach slower cut-in car at 20m/s", initial_distance_lead=120.,
duration=20., speed_lead_values=[0.0, 0., 0.],
initial_speed=20., prob_lead_values=[0.0, 0., 1.],
lead_relevancy=True, cruise_values=[20., 20., 20.],
initial_distance_lead=50., breakpoints=[0.0, 2., 2.01],
speed_lead_values=[15., 15.], e2e=e2e,
breakpoints=[1., 11.], ),
only_lead2=True, Maneuver(
), "approach slower cut-in car at 20m/s",
Maneuver( duration=20.,
"stay stopped behind radar override lead", initial_speed=20.,
duration=20., lead_relevancy=True,
initial_speed=0., initial_distance_lead=50.,
lead_relevancy=True, speed_lead_values=[15., 15.],
initial_distance_lead=10., breakpoints=[1., 11.],
speed_lead_values=[0., 0.], only_lead2=True,
prob_lead_values=[0., 0.], e2e=e2e,
breakpoints=[1., 11.], ),
only_radar=True, Maneuver(
), "stay stopped behind radar override lead",
Maneuver( duration=20.,
"NaN recovery", initial_speed=0.,
duration=30., lead_relevancy=True,
initial_speed=15., initial_distance_lead=10.,
lead_relevancy=True, speed_lead_values=[0., 0.],
initial_distance_lead=60., prob_lead_values=[0., 0.],
speed_lead_values=[0., 0., 0.0], breakpoints=[1., 11.],
breakpoints=[1., 1.01, 11.], only_radar=True,
cruise_values=[float("nan"), 15., 15.], e2e=e2e,
), ),
# controls relies on planner commanding to move for stock-ACC resume spamming Maneuver(
Maneuver( "NaN recovery",
"resume from a stop", duration=30.,
duration=20., initial_speed=15.,
initial_speed=0., lead_relevancy=True,
lead_relevancy=True, initial_distance_lead=60.,
initial_distance_lead=STOP_DISTANCE, speed_lead_values=[0., 0., 0.0],
speed_lead_values=[0., 0., 2.], breakpoints=[1., 1.01, 11.],
breakpoints=[1., 10., 15.], cruise_values=[float("nan"), 15., 15.],
ensure_start=True, e2e=e2e,
), ),
Maneuver( # controls relies on planner commanding to move for stock-ACC resume spamming
'cruising at 25 m/s while disabled', Maneuver(
duration=20., "resume from a stop",
initial_speed=25., duration=20.,
lead_relevancy=False, initial_speed=0.,
enabled=False, 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): class LongitudinalControl(unittest.TestCase):
@ -140,29 +154,11 @@ class LongitudinalControl(unittest.TestCase):
params.put_bool("Passive", bool(os.getenv("PASSIVE"))) params.put_bool("Passive", bool(os.getenv("PASSIVE")))
params.put_bool("OpenpilotEnabledToggle", True) params.put_bool("OpenpilotEnabledToggle", True)
# hack @parameterized.expand([(man,) for e2e in [True, False] for man in create_maneuvers(e2e)])
def test_longitudinal_setup(self): def test_maneuver(self, maneuver):
pass print(maneuver.title, f'in {"e2e" if maneuver.e2e else "acc"} mode')
valid, _ = maneuver.evaluate()
self.assertTrue(valid, msg=maneuver.title)
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))
if __name__ == "__main__": if __name__ == "__main__":

Write Preview
Loading…
Cancel
Save