87 lines
3.4 KiB
Python
87 lines
3.4 KiB
Python
from collections import defaultdict
|
|
from selfdrive.test.longitudinal_maneuvers.maneuverplots import ManeuverPlot
|
|
from selfdrive.test.longitudinal_maneuvers.plant import Plant
|
|
import numpy as np
|
|
|
|
|
|
class Maneuver():
|
|
def __init__(self, title, duration, **kwargs):
|
|
# Was tempted to make a builder class
|
|
self.distance_lead = kwargs.get("initial_distance_lead", 200.0)
|
|
self.speed = kwargs.get("initial_speed", 0.0)
|
|
self.lead_relevancy = kwargs.get("lead_relevancy", 0)
|
|
|
|
self.grade_values = kwargs.get("grade_values", [0.0, 0.0])
|
|
self.grade_breakpoints = kwargs.get("grade_breakpoints", [0.0, duration])
|
|
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.cruise_button_presses = kwargs.get("cruise_button_presses", [])
|
|
self.checks = kwargs.get("checks", [])
|
|
|
|
self.duration = duration
|
|
self.title = title
|
|
|
|
def evaluate(self):
|
|
"""runs the plant sim and returns (score, run_data)"""
|
|
plant = Plant(
|
|
lead_relevancy=self.lead_relevancy,
|
|
speed=self.speed,
|
|
distance_lead=self.distance_lead
|
|
)
|
|
|
|
logs = defaultdict(list)
|
|
last_controls_state = None
|
|
plot = ManeuverPlot(self.title)
|
|
|
|
buttons_sorted = sorted(self.cruise_button_presses, key=lambda a: a[1])
|
|
current_button = 0
|
|
while plant.current_time() < self.duration:
|
|
while buttons_sorted and plant.current_time() >= buttons_sorted[0][1]:
|
|
current_button = buttons_sorted[0][0]
|
|
buttons_sorted = buttons_sorted[1:]
|
|
print("current button changed to {0}".format(current_button))
|
|
|
|
grade = np.interp(plant.current_time(), self.grade_breakpoints, self.grade_values)
|
|
speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
|
|
|
|
# distance, speed, acceleration, distance_lead, brake, gas, steer_torque, fcw, controls_state= plant.step(speed_lead, current_button, grade)
|
|
log = plant.step(speed_lead, current_button, grade)
|
|
|
|
if log['controls_state_msgs']:
|
|
last_controls_state = log['controls_state_msgs'][-1]
|
|
|
|
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.
|
|
log['d_rel'] = d_rel
|
|
log['v_rel'] = v_rel
|
|
|
|
if last_controls_state:
|
|
# print(last_controls_state)
|
|
#develop plots
|
|
plot.add_data(
|
|
time=plant.current_time(),
|
|
gas=log['gas'], brake=log['brake'], steer_torque=log['steer_torque'],
|
|
distance=log['distance'], speed=log['speed'], acceleration=log['acceleration'],
|
|
up_accel_cmd=last_controls_state.upAccelCmd, ui_accel_cmd=last_controls_state.uiAccelCmd,
|
|
uf_accel_cmd=last_controls_state.ufAccelCmd,
|
|
d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead,
|
|
v_target_lead=last_controls_state.vTargetLead, pid_speed=last_controls_state.vPid,
|
|
cruise_speed=last_controls_state.vCruise,
|
|
jerk_factor=last_controls_state.jerkFactor,
|
|
a_target=last_controls_state.aTarget,
|
|
fcw=log['fcw'])
|
|
|
|
for k, v in log.items():
|
|
logs[k].append(v)
|
|
|
|
valid = True
|
|
for check in self.checks:
|
|
c = check(logs)
|
|
if not c:
|
|
print(check.__name__ + " not valid!")
|
|
valid = valid and c
|
|
|
|
print("maneuver end", valid)
|
|
return (plot, valid)
|