#!/usr/bin/env python3
import itertools
import unittest
from parameterized import parameterized_class
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
# TODO: make new FCW tests
[docs]
def create_maneuvers(kwargs):
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.],
**kwargs,
),
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.],
**kwargs,
),
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],
**kwargs,
),
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],
**kwargs,
),
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],
**kwargs,
),
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],
**kwargs,
),
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],
**kwargs,
),
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,
**kwargs,
),
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,
**kwargs,
),
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.],
**kwargs,
),
Maneuver(
'cruising at 25 m/s while disabled',
duration=20.,
initial_speed=25.,
lead_relevancy=False,
enabled=False,
**kwargs,
),
]
if not kwargs['force_decel']:
# controls relies on planner commanding to move for stock-ACC resume spamming
maneuvers.append(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,
**kwargs,
))
return maneuvers
[docs]
@parameterized_class(("e2e", "force_decel"), itertools.product([True, False], repeat=2))
class LongitudinalControl(unittest.TestCase):
e2e: bool
force_decel: bool
def test_maneuver(self):
for maneuver in create_maneuvers({"e2e": self.e2e, "force_decel": self.force_decel}):
with self.subTest(title=maneuver.title, e2e=maneuver.e2e, force_decel=maneuver.force_decel):
print(maneuver.title, f'in {"e2e" if maneuver.e2e else "acc"} mode')
valid, _ = maneuver.evaluate()
self.assertTrue(valid)
if __name__ == "__main__":
unittest.main(failfast=True)