#!/usr/bin/env python3
import unittest
import numpy as np
from common . params import Params
from selfdrive . test . longitudinal_maneuvers . maneuver import Maneuver
def run_cruise_simulation ( cruise , t_end = 20. ) :
man = Maneuver (
' ' ,
duration = t_end ,
initial_speed = max ( cruise - 1. , 0.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 ) :
params = Params ( )
for e2e in [ False , True ] :
params . put_bool ( " EndToEndLong " , e2e )
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 ( )