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)
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 = [pt_to_car(pt) for pt in 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 ) 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)