class ImageAgent(BaseAgent): def setup(self, path_to_conf_file, data_folder, route_folder, k, model_path): super().setup(path_to_conf_file, data_folder, route_folder, k, model_path) self.converter = Converter() self.net = ImageModel.load_from_checkpoint(path_to_conf_file) self.data_folder = data_folder self.route_folder = route_folder self.scene_number = k self.weather_file = self.route_folder + "/weather_data.csv" self.model_path = model_path self.model_vae = None self.net.cuda() self.net.eval() self.run = 0 self.risk = 0 self.state = [] self.monitors = [] self.blur_queue = Queue(maxsize=1) self.occlusion_queue = Queue(maxsize=1) self.am_queue = Queue(maxsize=1) self.pval_queue = Queue(maxsize=1) self.sval_queue = Queue(maxsize=1) self.mval_queue = Queue(maxsize=1) self.avg_risk_queue = Queue(maxsize=1) self.calib_set = [] self.result = [] self.blur = [] self.occlusion = [] self.detector_file = None self.detector_file = None K.clear_session() config = tf.ConfigProto() sess = tf.Session(config=config) set_session(sess) with open(self.model_path + 'auto_model.json', 'r') as jfile: self.model_vae = model_from_json(jfile.read()) self.model_vae.load_weights(self.model_path + 'auto_model.h5') self.model_vae._make_predict_function() self.fields = [ 'step', 'monitor_result', 'risk_score', 'rgb_blur', 'rgb_left_blur', 'rgb_right_blur', 'rgb_occluded', 'rgb_left_occluded', 'rgb_right_occluded', ] self.weather, self.run_number = process_weather_data( self.weather_file, self.scene_number) print(self.weather) with open(self.model_path + 'calibration.csv', 'r') as file: reader = csv.reader(file) for row in reader: self.calib_set.append(row) def _init(self): super()._init() self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) def blur_detection(self, result): self.blur = [] fm1, rgb_blur = blur_detector(result['rgb'], threshold=20) fm2, rgb_left_blur = blur_detector(result['rgb_left'], threshold=20) fm3, rgb_right_blur = blur_detector(result['rgb_right'], threshold=20) self.blur.append(rgb_blur) self.blur.append(rgb_left_blur) self.blur.append(rgb_right_blur) self.blur_queue.put(self.blur) def occlusion_detection(self, result): self.occlusion = [] percent1, rgb_occluded = occlusion_detector(result['rgb'], threshold=25) percent2, rgb_left_occluded = occlusion_detector(result['rgb_left'], threshold=25) percent3, rgb_right_occluded = occlusion_detector(result['rgb_right'], threshold=25) self.occlusion.append(rgb_occluded) self.occlusion.append(rgb_left_occluded) self.occlusion.append(rgb_right_occluded) self.occlusion_queue.put(self.occlusion) def integrand1(self, p_anomaly): result = 0.0 for i in range(len(p_anomaly)): result += p_anomaly[i] result = result / len(p_anomaly) return result def integrand(self, k, p_anomaly): result = 1.0 for i in range(len(p_anomaly)): result *= k * (p_anomaly[i]**(k - 1.0)) return result def mse(self, imageA, imageB): err = np.mean(np.power(imageA - imageB, 2), axis=1) return err def assurance_monitor(self, dist): if (self.step == 0): p_anomaly = [] prev_value = [] else: p_anomaly = self.pval_queue.get() prev_value = self.sval_queue.get() anomaly = 0 m = 0 delta = 10 threshold = 20 sliding_window = 15 threshold = 10.0 for i in range(len(self.calib_set)): if (float(dist) <= float(self.calib_set[i][0])): anomaly += 1 p_value = anomaly / len(self.calib_set) if (p_value < 0.005): p_anomaly.append(0.005) else: p_anomaly.append(p_value) if (len(p_anomaly)) >= sliding_window: p_anomaly = p_anomaly[-1 * sliding_window:] m = integrate.quad(self.integrand, 0.0, 1.0, args=(p_anomaly)) m_val = round(math.log(m[0]), 2) if (self.step == 0): S = 0 S_prev = 0 else: S = max(0, prev_value[0] + prev_value[1] - delta) prev_value = [] S_prev = S m_prev = m[0] prev_value.append(S_prev) prev_value.append(m_prev) self.pval_queue.put(p_anomaly) self.sval_queue.put(prev_value) self.mval_queue.put(m_val) def risk_computation(self, weather, blur_queue, am_queue, occlusion_queue, fault_scenario, fault_type, fault_time, fault_step): monitors = [] faults = [] faults.append(fault_type) blur = self.blur_queue.get() occlusion = self.occlusion_queue.get() mval = self.mval_queue.get() if (self.step == 0): avg_risk = [] else: avg_risk = self.avg_risk_queue.get() monitors = blur + occlusion state = {"enviornment": {}, "fault_modes": None, "monitor_values": {}} for i in range(len(weather)): label = ENV_LABELS[i] state["enviornment"][label] = weather[i] state["fault_modes"] = fault_type for j in range(len(monitors)): label = MONITOR_LABELS[j] state["monitor_values"][label] = monitors[j] state["monitor_values"]["lec_martingale"] = mval fault_modes = state["fault_modes"][0] environment = state["enviornment"] monitor_values = state["monitor_values"] if (fault_scenario == 0): fault_modes = state["fault_modes"][0] if (fault_scenario == 1): if (self.step >= fault_step[0] and self.step < fault_step[0] + fault_time[0]): fault_modes = state["fault_modes"][0] if (fault_scenario > 1): if (self.step >= fault_step[0] and self.step < fault_step[0] + fault_time[0]): fault_modes = state["fault_modes"][0] elif (self.step >= fault_step[1] and self.step < fault_step[1] + fault_time[1]): fault_modes = state["fault_modes"][1] bowtie = BowTie() r_t1_top = bowtie.rate_t1(state) * (1 - bowtie.prob_b1(state, fault_modes)) r_t2_top = bowtie.rate_t2(state) * (1 - bowtie.prob_b2(state, fault_modes)) r_top = r_t1_top + r_t2_top r_c1 = r_top * (1 - bowtie.prob_b3(state)) print("Dynamic Risk Score:%f" % r_c1) avg_risk.append(r_c1) if (len(avg_risk)) >= 20: avg_risk = avg_risk[-1 * 20:] #m = integrate.cumtrapz(avg_risk) m = self.integrand1(avg_risk) self.avg_risk_queue.put(avg_risk) dict = [{ 'step': self.step, 'monitor_result': mval, 'risk_score': r_c1, 'rgb_blur': blur[0], 'rgb_left_blur': blur[1], 'rgb_right_blur': blur[2], 'rgb_occluded': occlusion[0], 'rgb_left_occluded': occlusion[1], 'rgb_right_occluded': occlusion[2] }] if (self.step == 0): self.detector_file = self.data_folder + "/run%d.csv" % ( self.scene_number) file_exists = os.path.isfile(self.detector_file) with open(self.detector_file, 'a') as csvfile: # creating a csv dict writer object writer = csv.DictWriter(csvfile, fieldnames=self.fields) if not file_exists: writer.writeheader() writer.writerows(dict) return m, mval, blur[0], blur[1], blur[2], occlusion[0], occlusion[ 1], occlusion[2] def tick(self, input_data): self.time = time.time() result = super().tick(input_data) result['rgb_detector'] = cv2.resize(result['rgb'], (224, 224)) result['rgb_detector_left'] = cv2.resize(result['rgb_left'], (224, 224)) result['rgb_detector_right'] = cv2.resize(result['rgb_right'], (224, 224)) result['rgb'] = cv2.resize(result['rgb'], (256, 144)) result['rgb_left'] = cv2.resize(result['rgb_left'], (256, 144)) result['rgb_right'] = cv2.resize(result['rgb_right'], (256, 144)) detection_image = cv2.cvtColor(result['rgb_detector_right'], cv2.COLOR_BGR2RGB) detection_image = detection_image / 255. detection_image = np.reshape(detection_image, [ -1, detection_image.shape[0], detection_image.shape[1], detection_image.shape[2] ]) #B-VAE reconstruction based assurance monitor # TODO: Move this prediction into a thread. I had problems of using keras model in threads. predicted_reps = self.model_vae.predict_on_batch(detection_image) dist = np.square(np.subtract(np.array(predicted_reps), detection_image)).mean() #start other detectors BlurDetectorThread = Thread(target=self.blur_detection, args=(result, )) BlurDetectorThread.daemon = True OccusionDetectorThread = Thread(target=self.occlusion_detection, args=(result, )) OccusionDetectorThread.daemon = True AssuranceMonitorThread = Thread( target=self.assurance_monitor, args=(dist, )) #image,model,calibration_set,pval_queue,sval_queue AssuranceMonitorThread.daemon = True AssuranceMonitorThread.start() BlurDetectorThread.start() OccusionDetectorThread.start() result['image'] = np.concatenate( tuple(result[x] for x in ['rgb', 'rgb_left', 'rgb_right']), -1) theta = result['compass'] theta = 0.0 if np.isnan(theta) else theta theta = theta + np.pi / 2 R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) gps = self._get_position(result) result['gps_y'] = gps[0] result['gps_x'] = gps[1] far_node, _ = self._command_planner.run_step(gps) result['actual_y'] = far_node[0] result['actual_x'] = far_node[1] result['far_node'] = far_node target = R.T.dot(far_node - gps) target *= 5.5 target += [128, 256] target = np.clip(target, 0, 256) #waypoints_left = self._command_planner.get_waypoints_remaining(gps) #print("step:%d waypoints:%d"%(self.step,(9-waypoints_left))) #Synthetically add noise to the radar data based on the weather if (self.weather[0] <= "20.0" and self.weather[1] <= "20.0" and self.weather[2] <= "20.0"): result['cloud'][0] = result['cloud'][0] result['cloud_right'][0] = result['cloud_right'][0] elif ((self.weather[0] > "20.0" and self.weather[0] < "50.0") and (self.weather[1] > "20.0" and self.weather[1] < "50.0") and (self.weather[2] > "20.0" and self.weather[2] < "50.0")): noise = np.random.normal(0, 0.5, result['cloud'][0].shape) result['cloud'][0] += noise result['cloud_right'][0] += noise elif ((self.weather[0] > "50.0" and self.weather[0] < "70.0") and (self.weather[1] > "50.0" and self.weather[1] < "70.0") and (self.weather[2] > "50.0" and self.weather[2] < "70.0")): noise = np.random.normal(0, 1.5, result['cloud'][0].shape) result['cloud'][0] += noise result['cloud_right'][0] += noise elif ((self.weather[0] > "70.0" and self.weather[0] < "100.0") and (self.weather[1] > "70.0" and self.weather[1] < "100.0") and (self.weather[2] > "70.0" and self.weather[2] < "100.0")): noise = np.random.normal(0, 2, result['cloud'][0].shape) result['cloud'][0] += noise result['cloud_right'][0] += noise else: noise = np.random.normal(0, 0.5, result['cloud'][0].shape) result['cloud'][0] += noise result['cloud_right'][0] += noise result['target'] = target #Extract minimum distance from the 2 RADAR's. Center and right RADAR's object_depth = [] object_vel = [] for i in range(len(result['cloud'])): object_depth.append(result['cloud'][i][0]) object_vel.append(result['cloud'][i][3]) index = np.argmin(object_depth) result['object_velocity'] = object_vel[index] result['object_distance'] = abs(min(object_depth)) object_depth_right = [] object_vel_right = [] for i in range(len(result['cloud_right'])): object_depth_right.append(result['cloud_right'][i][0]) object_vel_right.append(result['cloud_right'][i][3]) index = np.argmin(object_depth_right) result['object_velocity_right'] = object_vel_right[index] result['object_distance_right'] = abs(min(object_depth_right)) #Join the threads BlurDetectorThread.join() OccusionDetectorThread.join() AssuranceMonitorThread.join() #Compute risk # TODO: We could do it in a thread in the future when risk is computed only once every few cycles. Now risk is computed every cycle t = time.time() risk, mval, blur0, blur1, blur2, Occlusion0, Occlusion1, Occlusion2 = self.risk_computation( self.weather, self.blur_queue, self.am_queue, self.occlusion_queue, result["fault_scenario"], result["fault_type"], result["fault_duration"], result["fault_step"]) like = 1 - math.exp(-risk) result['time'] = time.time() - t result['risk'] = risk del predicted_reps gc.collect() return result #Step function that runs the Primary LEC and the secondary AEBS controller @torch.no_grad() def run_step(self, input_data, timestamp): if not self.initialized: self._init() time_val = 0.0 tick_data = self.tick(input_data) img = torchvision.transforms.functional.to_tensor(tick_data['image']) img = img[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() #Primary LEC controller with torch.no_grad(): points, (target_cam, _) = self.net.forward(img, target) points_cam = points.clone().cpu() points_cam[..., 0] = (points_cam[..., 0] + 1) / 2 * img.shape[-1] points_cam[..., 1] = (points_cam[..., 1] + 1) / 2 * img.shape[-2] points_cam = points_cam.squeeze() points_world = self.converter.cam_to_world(points_cam).numpy() aim = (points_world[1] + points_world[0]) / 2.0 angle = np.degrees(np.pi / 2 - np.arctan2(aim[1], aim[0])) / 90 steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) desired_speed = np.linalg.norm(points_world[0] - points_world[1]) * 2.0 speed = tick_data['speed'] stopping_distance = ((speed * speed) / 13.72) #Supervisory AEBS controller if (speed >= 0.5): if (tick_data['object_distance'] < (1.5 * speed + (speed * speed) / 13.72) or tick_data['object_distance_right'] < (1.5 * speed + (speed * speed) / 13.72)): brake = True val = 1 print("emergency") else: brake = False elif (speed < 0.5): if ((tick_data['object_distance'] < 3) or (tick_data['object_distance_right'] < 3)): brake = True val = 1 print("emergency") else: brake = False else: brake = desired_speed < 0.2 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) #Log AV stats fields = [ 'step', 'stopping_distance', 'Radar_distance', 'speed', 'steer', 'gps_x', 'gps_y', 'actual_x', 'actual_y', 'time', 'risk_time' ] dict = [{ 'step': self.step, 'stopping_distance': abs(stopping_distance), 'Radar_distance': tick_data['object_distance'], 'speed': speed, 'steer': steer, 'gps_x': tick_data['gps_x'], 'gps_y': tick_data['gps_y'], 'actual_x': tick_data['actual_x'], 'actual_y': tick_data['actual_y'], 'time': float(time.time() - self.time), 'risk_time': tick_data['time'] }] if (self.step == 0): self.braking_file = self.data_folder + "/emergency_braking%d.csv" % ( self.scene_number) file_exists = os.path.isfile(self.braking_file) with open(self.braking_file, 'a') as csvfile: # creating a csv dict writer object writer = csv.DictWriter(csvfile, fieldnames=fields) if not file_exists: writer.writeheader() writer.writerows(dict) if DEBUG: debug_display(tick_data, target_cam.squeeze(), points.cpu().squeeze(), steer, throttle, brake, desired_speed, self.step) return control
class ImageAgent(BaseAgent): def setup(self, path_to_conf_file): super().setup(path_to_conf_file) self.converter = Converter() self.net = ImageModel.load_from_checkpoint(path_to_conf_file) self.net.cuda() self.net.eval() # addition: modified from leaderboard/team_code/auto_pilot.py def save(self, steer, throttle, brake, tick_data): # frame = self.step // 10 frame = self.step pos = self._get_position(tick_data) far_command = tick_data['far_command'] speed = tick_data['speed'] center = os.path.join('rgb', ('%04d.png' % frame)) left = os.path.join('rgb_left', ('%04d.png' % frame)) right = os.path.join('rgb_right', ('%04d.png' % frame)) topdown = os.path.join('topdown', ('%04d.png' % frame)) rgb_with_car = os.path.join('rgb_with_car', ('%04d.png' % frame)) data_row = ','.join([str(i) for i in [frame, far_command, speed, steer, throttle, brake, str(center), str(left), str(right)]]) with (self.save_path / 'measurements.csv' ).open("a") as f_out: f_out.write(data_row+'\n') Image.fromarray(tick_data['rgb']).save(self.save_path / center) Image.fromarray(tick_data['rgb_left']).save(self.save_path / left) Image.fromarray(tick_data['rgb_right']).save(self.save_path / right) # addition Image.fromarray(COLOR[CONVERTER[tick_data['topdown']]]).save(self.save_path / topdown) Image.fromarray(tick_data['rgb_with_car']).save(self.save_path / rgb_with_car) ######################################################################## # log necessary info for action-based if self.args.save_action_based_measurements: from affordances import get_driving_affordances self._pedestrian_forbidden_distance = 10.0 self._pedestrian_max_detected_distance = 50.0 self._vehicle_forbidden_distance = 10.0 self._vehicle_max_detected_distance = 50.0 self._tl_forbidden_distance = 10.0 self._tl_max_detected_distance = 50.0 self._speed_detected_distance = 10.0 self._default_target_speed = 10 self._angle = None current_affordances = get_driving_affordances(self, self._pedestrian_forbidden_distance, self._pedestrian_max_detected_distance, self._vehicle_forbidden_distance, self._vehicle_max_detected_distance, self._tl_forbidden_distance, self._tl_max_detected_distance, self._angle_rad, self._default_target_speed, self._target_speed, self._speed_detected_distance, angle=True) is_vehicle_hazard = current_affordances['is_vehicle_hazard'] is_red_tl_hazard = current_affordances['is_red_tl_hazard'] is_pedestrian_hazard = current_affordances['is_pedestrian_hazard'] forward_speed = current_affordances['forward_speed'] relative_angle = current_affordances['relative_angle'] target_speed = current_affordances['target_speed'] closest_pedestrian_distance = current_affordances['closest_pedestrian_distance'] closest_vehicle_distance = current_affordances['closest_vehicle_distance'] closest_red_tl_distance = current_affordances['closest_red_tl_distance'] log_folder = str(self.save_path / 'affordance_measurements') if not os.path.exists(log_folder): os.mkdir(log_folder) log_path = os.path.join(log_folder, f'{self.step:06}.json') ego_transform = self._vehicle.get_transform() ego_location = ego_transform.location ego_rotation = ego_transform.rotation ego_velocity = self._vehicle.get_velocity() brake_noise = 0.0 throttle_noise = 0.0 # 1.0 -> 0.0 steer_noise = 0.0 # NaN -> 0.0 # class RoadOption map_roadoption_to_action_based_roadoption = {-1:2, 1:3, 2:4, 3:5, 4:2, 5:2, 6:2} # save info for action-based rep json_log_data = { "brake": float(brake), "closest_red_tl_distance": closest_red_tl_distance, "throttle": throttle, "directions": float(map_roadoption_to_action_based_roadoption[far_command.value]), "brake_noise": brake_noise, "is_red_tl_hazard": is_red_tl_hazard, "opponents": {}, "closest_pedestrian_distance": closest_pedestrian_distance, "is_pedestrian_hazard": is_pedestrian_hazard, "lane": {}, "is_vehicle_hazard": is_vehicle_hazard, "throttle_noise": throttle_noise, "ego_actor": { "velocity": [ ego_velocity.x, ego_velocity.y, ego_velocity.z ], "position": [ ego_location.x, ego_location.y, ego_location.z ], "orientation": [ ego_rotation.roll, ego_rotation.pitch, ego_rotation.yaw ] }, "hand_brake": False, "steer_noise": steer_noise, "reverse": False, "relative_angle": relative_angle, "closest_vehicle_distance": closest_vehicle_distance, "walkers": {}, "forward_speed": forward_speed, "steer": steer, "target_speed": target_speed } with open(log_path, 'w') as f_out: json.dump(json_log_data, f_out, indent=4) def _init(self): super()._init() self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) # addition: self._vehicle = CarlaDataProvider.get_hero_actor() self._world = self._vehicle.get_world() # ------------------------------------------------------- # add a local_planner in order to estimate relative angle # self.target_speed = 10 # args_lateral_dict = { # 'K_P': 1, # 'K_D': 0.4, # 'K_I': 0, # 'dt': 1.0/10.0} # self._local_planner = LocalPlanner( # self._vehicle, opt_dict={'target_speed' : self.target_speed, # 'lateral_control_dict':args_lateral_dict}) # self._hop_resolution = 2.0 # self._path_seperation_hop = 2 # self._path_seperation_threshold = 0.5 # self._grp = None # # self._map = CarlaDataProvider.get_map() # route = [(self._map.get_waypoint(x[0].location), x[1]) for x in self._global_plan_world_coord] # # self._local_planner.set_global_plan(route) def tick(self, input_data): result = super().tick(input_data) result['image'] = np.concatenate(tuple(result[x] for x in ['rgb', 'rgb_left', 'rgb_right']), -1) rgb_with_car = cv2.cvtColor(input_data['rgb_with_car'][1][:, :, :3], cv2.COLOR_BGR2RGB) result['rgb_with_car'] = rgb_with_car result['radar_central'] = input_data['radar_central'] theta = result['compass'] theta = 0.0 if np.isnan(theta) else theta theta = theta + np.pi / 2 R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) gps = self._get_position(result) # modification far_node, far_command = self._command_planner.run_step(gps) target = R.T.dot(far_node - gps) target *= 5.5 target += [128, 256] target = np.clip(target, 0, 256) result['target'] = target # addition: self._actors = self._world.get_actors() self._traffic_lights = get_nearby_lights(self._vehicle, self._actors.filter('*traffic_light*')) result['far_command'] = far_command topdown = input_data['map'][1][:, :, 2] topdown = draw_traffic_lights(topdown, self._vehicle, self._traffic_lights) result['topdown'] = topdown return result @torch.no_grad() def run_step_using_learned_controller(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) img = torchvision.transforms.functional.to_tensor(tick_data['image']) img = img[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() import random torch.manual_seed(2) torch.cuda.manual_seed_all(2) torch.backends.cudnn.deterministic = True np.random.seed(1) random.seed(1) device = torch.device('cuda') torch.backends.cudnn.benchmark = False points, (target_cam, _) = self.net.forward(img, target) control = self.net.controller(points).cpu().squeeze() steer = control[0].item() desired_speed = control[1].item() speed = tick_data['speed'] brake = desired_speed < 0.4 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) if DEBUG: debug_display( tick_data, target_cam.squeeze(), points.cpu().squeeze(), steer, throttle, brake, desired_speed, self.step) return control @torch.no_grad() def run_step(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) radar_data = tick_data['radar_central'][1] img = torchvision.transforms.functional.to_tensor(tick_data['image']) img = img[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() points, (target_cam, _) = self.net.forward(img, target) points_cam = points.clone().cpu() if self.step == 0: print('\n'*5) print('step :', self.step) # print('radar') # print(radar_data.shape) # print(radar_data) # print(np.max(radar_data, axis=0)) print('image', np.sum(tick_data['image'])) # print('img', torch.sum(img)) # print('target', target) # print('points', points) print('\n'*5) points_cam[..., 0] = (points_cam[..., 0] + 1) / 2 * img.shape[-1] points_cam[..., 1] = (points_cam[..., 1] + 1) / 2 * img.shape[-2] points_cam = points_cam.squeeze() points_world = self.converter.cam_to_world(points_cam).numpy() aim = (points_world[1] + points_world[0]) / 2.0 angle = np.degrees(np.pi / 2 - np.arctan2(aim[1], aim[0])) self._angle_rad = np.radians(angle) angle = angle / 90 steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) desired_speed = np.linalg.norm(points_world[0] - points_world[1]) * 2.0 # desired_speed *= (1 - abs(angle)) ** 2 self._target_speed = desired_speed speed = tick_data['speed'] brake = desired_speed < 0.4 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) if DEBUG: debug_display(tick_data, target_cam.squeeze(), points.cpu().squeeze(), steer, throttle, brake, desired_speed, self.step) # addition: from leaderboard/team_code/auto_pilot.py if self.step == 0: title_row = ','.join(['frame_id', 'far_command', 'speed', 'steering', 'throttle', 'brake', 'center', 'left', 'right']) with (self.save_path / 'measurements.csv' ).open("a") as f_out: f_out.write(title_row+'\n') if self.step % 1 == 0: self.gather_info((steer, throttle, float(brake), speed)) record_every_n_steps = self.record_every_n_step if self.step % record_every_n_steps == 0: self.save(steer, throttle, brake, tick_data) return control
class PrivilegedAgent(MapAgent): def setup(self, path_to_conf_file): # make conf file a json file? # store hparams for map model # can extend to store all of the save paths so we don't rely on os environ stuff # can copy this to the corresponding save path super().setup(path_to_conf_file) self.converter = Converter() self.net = MapModel.load_from_checkpoint(path_to_conf_file) self.net.cuda() self.net.eval() def _init(self): super()._init() self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) self.save_images_path = Path(f'{SAVE_PATH_BASE}/images/{ROUTE_NAME}') #self.save_path.mkdir() def tick(self, input_data): result = super().tick(input_data) result['image'] = np.concatenate( tuple(result[x] for x in ['rgb', 'rgb_left', 'rgb_right']), -1) theta = result['compass'] theta = 0.0 if np.isnan(theta) else theta theta = theta + np.pi / 2 result['theta'] = theta #print((theta * 180 / np.pi)%360) R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) result['R'] = R gps = self._get_position(result) # method returns position in meters # transform route waypoints to overhead map view route = self._command_planner.run_step(gps) # oriented in world frame nodes = np.array([node for node, _ in route]) # (N,2) nodes = nodes - gps # center at agent position and rotate nodes = R.T.dot(nodes.T) # (2,2) x (2,N) = (2,N) nodes = nodes.T * 5.5 # (N,2) # to map frame (5.5 pixels per meter) nodes += [128, 256] #nodes = np.clip(nodes, 0, 256) commands = [command for _, command in route] target = np.clip(nodes[1], 0, 256) # populate results result['num_waypoints'] = len(route) result['route_map'] = nodes result['commands'] = commands result['target'] = target return result @torch.no_grad() def run_step_using_learned_controller(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) img = torchvision.transforms.functional.to_tensor(tick_data['image']) img = img[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() #points, (target_cam, _) = self.net.forward(img, target) points, (target_cam, _) = self.net.forward(img, target) control = self.net.controller(points).cpu().squeeze() steer = control[0].item() desired_speed = control[1].item() speed = tick_data['speed'] brake = desired_speed < 0.4 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) if DEBUG: debug_display(tick_data, target_cam.squeeze(), points.cpu().squeeze(), steer, throttle, brake, desired_speed, self.step) return control @torch.no_grad() def run_step(self, input_data, timestamp): if not self.initialized: self._init() tick_data = self.tick(input_data) topdown = Image.fromarray(tick_data['topdown']) topdown = topdown.crop((128, 0, 128 + 256, 256)) topdown = np.array(topdown) topdown = preprocess_semantic(topdown) topdown = topdown[None].cuda() target = torch.from_numpy(tick_data['target']) target = target[None].cuda() #points, (target_cam, _) = self.net.forward(topdown, target) points = self.net.forward(topdown, target) # world frame points_map = points.clone().cpu().squeeze() # what's this conversion for? # was this originally normalized for training stability or something? points_map = points_map + 1 points_map = points_map / 2 * 256 points_map = np.clip(points_map, 0, 256) points_cam = self.converter.map_to_cam(points_map).numpy() points_world = self.converter.map_to_world(points_map).numpy() points_map = points_map.numpy() tick_data['points_map'] = points_map tick_data['points_cam'] = points_cam tick_data['points_world'] = points_world #img = tick_data['image'] aim = (points_world[1] + points_world[0]) / 2.0 angle = np.degrees(np.pi / 2 - np.arctan2(aim[1], aim[0])) / 90 steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) desired_speed = np.linalg.norm(points_world[0] - points_world[1]) * 2.0 tick_data['aim_world'] = aim speed = tick_data['speed'] brake = desired_speed < 0.4 or (speed / desired_speed) > 1.1 delta = np.clip(desired_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) throttle = throttle if not brake else 0.0 control = carla.VehicleControl() control.steer = steer control.throttle = throttle control.brake = float(brake) #print(timestamp) # GAMETIME if DEBUG or SAVE_IMAGES: # transform image model cam points to overhead BEV image (spectator frame?) self.debug_display(tick_data, steer, throttle, brake, desired_speed) return control def debug_display(self, tick_data, steer, throttle, brake, desired_speed, r=2): topdown = tick_data['topdown'] _topdown = Image.fromarray(COLOR[CONVERTER[topdown]]) _topdown_draw = ImageDraw.Draw(_topdown) # model points points_map = tick_data['points_map'] points_td = points_map + [128, 0] for i, (x, y) in enumerate(points_td): _topdown_draw.ellipse((x - 2 * r, y - 2 * r, x + 2 * r, y + 2 * r), (0, 191, 255)) route_map = tick_data['route_map'] route_td = route_map + [128, 0] # control point aim_world = np.array(tick_data['aim_world']) aim_map = self.converter.world_to_map(torch.Tensor(aim_world)).numpy() aim_map = aim_map + [128, 0] x, y = aim_map _topdown_draw.ellipse((x - 2, y - 2, x + 2, y + 2), (255, 105, 147)) # route waypoints for i, (x, y) in enumerate(route_td[:3]): if i == 0: color = (0, 255, 0) elif i == 1: color = (255, 0, 0) elif i == 2: color = (139, 0, 139) _topdown_draw.ellipse((x - 2 * r, y - 2 * r, x + 2 * r, y + 2 * r), color) # make RGB images # draw center RGB image _rgb = Image.fromarray(tick_data['rgb']) _draw_rgb = ImageDraw.Draw(_rgb) for x, y in tick_data['points_cam']: # image model waypoints #x = (x + 1)/2 * 256 #y = (y + 1)/2 * 144 _draw_rgb.ellipse((x - 2, y - 2, x + 2, y + 2), (0, 191, 255)) # transform aim from world to cam aim_cam = self.converter.world_to_cam(torch.Tensor(aim_world)).numpy() x, y = aim_cam _draw_rgb.ellipse((x - 2, y - 2, x + 2, y + 2), (255, 105, 147)) # draw route waypoints in RGB image route_map = np.array(tick_data['route_map']) route_map = np.clip(route_map, 0, 256) route_map = route_map[:3].squeeze() # just the next couple route_cam = self.converter.map_to_cam(torch.Tensor(route_map)).numpy() for i, (x, y) in enumerate(route_cam): if i == 0: # waypoint we just passed if not (0 < y < 143 and 0 < x < 255): continue color = (0, 255, 0) # green elif i == 1: # target color = (255, 0, 0) # red elif i == 2: # beyond target color = (139, 0, 139) # darkmagenta else: continue _draw_rgb.ellipse((x - 2, y - 2, x + 2, y + 2), color) _combined = Image.fromarray( np.hstack([tick_data['rgb_left'], _rgb, tick_data['rgb_right']])) _draw = ImageDraw.Draw(_combined) # draw debug text text_color = (139, 0, 139) #darkmagenta _draw.text((5, 10), 'Steer: %.3f' % steer, text_color) _draw.text((5, 30), 'Throttle: %.3f' % throttle, text_color) _draw.text((5, 50), 'Brake: %s' % brake, text_color) _draw.text((5, 70), 'Speed: %.3f' % tick_data['speed'], text_color) _draw.text((5, 90), 'Desired: %.3f' % desired_speed, text_color) cur_command, next_command = tick_data['commands'][:2] _draw.text((5, 110), f'Current: {cur_command}', text_color) _draw.text((5, 130), f'Next: {next_command}', text_color) _rgb_img = _combined.resize( (int(256 / _combined.size[1] * _combined.size[0]), 256)) _topdown = _topdown.resize((256, 256)) _save_img = Image.fromarray(np.hstack([_rgb_img, _topdown])) _save_img = cv2.cvtColor(np.array(_save_img), cv2.COLOR_BGR2RGB) if self.step % 10 == 0 and SAVE_IMAGES: frame_number = self.step // 10 rep_number = int(os.environ.get('REP', 0)) save_path = self.save_images_path / f'repetition_{rep_number:02d}' / f'{frame_number:06d}.png' cv2.imwrite(str(save_path), _save_img) if DEBUG: cv2.imshow('debug', _save_img) cv2.waitKey(1)
class AutoPilot(MapAgent): def setup(self, path_to_conf_file): super().setup(path_to_conf_file) self.save_path = None parent_folder = os.environ['SAVE_FOLDER'] string = pathlib.Path(os.environ['ROUTES']).stem self.save_path = pathlib.Path(parent_folder) / string def _init(self): super()._init() self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) def _get_angle_to(self, pos, theta, target): R = np.array([ [np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)], ]) aim = R.T.dot(target - pos) angle = -np.degrees(np.arctan2(-aim[1], aim[0])) angle = 0.0 if np.isnan(angle) else angle return angle def _get_control(self, target, far_target, tick_data, _draw): pos = self._get_position(tick_data) theta = tick_data['compass'] speed = tick_data['speed'] # Steering. angle_unnorm = self._get_angle_to(pos, theta, target) angle = angle_unnorm / 90 steer = self._turn_controller.step(angle) steer = np.clip(steer, -1.0, 1.0) steer = round(steer, 3) # Acceleration. angle_far_unnorm = self._get_angle_to(pos, theta, far_target) should_slow = abs(angle_far_unnorm) > 45.0 or abs(angle_unnorm) > 5.0 target_speed = 4 if should_slow else 7.0 brake = self._should_brake() target_speed = target_speed if not brake else 0.0 delta = np.clip(target_speed - speed, 0.0, 0.25) throttle = self._speed_controller.step(delta) throttle = np.clip(throttle, 0.0, 0.75) if brake: steer *= 0.5 throttle = 0.0 _draw.text((5, 90), 'Speed: %.3f' % speed) _draw.text((5, 110), 'Target: %.3f' % target_speed) _draw.text((5, 130), 'Angle: %.3f' % angle_unnorm) _draw.text((5, 150), 'Angle Far: %.3f' % angle_far_unnorm) return steer, throttle, brake, target_speed def run_step(self, input_data, timestamp): if not self.initialized: self._init() # self._world.set_weather(WEATHERS[int(os.environ['WEATHER_INDEX'])]) # if self.step % 100 == 0: # index = (self.step // 100) % len(WEATHERS) # self._world.set_weather(WEATHERS[index]) data = self.tick(input_data) rgb_with_car = cv2.cvtColor(input_data['rgb_with_car'][1][:, :, :3], cv2.COLOR_BGR2RGB) data['rgb_with_car'] = rgb_with_car topdown = data['topdown'] rgb = np.hstack((data['rgb_left'], data['rgb'], data['rgb_right'])) gps = self._get_position(data) near_node, near_command = self._waypoint_planner.run_step(gps) far_node, far_command = self._command_planner.run_step(gps) _topdown = Image.fromarray(COLOR[CONVERTER[topdown]]) _rgb = Image.fromarray(rgb) _draw = ImageDraw.Draw(_topdown) _topdown.thumbnail((256, 256)) _rgb = _rgb.resize((int(256 / _rgb.size[1] * _rgb.size[0]), 256)) _combined = Image.fromarray(np.hstack((_rgb, _topdown))) _draw = ImageDraw.Draw(_combined) steer, throttle, brake, target_speed = self._get_control( near_node, far_node, data, _draw) _draw.text((5, 10), 'FPS: %.3f' % (self.step / (time.time() - self.wall_start))) _draw.text((5, 30), 'Steer: %.3f' % steer) _draw.text((5, 50), 'Throttle: %.3f' % throttle) _draw.text((5, 70), 'Brake: %s' % brake) if HAS_DISPLAY: cv2.imshow('map', cv2.cvtColor(np.array(_combined), cv2.COLOR_BGR2RGB)) cv2.waitKey(1) control = carla.VehicleControl() control.steer = steer + 1e-2 * np.random.randn() control.throttle = throttle control.brake = float(brake) # we only gether info every 2 frames for faster processing speed if self.step % 2 == 0: self.gather_info() # if this number is very small, we may not have the exact numbers and images for the event happening (e.g. the frame when a collision happen). However, this is usually ok if we only use these for retraining purpose record_every_n_steps = 3 if self.step % record_every_n_steps == 0: self.save(record_every_n_steps, far_command, steer, throttle, brake, target_speed, data) self.save_json(record_every_n_steps, far_node, near_command, steer, throttle, brake, target_speed, data) return control def save_json(self, record_every_n_steps, far_node, near_command, steer, throttle, brake, target_speed, tick_data): frame = int(self.step // record_every_n_steps) pos = self._get_position(tick_data) theta = tick_data['compass'] speed = tick_data['speed'] # pos, , far_node, near_command data = { 'x': pos[0], 'y': pos[1], 'theta': theta, 'speed': speed, 'target_speed': target_speed, 'x_command': far_node[0], 'y_command': far_node[1], 'command': near_command.value, 'steer': steer, 'throttle': throttle, 'brake': brake, } pth = self.save_path / 'measurements' pth.mkdir(parents=False, exist_ok=True) (pth / ('%04d.json' % frame)).write_text(str(data)) def save(self, record_every_n_steps, far_command, steer, throttle, brake, target_speed, tick_data): frame = int(self.step // record_every_n_steps) speed = tick_data['speed'] string = os.environ['SAVE_FOLDER'] + '/' + pathlib.Path( os.environ['ROUTES']).stem center_str = string + '/' + 'rgb' + '/' + ('%04d.png' % frame) left_str = string + '/' + 'rgb_left' + '/' + ('%04d.png' % frame) right_str = string + '/' + 'rgb_right' + '/' + ('%04d.png' % frame) # topdown_str = string + '/' + 'topdown' + '/' + ('%04d.png' % frame) center = self.save_path / 'rgb' / ('%04d.png' % frame) left = self.save_path / 'rgb_left' / ('%04d.png' % frame) right = self.save_path / 'rgb_right' / ('%04d.png' % frame) topdown = self.save_path / 'topdown' / ('%04d.png' % frame) rgb_with_car = self.save_path / 'rgb_with_car' / ('%04d.png' % frame) data_row = ','.join([ str(i) for i in [ frame, far_command, speed, steer, throttle, brake, center_str, left_str, right_str ] ]) with (self.save_path / 'measurements.csv').open("a") as f_out: f_out.write(data_row + '\n') Image.fromarray(tick_data['rgb']).save(center) Image.fromarray(tick_data['rgb_left']).save(left) Image.fromarray(tick_data['rgb_right']).save(right) # modification # Image.fromarray(COLOR[CONVERTER[tick_data['topdown']]]).save(topdown) Image.fromarray(tick_data['topdown']).save(topdown) Image.fromarray(tick_data['rgb_with_car']).save(rgb_with_car) def _should_brake(self): actors = self._world.get_actors() vehicle = self._is_vehicle_hazard(actors.filter('*vehicle*')) light = self._is_light_red(actors.filter('*traffic_light*')) walker = self._is_walker_hazard(actors.filter('*walker*')) return any(x is not None for x in [vehicle, light, walker]) def _draw_line(self, p, v, z, color=(255, 0, 0)): if not DEBUG: return p1 = _location(p[0], p[1], z) p2 = _location(p[0] + v[0], p[1] + v[1], z) color = carla.Color(*color) self._world.debug.draw_line(p1, p2, 0.25, color, 0.01) def _is_light_red(self, lights_list): if self._vehicle.get_traffic_light_state( ) != carla.libcarla.TrafficLightState.Green: affecting = self._vehicle.get_traffic_light() for light in self._traffic_lights: if light.id == affecting.id: return affecting return None def _is_walker_hazard(self, walkers_list): z = self._vehicle.get_location().z p1 = _numpy(self._vehicle.get_location()) v1 = 10.0 * _orientation(self._vehicle.get_transform().rotation.yaw) self._draw_line(p1, v1, z + 2.5, (0, 0, 255)) for walker in walkers_list: v2_hat = _orientation(walker.get_transform().rotation.yaw) s2 = np.linalg.norm(_numpy(walker.get_velocity())) if s2 < 0.05: v2_hat *= s2 p2 = -3.0 * v2_hat + _numpy(walker.get_location()) v2 = 8.0 * v2_hat self._draw_line(p2, v2, z + 2.5) collides, collision_point = get_collision(p1, v1, p2, v2) if collides: return walker return None def _is_vehicle_hazard(self, vehicle_list): z = self._vehicle.get_location().z o1 = _orientation(self._vehicle.get_transform().rotation.yaw) p1 = _numpy(self._vehicle.get_location()) s1 = max(7.5, 2.0 * np.linalg.norm(_numpy(self._vehicle.get_velocity()))) v1_hat = o1 v1 = s1 * v1_hat self._draw_line(p1, v1, z + 2.5, (255, 0, 0)) for target_vehicle in vehicle_list: if target_vehicle.id == self._vehicle.id: continue o2 = _orientation(target_vehicle.get_transform().rotation.yaw) p2 = _numpy(target_vehicle.get_location()) s2 = max( 5.0, 2.0 * np.linalg.norm(_numpy(target_vehicle.get_velocity()))) v2_hat = o2 v2 = s2 * v2_hat p2_p1 = p2 - p1 distance = np.linalg.norm(p2_p1) p2_p1_hat = p2_p1 / (distance + 1e-4) self._draw_line(p2, v2, z + 2.5, (255, 0, 0)) angle_to_car = np.degrees(np.arccos(v1_hat.dot(p2_p1_hat))) angle_between_heading = np.degrees(np.arccos(o1.dot(o2))) if angle_between_heading > 60.0 and not (angle_to_car < 15 and distance < s1): continue elif angle_to_car > 30.0: continue elif distance > s1: continue return target_vehicle return None