def evaluate(self):
plant = Plant(lead_relevancy=self.lead_relevancy,
speed=self.speed,
distance_lead=self.distance_lead,
only_lead2=self.only_lead2,
only_radar=self.only_radar)
valid = True
while plant.current_time() < self.duration:
speed_lead = np.interp(plant.current_time(),
self.speed_lead_breakpoints,
self.speed_lead_values)
log = plant.step(speed_lead)
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 d_rel < 1.0:
print("Crashed!!!!")
valid = False
print("maneuver end", valid)
return valid
def evaluate(self):
plant = Plant(
lead_relevancy=self.lead_relevancy,
speed=self.speed,
distance_lead=self.distance_lead,
only_lead2=self.only_lead2,
only_radar=self.only_radar
)
valid = True
logs = []
while plant.current_time() < self.duration:
speed_lead = np.interp(plant.current_time(), self.breakpoints, self.speed_lead_values)
prob = np.interp(plant.current_time(), self.breakpoints, self.prob_lead_values)
cruise = np.interp(plant.current_time(), self.breakpoints, self.cruise_values)
log = plant.step(speed_lead, prob, cruise)
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
logs.append(np.array([plant.current_time(),
log['distance'],
log['distance_lead'],
log['speed'],
speed_lead,
log['acceleration']]))
if d_rel < .4 and (self.only_radar or prob > 0.5):
print("Crashed!!!!")
valid = False
print("maneuver end", valid)
return valid, np.array(logs)
car.fill((10,10,10))
car.set_alpha(128)
pygame.draw.rect(car, c, (METER*1.25, 0, METER*CAR_WIDTH, METER*CAR_LENGTH), 1)
return car
if __name__ == "__main__":
pygame.init()
display = pygame.display.set_mode((1000, 1000))
pygame.display.set_caption('Plant UI')
car = car_w_color((255,0,255))
leadcar = car_w_color((255,0,0))
carx, cary, heading = 10.0, 50.0, 0.0
plant = Plant(100, distance_lead = 40.0)
control_offset = 2.0
control_pts = list(zip(np.arange(0, 100.0, 10.0), [50.0 + control_offset]*10))
def pt_to_car(pt):
x,y = pt
x -= carx
y -= cary
rx = x * math.cos(-heading) + y * -math.sin(-heading)
ry = x * math.sin(-heading) + y * math.cos(-heading)
return rx, ry
def pt_from_car(pt):
x,y = pt
rx = x * math.cos(heading) + y * -math.sin(heading)
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)