md.model.frameId = 0
for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
x.points = [0.0]*50
x.prob = 0.0
x.std = 1.0
car_pts = map(pt_to_car, control_pts)
print(car_pts)
car_poly = np.polyfit([x[0] for x in car_pts], [x[1] for x in car_pts], 3)
md.model.path.points = np.polyval(car_poly, np.arange(0, 50)).tolist()
md.model.path.prob = 1.0
Plant.model.send(md.to_bytes())
plant.step(cruise_buttons = cruise_buttons, v_lead = 2.0, publish_model = False)
display.fill((10,10,10))
carx += plant.speed * plant.ts * math.cos(heading)
cary += plant.speed * plant.ts * math.sin(heading)
# positive steering angle = steering right
print(plant.angle_steer)
heading += plant.angle_steer * plant.ts
print(heading)
# draw my car
display.blit(pygame.transform.rotate(car, 90-math.degrees(heading)), (carx*METER, cary*METER))
# draw control pts
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)
last_live100 = None
event_queue = sorted(self.cruise_button_presses,
key=lambda a: a[1])[::-1]
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", 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, live100 = plant.step(
speed_lead, current_button, grade)
if live100:
last_live100 = live100[-1]
d_rel = distance_lead - distance if self.lead_relevancy else 200.
v_rel = speed_lead - speed if self.lead_relevancy else 0.
if last_live100:
# print last_live100
#develop plots
plot.add_data(time=plant.current_time(),
gas=gas,
brake=brake,
steer_torque=steer_torque,
distance=distance,
speed=speed,
acceleration=acceleration,
up_accel_cmd=last_live100.upAccelCmd,
ui_accel_cmd=last_live100.uiAccelCmd,
d_rel=d_rel,
v_rel=v_rel,
v_lead=speed_lead,
v_target_lead=last_live100.vTargetLead,
pid_speed=last_live100.vPid,
cruise_speed=last_live100.vCruise,
jerk_factor=last_live100.jerkFactor,
a_target=last_live100.aTarget)
print "maneuver end"
return (None, plot)
