|
|
@ -1,5 +1,8 @@ |
|
|
|
#!/usr/bin/env python3 |
|
|
|
#!/usr/bin/env python3 |
|
|
|
import unittest |
|
|
|
import unittest |
|
|
|
|
|
|
|
import itertools |
|
|
|
|
|
|
|
from parameterized import parameterized_class |
|
|
|
|
|
|
|
|
|
|
|
from openpilot.common.params import Params |
|
|
|
from openpilot.common.params import Params |
|
|
|
from cereal import log |
|
|
|
from cereal import log |
|
|
|
|
|
|
|
|
|
|
@ -23,22 +26,21 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False): |
|
|
|
return output[-1,2] - output[-1,1] |
|
|
|
return output[-1,2] - output[-1,1] |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
@parameterized_class(("e2e", "personality", "speed"), itertools.product( |
|
|
|
|
|
|
|
[True, False], # e2e |
|
|
|
|
|
|
|
[log.LongitudinalPersonality.relaxed, # personality |
|
|
|
|
|
|
|
log.LongitudinalPersonality.standard, |
|
|
|
|
|
|
|
log.LongitudinalPersonality.aggressive], |
|
|
|
|
|
|
|
[0,10,35])) # speed |
|
|
|
class TestFollowingDistance(unittest.TestCase): |
|
|
|
class TestFollowingDistance(unittest.TestCase): |
|
|
|
def test_following_distance(self): |
|
|
|
def test_following_distance(self): |
|
|
|
params = Params() |
|
|
|
params = Params() |
|
|
|
personalities = [log.LongitudinalPersonality.relaxed, |
|
|
|
params.put("LongitudinalPersonality", str(self.personality)) |
|
|
|
log.LongitudinalPersonality.standard, |
|
|
|
v_lead = float(self.speed) |
|
|
|
log.LongitudinalPersonality.aggressive] |
|
|
|
simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e) |
|
|
|
for personality in personalities: |
|
|
|
correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality)) |
|
|
|
params.put("LongitudinalPersonality", str(personality)) |
|
|
|
err_ratio = 0.2 if self.e2e else 0.1 |
|
|
|
for e2e in [False, True]: |
|
|
|
self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(err_ratio * correct_steady_state + .5)) |
|
|
|
for speed in [0,10,35]: |
|
|
|
|
|
|
|
print(f'Testing {speed} m/s') |
|
|
|
|
|
|
|
v_lead = float(speed) |
|
|
|
|
|
|
|
simulation_steady_state = run_following_distance_simulation(v_lead, e2e=e2e) |
|
|
|
|
|
|
|
correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(personality)) |
|
|
|
|
|
|
|
err_ratio = 0.2 if e2e else 0.1 |
|
|
|
|
|
|
|
self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(err_ratio * correct_steady_state + .5)) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if __name__ == "__main__": |
|
|
|
if __name__ == "__main__": |
|
|
|