diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 23fb1b7790..b5998a2564 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -357,7 +357,7 @@ class LongitudinalMpc: v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) - cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, get_T_FOLLOW()) + cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, get_T_FOLLOW(personality)) x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] diff --git a/selfdrive/controls/tests/test_cruise_speed.py b/selfdrive/controls/tests/test_cruise_speed.py index b83116af45..6d11c30ab2 100755 --- a/selfdrive/controls/tests/test_cruise_speed.py +++ b/selfdrive/controls/tests/test_cruise_speed.py @@ -1,8 +1,10 @@ #!/usr/bin/env python3 import numpy as np -from parameterized import parameterized_class import unittest +from parameterized import parameterized_class +from cereal import log +from common.params import Params from selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT from cereal import car from common.conversions import Conversions as CV @@ -31,13 +33,19 @@ def run_cruise_simulation(cruise, e2e, t_end=20.): class TestCruiseSpeed(unittest.TestCase): def test_cruise_speed(self): - for e2e in [False, True]: - for speed in np.arange(5, 40, 5): - print(f'Testing {speed} m/s') - cruise_speed = float(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') + params = Params() + personalities = [log.LongitudinalPersonality.relaxed, + log.LongitudinalPersonality.standard, + log.LongitudinalPersonality.aggressive] + for personality in personalities: + params.put("LongitudinalPersonality", str(personality)) + for e2e in [False, True]: + for speed in [5,35]: + print(f'Testing {speed} m/s') + cruise_speed = float(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') # TODO: test pcmCruise diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 5185867d2d..9ee7bdfb3b 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -1,8 +1,9 @@ #!/usr/bin/env python3 import unittest -import numpy as np +from common.params import Params +from cereal import log -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, get_T_FOLLOW from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver @@ -24,14 +25,20 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False): class TestFollowingDistance(unittest.TestCase): def test_following_distance(self): - for e2e in [False, True]: - for speed in np.arange(0, 40, 5): - 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) - err_ratio = 0.2 if e2e else 0.1 - self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(err_ratio * correct_steady_state + .5)) + params = Params() + personalities = [log.LongitudinalPersonality.relaxed, + log.LongitudinalPersonality.standard, + log.LongitudinalPersonality.aggressive] + for personality in personalities: + params.put("LongitudinalPersonality", str(personality)) + for e2e in [False, True]: + 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__":