|
|
@ -3,14 +3,13 @@ import unittest |
|
|
|
import itertools |
|
|
|
import itertools |
|
|
|
from parameterized import parameterized_class |
|
|
|
from parameterized import parameterized_class |
|
|
|
|
|
|
|
|
|
|
|
from openpilot.common.params import Params |
|
|
|
|
|
|
|
from cereal import log |
|
|
|
from cereal import log |
|
|
|
|
|
|
|
|
|
|
|
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance, get_T_FOLLOW |
|
|
|
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance, get_T_FOLLOW |
|
|
|
from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver |
|
|
|
from openpilot.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, personality=0): |
|
|
|
man = Maneuver( |
|
|
|
man = Maneuver( |
|
|
|
'', |
|
|
|
'', |
|
|
|
duration=t_end, |
|
|
|
duration=t_end, |
|
|
@ -20,6 +19,7 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False): |
|
|
|
speed_lead_values=[v_lead], |
|
|
|
speed_lead_values=[v_lead], |
|
|
|
breakpoints=[0.], |
|
|
|
breakpoints=[0.], |
|
|
|
e2e=e2e, |
|
|
|
e2e=e2e, |
|
|
|
|
|
|
|
personality=personality, |
|
|
|
) |
|
|
|
) |
|
|
|
valid, output = man.evaluate() |
|
|
|
valid, output = man.evaluate() |
|
|
|
assert valid |
|
|
|
assert valid |
|
|
@ -34,10 +34,8 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False): |
|
|
|
[0,10,35])) # speed |
|
|
|
[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.put("LongitudinalPersonality", str(self.personality)) |
|
|
|
|
|
|
|
v_lead = float(self.speed) |
|
|
|
v_lead = float(self.speed) |
|
|
|
simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e) |
|
|
|
simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e, personality=self.personality) |
|
|
|
correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality)) |
|
|
|
correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality)) |
|
|
|
err_ratio = 0.2 if self.e2e else 0.1 |
|
|
|
err_ratio = 0.2 if self.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)) |
|
|
|