diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index b4a551f06..ac5f1096a 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -115,7 +115,7 @@ class LongitudinalMpc(): self.v_solution = [0.0 for i in range(len(T_IDXS))] self.a_solution = [0.0 for i in range(len(T_IDXS))] self.j_solution = [0.0 for i in range(len(T_IDXS)-1)] - self.yref = np.ones((N+1, 4)) + self.yref = np.zeros((N+1, 4)) self.solver.cost_set_slice(0, N, "yref", self.yref[:N]) self.solver.cost_set(N, "yref", self.yref[N][:3]) self.T_IDXS = np.array(T_IDXS[:N+1]) diff --git a/selfdrive/controls/tests/test_cruise_speed.py b/selfdrive/controls/tests/test_cruise_speed.py new file mode 100644 index 000000000..364be8db7 --- /dev/null +++ b/selfdrive/controls/tests/test_cruise_speed.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python3 +import unittest +import numpy as np +from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver + +def run_cruise_simulation(cruise, t_end=100.): + man = Maneuver( + '', + duration=t_end, + initial_speed=float(0.), + lead_relevancy=True, + initial_distance_lead=100, + cruise_values=[cruise], + prob_lead_values=[0.0], + breakpoints=[0.], + ) + valid, output = man.evaluate() + assert valid + return output[-1,3] + + +class TestCruiseSpeed(unittest.TestCase): + def test_cruise_speed(self): + 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) + self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {speed} m/s') + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 69fb471e1..2c26d84a8 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -2,70 +2,29 @@ import unittest import numpy as np -from cereal import log -import cereal.messaging as messaging -from selfdrive.config import Conversions as CV -from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import desired_follow_distance, LeadMpc +from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import desired_follow_distance +from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver - -class FakePubMaster(): - def send(self, s, data): - assert data - - -def run_following_distance_simulation(v_lead, t_end=200.0): - dt = 0.2 - t = 0. - - x_lead = 200.0 - - x_ego = 0.0 - v_ego = v_lead - a_ego = 0.0 - - mpc = LeadMpc(0) - - first = True - while t < t_end: - - # Setup CarState - CS = messaging.new_message('carState') - CS.carState.vEgo = float(v_ego) - CS.carState.aEgo = float(a_ego) - - # Setup model packet - radarstate = messaging.new_message('radarState') - lead = log.RadarState.LeadData.new_message() - lead.modelProb = .75 - lead.dRel = x_lead - x_ego - lead.vLead = v_lead - lead.aLeadK = 0.0 - lead.status = True - radarstate.radarState.leadOne = lead - - # Run MPC - mpc.set_cur_state(v_ego, a_ego) - if first: # Make sure MPC is converged on first timestep - for _ in range(20): - mpc.update(CS.carState, radarstate.radarState, 0) - mpc.update(CS.carState, radarstate.radarState, 0) - - # Choose slowest of two solutions - v_ego, a_ego = float(mpc.v_solution[5]), float(mpc.a_solution[5]) - - # Update state - x_lead += v_lead * dt - x_ego += v_ego * dt - t += dt - first = False - - return x_lead - x_ego +def run_following_distance_simulation(v_lead, t_end=150.0): + man = Maneuver( + '', + duration=t_end, + initial_speed=float(v_lead), + lead_relevancy=True, + initial_distance_lead=100, + speed_lead_values=[v_lead], + breakpoints=[0.], + ) + valid, output = man.evaluate() + assert valid + return output[-1,2] - output[-1,1] class TestFollowingDistance(unittest.TestCase): def test_following_distanc(self): - for speed_mph in np.linspace(10, 100, num=10): - v_lead = float(speed_mph * CV.MPH_TO_MS) + 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) correct_steady_state = desired_follow_distance(v_lead, v_lead) diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 8bf675403..b3482f4b7 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -9,9 +9,10 @@ class Maneuver(): self.speed = kwargs.get("initial_speed", 0.0) self.lead_relevancy = kwargs.get("lead_relevancy", 0) - self.speed_lead_breakpoints = kwargs.get("speed_lead_breakpoints", [0.0, duration]) - self.speed_lead_values = kwargs.get("speed_lead_values", [0.0, 0.0]) - self.prob_lead_values = kwargs.get("prob_lead_values", [1.0 for i in range(len(self.speed_lead_breakpoints))]) + self.breakpoints = kwargs.get("breakpoints", [0.0, duration]) + self.speed_lead_values = kwargs.get("speed_lead_values", [0.0 for i in range(len(self.breakpoints))]) + self.prob_lead_values = kwargs.get("prob_lead_values", [1.0 for i in range(len(self.breakpoints))]) + self.cruise_values = kwargs.get("cruise_values", [50.0 for i in range(len(self.breakpoints))]) self.only_lead2 = kwargs.get("only_lead2", False) self.only_radar = kwargs.get("only_radar", False) @@ -31,9 +32,10 @@ class Maneuver(): valid = True logs = [] while plant.current_time() < self.duration: - speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values) - prob = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.prob_lead_values) - log = plant.step(speed_lead, prob) + speed_lead = np.interp(plant.current_time(), self.breakpoints, self.speed_lead_values) + prob = np.interp(plant.current_time(), self.breakpoints, self.prob_lead_values) + cruise = np.interp(plant.current_time(), self.breakpoints, self.cruise_values) + log = plant.step(speed_lead, prob, cruise) d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200. v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0. @@ -46,7 +48,7 @@ class Maneuver(): speed_lead, log['acceleration']])) - if d_rel < 1.0: + if d_rel < 1.0 and (self.only_radar or prob > 0.5): print("Crashed!!!!") valid = False diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 6cdd5bf82..ac19d2754 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -48,7 +48,7 @@ class Plant(): def current_time(self): return float(self.rk.frame) / self.rate - def step(self, v_lead=0.0, prob=1.0): + def step(self, v_lead=0.0, prob=1.0, v_cruise=50.): # ******** publish a fake model going straight and fake calibration ******** # note that this is worst case for MPC, since model will delay long mpc by one time step radar = messaging.new_message('radarState') @@ -89,7 +89,7 @@ class Plant(): control.controlsState.longControlState = LongCtrlState.pid - control.controlsState.vCruise = 130 + control.controlsState.vCruise = float(v_cruise * 3.6) car_state.carState.vEgo = float(self.speed) diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index ddd6a2b61..8d30f40ed 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -15,7 +15,7 @@ maneuvers = [ lead_relevancy=True, initial_distance_lead=120., speed_lead_values=[30., 0.], - speed_lead_breakpoints=[0., 1.], + breakpoints=[0., 1.], ), Maneuver( 'approach stopped car at 20m/s', @@ -24,7 +24,7 @@ maneuvers = [ lead_relevancy=True, initial_distance_lead=60., speed_lead_values=[20., 0.], - speed_lead_breakpoints=[0., 1.], + breakpoints=[0., 1.], ), Maneuver( 'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', @@ -33,7 +33,7 @@ maneuvers = [ lead_relevancy=True, initial_distance_lead=35., speed_lead_values=[20., 20., 0.], - speed_lead_breakpoints=[0., 15., 35.0], + breakpoints=[0., 15., 35.0], ), Maneuver( 'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', @@ -42,7 +42,7 @@ maneuvers = [ lead_relevancy=True, initial_distance_lead=35., speed_lead_values=[20., 20., 0.], - speed_lead_breakpoints=[0., 15., 25.0], + breakpoints=[0., 15., 25.0], ), Maneuver( 'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2', @@ -51,7 +51,7 @@ maneuvers = [ lead_relevancy=True, initial_distance_lead=35., speed_lead_values=[20., 20., 0.], - speed_lead_breakpoints=[0., 15., 21.66], + breakpoints=[0., 15., 21.66], ), Maneuver( 'steady state following a car at 20m/s, then lead decel to 0mph at 5m/s^2', @@ -60,7 +60,7 @@ maneuvers = [ lead_relevancy=True, initial_distance_lead=35., speed_lead_values=[20., 20., 0.], - speed_lead_breakpoints=[0., 15., 19.], + breakpoints=[0., 15., 19.], ), Maneuver( "approach stopped car at 20m/s", @@ -69,7 +69,7 @@ maneuvers = [ lead_relevancy=True, initial_distance_lead=50., speed_lead_values=[0., 0.], - speed_lead_breakpoints=[1., 11.], + breakpoints=[1., 11.], ), Maneuver( "approach slower cut-in car at 20m/s", @@ -78,7 +78,7 @@ maneuvers = [ lead_relevancy=True, initial_distance_lead=50., speed_lead_values=[15., 15.], - speed_lead_breakpoints=[1., 11.], + breakpoints=[1., 11.], only_lead2=True, ), Maneuver( @@ -89,7 +89,7 @@ maneuvers = [ initial_distance_lead=10., speed_lead_values=[0., 0.], prob_lead_values=[0., 0.], - speed_lead_breakpoints=[1., 11.], + breakpoints=[1., 11.], only_radar=True, ), ] diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 256adfe65..092bb061c 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -03585a6e067ff58f1030450efc55cd225dc2ffc3 \ No newline at end of file +1e029954854211b6277f98e6b0c3438fd11e6667 \ No newline at end of file