mirror of
https://github.com/infiniteCable2/openpilot.git
synced 2026-09-12 03:03:41 +08:00
HaraldSchafer
96d4bfbff3
* refactor * needs casting * tests pass * fix that test * refactor in controls * lets not go crazy * change of names * use constants * better naming * renamed * soft constraints * compile slack variables * rm git conflict * add slack variables * unused * new edition * fcw * fix tests * dividing causes problems * was way too slow * take a step back * byeeee * for another time * bad idxs * little more cpu for cruise mpc * update refs * these limits seem fine * rename * test model timings fails sometimes * add default * save some cpu * Revert "little more cpu for cruise mpc" This reverts commit f0a8163ec90e8dc1eabb3c4a4268ad330d23374d. * Revert "test model timings fails sometimes" This reverts commit d259d845710ed2cbeb28b383e2600476527d4838. * update refs * less cpu * Revert "Revert "test model timings fails sometimes"" This reverts commit e0263050d9929bfc7ee70c9788234541a4a8461c. * Revert "less cpu" This reverts commit 679007472bc2013e7fafb7b17de7a43d6f82359a. * cleanup * not too much until we clean up mpc * more cost on jerk * change ref * add todo * new ref * indentation old-commit-hash: be5ddd25cd3f4c8b7e46cdeecf303e2c73301003
84 lines
1.9 KiB
Python
84 lines
1.9 KiB
Python
#!/usr/bin/env python3
|
|
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 import LeadMpc
|
|
|
|
|
|
def RW(v_ego, v_l):
|
|
TR = 1.8
|
|
G = 9.81
|
|
return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G))
|
|
|
|
|
|
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 = v_ego
|
|
CS.carState.aEgo = 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 = mpc.mpc_solution.v_ego[5], mpc.mpc_solution.a_ego[5]
|
|
|
|
# Update state
|
|
x_lead += v_lead * dt
|
|
x_ego += v_ego * dt
|
|
t += dt
|
|
first = False
|
|
|
|
return x_lead - x_ego
|
|
|
|
|
|
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)
|
|
|
|
simulation_steady_state = run_following_distance_simulation(v_lead)
|
|
correct_steady_state = RW(v_lead, v_lead) + 4.0
|
|
|
|
self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=.1)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|