mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-07 22:52:06 +08:00
Jonathan Frey
baab5d7a7b
* acados_ocp_solver_pyx.pyx: implement get_stats for timings and ints * long_mpc: use acados timers * acados_ocp_solver_pyx.pyx: fix dynamics_get * acados_ocp_solver_pyx.pyx: get statistics * use acados_ocp_solver_pyx.pyx from commaai/cython2 branch * acados_ocp_solver_pyx.pyx: implement store_iterate * acados_ocp_solver_pyx.pyx: implement get_residuals * acados_ocp_solver_pyx.pyx: fix set() for empty fields * acados_ocp_solver_pyx.pyx: load_iterate * cython acados: add print_statistics * test_following_distance: fix typo * test_longitudinal: unique names for test maneuvers * longitudinal MPC: comments for evaluation * longitudinal MPC: add comments to eval acados residuals * long_mpc: use qp_solver_cond_N = 1 * long MPC: comments, simplify set_cur_state * update acados version in build script * longitudinal mpc: weigh a_change in 1 place only * update ref * Update ref Co-authored-by: Harald Schafer <harald.the.engineer@gmail.com> old-commit-hash: d09dffb7cd99aefb7e3c251265f2cfd9ae9e72f8
37 lines
1.0 KiB
Python
37 lines
1.0 KiB
Python
#!/usr/bin/env python3
|
|
import unittest
|
|
import numpy as np
|
|
|
|
from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance
|
|
from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
|
|
|
|
def run_following_distance_simulation(v_lead, t_end=100.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_distance(self):
|
|
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)
|
|
|
|
self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(correct_steady_state*.1 + .5))
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|