123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160 |
- #!/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
- 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
- @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)
|