Extra longitudinal unit tests (#21601)
* linter needs this * unit test radar overridepull/21603/head
parent
ef0b120a9a
commit
9e8d99b530
|
@ -12,6 +12,9 @@ class Maneuver():
|
|||
self.speed_lead_values = kwargs.get("speed_lead_values", [0.0, 0.0])
|
||||
self.speed_lead_breakpoints = kwargs.get("speed_lead_breakpoints", [0.0, duration])
|
||||
|
||||
self.only_lead2 = kwargs.get("only_lead2", False)
|
||||
self.only_radar = kwargs.get("only_radar", False)
|
||||
|
||||
self.duration = duration
|
||||
self.title = title
|
||||
|
||||
|
@ -19,7 +22,9 @@ class Maneuver():
|
|||
plant = Plant(
|
||||
lead_relevancy=self.lead_relevancy,
|
||||
speed=self.speed,
|
||||
distance_lead=self.distance_lead
|
||||
distance_lead=self.distance_lead,
|
||||
only_lead2=self.only_lead2,
|
||||
only_radar=self.only_radar
|
||||
)
|
||||
|
||||
valid = True
|
||||
|
|
|
@ -11,7 +11,8 @@ from selfdrive.controls.lib.longcontrol import LongCtrlState
|
|||
class Plant():
|
||||
messaging_initialized = False
|
||||
|
||||
def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0):
|
||||
def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0,
|
||||
only_lead2=False, only_radar=False):
|
||||
self.rate = 1. / DT_MDL
|
||||
|
||||
if not Plant.messaging_initialized:
|
||||
|
@ -30,12 +31,28 @@ class Plant():
|
|||
# lead car
|
||||
self.distance_lead = distance_lead
|
||||
self.lead_relevancy = lead_relevancy
|
||||
self.only_lead2=only_lead2
|
||||
self.only_radar=only_radar
|
||||
|
||||
self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
|
||||
self.ts = 1. / self.rate
|
||||
time.sleep(1)
|
||||
self.sm = messaging.SubMaster(['longitudinalPlan'])
|
||||
|
||||
# make sure planner has time to be fully initialized
|
||||
# TODO planner should just be explicitly initialized
|
||||
for i in range(10000):
|
||||
assert i < 10000
|
||||
radar = messaging.new_message('radarState')
|
||||
control = messaging.new_message('controlsState')
|
||||
car_state = messaging.new_message('carState')
|
||||
control.controlsState.longControlState = LongCtrlState.pid
|
||||
control.controlsState.vCruise = speed*3.6
|
||||
car_state.carState.vEgo = self.speed
|
||||
Plant.radar.send(radar.to_bytes())
|
||||
Plant.controls_state.send(control.to_bytes())
|
||||
Plant.car_state.send(car_state.to_bytes())
|
||||
|
||||
def current_time(self):
|
||||
return float(self.rk.frame) / self.rate
|
||||
|
||||
|
@ -51,11 +68,16 @@ class Plant():
|
|||
if self.lead_relevancy:
|
||||
d_rel = np.maximum(0., self.distance_lead - self.distance)
|
||||
v_rel = v_lead - self.speed
|
||||
prob = 1.0
|
||||
if self.only_radar:
|
||||
prob = 0.0
|
||||
else:
|
||||
prob = 1.0
|
||||
status = True
|
||||
else:
|
||||
d_rel = 200.
|
||||
v_rel = 0.
|
||||
prob = 0.0
|
||||
status = False
|
||||
|
||||
lead = log.RadarState.LeadData.new_message()
|
||||
lead.dRel = float(d_rel)
|
||||
|
@ -66,9 +88,10 @@ class Plant():
|
|||
lead.vLeadK = float(v_lead)
|
||||
lead.aLeadK = float(a_lead)
|
||||
lead.aLeadTau = float(1.5)
|
||||
lead.status = True
|
||||
lead.status = status
|
||||
lead.modelProb = prob
|
||||
radar.radarState.leadOne = lead
|
||||
if not self.only_lead2:
|
||||
radar.radarState.leadOne = lead
|
||||
radar.radarState.leadTwo = lead
|
||||
|
||||
|
||||
|
|
|
@ -61,6 +61,26 @@ maneuvers = [
|
|||
speed_lead_values=[0., 0.],
|
||||
speed_lead_breakpoints=[1., 11.],
|
||||
),
|
||||
Maneuver(
|
||||
"approach slower cut-in car at 20m/s",
|
||||
duration=20.,
|
||||
initial_speed=20.,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=50.,
|
||||
speed_lead_values=[15., 15.],
|
||||
speed_lead_breakpoints=[1., 11.],
|
||||
only_lead2=True,
|
||||
),
|
||||
Maneuver(
|
||||
"stay stopped behind radar override lead",
|
||||
duration=20.,
|
||||
initial_speed=0.,
|
||||
lead_relevancy=True,
|
||||
initial_distance_lead=10.,
|
||||
speed_lead_values=[0., 0.],
|
||||
speed_lead_breakpoints=[1., 11.],
|
||||
only_radar=True,
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
|
|
Loading…
Reference in New Issue