class CoILBaseline(AutonomousAgent): def setup(self, path_to_config_file): yaml_conf, checkpoint_number = checkpoint_parse_configuration_file(path_to_config_file) # Take the checkpoint name and load it checkpoint = torch.load(os.path.join(os.sep, os.path.join(*os.path.realpath(__file__).split(os.sep)[:-2]), '_logs', yaml_conf.split(os.sep)[-2], yaml_conf.split('/')[-1].split('.')[-2] , 'checkpoints', str(checkpoint_number) + '.pth')) # do the merge here merge_with_yaml(os.path.join(os.sep, os.path.join(*os.path.realpath(__file__).split(os.sep)[:-2]), yaml_conf)) self.checkpoint = checkpoint # We save the checkpoint for some interesting future use. self._model = CoILModel(g_conf.MODEL_TYPE, g_conf.MODEL_CONFIGURATION) self.first_iter = True logging.info("Setup Model") # Load the model and prepare set it for evaluation self._model.load_state_dict(checkpoint['state_dict']) self._model.cuda() self._model.eval() self.latest_image = None self.latest_image_tensor = None # We add more time to the curve commands self._expand_command_front = 5 self._expand_command_back = 3 self.track = 2 # Track.CAMERAS def sensors(self): sensors = [{'type': 'sensor.camera.rgb', 'x': 2.0, 'y': 0.0, 'z': 1.40, 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, 'width': 800, 'height': 600, 'fov': 100, 'id': 'rgb'}, {'type': 'sensor.can_bus', 'reading_frequency': 25, 'id': 'can_bus' }, {'type': 'sensor.other.gnss', 'x': 0.7, 'y': -0.4, 'z': 1.60, 'id': 'GPS'} ] return sensors def run_step(self, input_data, timestamp): # Get the current directions for following the route directions = self._get_current_direction(input_data['GPS'][1]) logging.debug("Directions {}".format(directions)) # Take the forward speed and normalize it for it to go from 0-1 norm_speed = input_data['can_bus'][1]['speed'] / g_conf.SPEED_FACTOR norm_speed = torch.cuda.FloatTensor([norm_speed]).unsqueeze(0) directions_tensor = torch.cuda.LongTensor([directions]) # Compute the forward pass processing the sensors got from CARLA. model_outputs = self._model.forward_branch(self._process_sensors(input_data['rgb'][1]), norm_speed, directions_tensor) steer, throttle, brake = self._process_model_outputs(model_outputs[0]) control = carla.VehicleControl() control.steer = float(steer) control.throttle = float(throttle) control.brake = float(brake) logging.debug("Output ", control) # There is the posibility to replace some of the predictions with oracle predictions. self.first_iter = False return control def get_attentions(self, layers=None): """ Returns The activations obtained from the first layers of the latest iteration. """ if layers is None: layers = [0, 1, 2] if self.latest_image_tensor is None: raise ValueError('No step was ran yet. ' 'No image to compute the activations, Try Running ') all_layers = self._model.get_perception_layers(self.latest_image_tensor) cmap = plt.get_cmap('inferno') attentions = [] for layer in layers: y = all_layers[layer] att = torch.abs(y).mean(1)[0].data.cpu().numpy() att = att / att.max() att = cmap(att) att = np.delete(att, 3, 2) attentions.append(scipy.misc.imresize(att, [88, 200])) return attentions def _process_sensors(self, sensor): sensor = sensor[:, :, 0:3] # BGRA->BRG drop alpha channel sensor = sensor[:, :, ::-1] # BGR->RGB # sensor = sensor[g_conf.IMAGE_CUT[0]:g_conf.IMAGE_CUT[1], :, :] # crop # TODO: don't cut sensor = scipy.misc.imresize(sensor, (g_conf.SENSORS['rgb'][1], g_conf.SENSORS['rgb'][2])) self.latest_image = sensor sensor = np.swapaxes(sensor, 0, 1) sensor = np.transpose(sensor, (2, 1, 0)) sensor = torch.from_numpy(sensor / 255.0).type(torch.FloatTensor).cuda() image_input = sensor.unsqueeze(0) self.latest_image_tensor = image_input return image_input def _get_current_direction(self, vehicle_position): # for the current position and orientation try to get the closest one from the waypoints closest_id = 0 min_distance = 100000 for index in range(len(self._global_plan)): waypoint = self._global_plan[index][0] computed_distance = distance_vehicle(waypoint, vehicle_position) if computed_distance < min_distance: min_distance = computed_distance closest_id = index print(f'Closest waypoint {closest_id} dist {min_distance}') direction = self._global_plan[closest_id][1] print("Direction ", direction) if direction == RoadOption.LEFT: direction = 3.0 elif direction == RoadOption.RIGHT: direction = 4.0 elif direction == RoadOption.STRAIGHT: direction = 5.0 else: direction = 2.0 return direction @staticmethod def _process_model_outputs(outputs): """ A bit of heuristics in the control, to eventually make car faster, for instance. Returns: """ steer, throttle, brake = outputs[0], outputs[1], outputs[2] if brake < 0.05: brake = 0.0 if throttle > brake: brake = 0.0 return steer, throttle, brake def _expand_commands(self, topological_plan): """ The idea is to make the intersection indications to last longer""" # O(2*N) algorithm , probably it is possible to do in O(N) with queues. # Get the index where curves start and end curves_start_end = [] inside = False start = -1 current_curve = RoadOption.LANEFOLLOW for index in range(len(topological_plan)): command = topological_plan[index][1] if command != RoadOption.LANEFOLLOW and not inside: inside = True start = index current_curve = command if command == RoadOption.LANEFOLLOW and inside: inside = False # End now is the index. curves_start_end.append([start, index, current_curve]) if start == -1: raise ValueError("End of curve without start") start = -1 for start_end_index_command in curves_start_end: start_index = start_end_index_command[0] end_index = start_end_index_command[1] command = start_end_index_command[2] # Add the backwards curves ( Before the begginning) for index in range(1, self._expand_command_front + 1): changed_index = start_index - index if changed_index > 0: topological_plan[changed_index] = (topological_plan[changed_index][0], command) # add the onnes after the end for index in range(0, self._expand_command_back): changed_index = end_index + index if changed_index < len(topological_plan): topological_plan[changed_index] = (topological_plan[changed_index][0], command) return topological_plan
class CoILAgent(object): def __init__(self, checkpoint, town_name, carla_version='0.84', vae_params=None): # Set the carla version that is going to be used by the interface self._carla_version = carla_version self.checkpoint = checkpoint # We save the checkpoint for some interesting future use. self._model = CoILModel(g_conf.MODEL_TYPE, g_conf.MODEL_CONFIGURATION) self.first_iter = True # Load the model and prepare set it for evaluation self._model.load_state_dict(checkpoint['state_dict']) self._model.cuda() self._model.eval() self._vae_params = vae_params if g_conf.VAE_MODEL_CONFIGURATION != {}: # adding VAE model self._VAE_model = CoILModel('VAE', g_conf.VAE_MODEL_CONFIGURATION) self._VAE_model.cuda() VAE_checkpoint = torch.load( os.path.join('_logs', vae_params['vae_folder'], vae_params['vae_exp'], 'checkpoints', str(vae_params['vae_checkpoint']) + '.pth')) print( "VAE model ", str(vae_params['vae_checkpoint']), " already loaded from ", os.path.join('_logs', vae_params['vae_folder'], vae_params['vae_exp'], 'checkpoints')) self._VAE_model.load_state_dict(VAE_checkpoint['state_dict']) self._VAE_model.eval() self.latest_image = None self.latest_image_tensor = None if g_conf.USE_ORACLE or g_conf.USE_FULL_ORACLE: self.control_agent = CommandFollower(town_name) def run_step(self, measurements, sensor_data, directions, target): """ Run a step on the benchmark simulation Args: measurements: All the float measurements from CARLA ( Just speed is used) sensor_data: All the sensor data used on this benchmark directions: The directions, high level commands target: Final objective. Not used when the agent is predicting all outputs. Returns: Controls for the vehicle on the CARLA simulator. """ # Take the forward speed and normalize it for it to go from 0-1 norm_speed = measurements.player_measurements.forward_speed / g_conf.SPEED_FACTOR norm_speed = torch.cuda.FloatTensor([norm_speed]).unsqueeze(0) directions_tensor = torch.cuda.LongTensor([directions]) # Compute the forward pass processing the sensors got from CARLA. if g_conf.VAE_MODEL_CONFIGURATION != {}: input_data = self._process_sensors(sensor_data) _, _, _, z = self._VAE_model(input_data) model_outputs = self._model.forward_branch(z, norm_speed, directions_tensor) else: model_outputs = self._model.forward_branch( self._process_sensors(sensor_data), norm_speed, directions_tensor) steer, throttle, brake = self._process_model_outputs(model_outputs[0]) if self._carla_version == '0.9': import carla control = carla.VehicleControl() else: control = VehicleControl() control.steer = float(steer) control.throttle = float(throttle) control.brake = float(brake) # There is the posibility to replace some of the predictions with oracle predictions. if g_conf.USE_ORACLE: _, control.throttle, control.brake = self._get_oracle_prediction( measurements, target) if self.first_iter: coil_logger.add_message('Iterating', { "Checkpoint": self.checkpoint['iteration'], 'Agent': str(steer) }, self.checkpoint['iteration']) self.first_iter = False return control def get_attentions(self, layers=None): """ Returns The activations obtained from the first layers of the latest iteration. """ if layers is None: layers = [0, 1, 2] if self.latest_image_tensor is None: raise ValueError( 'No step was ran yet. ' 'No image to compute the activations, Try Running ') all_layers = self._model.get_perception_layers( self.latest_image_tensor) cmap = plt.get_cmap('inferno') attentions = [] for layer in layers: y = all_layers[layer] att = torch.abs(y).mean(1)[0].data.cpu().numpy() att = att / att.max() att = cmap(att) att = np.delete(att, 3, 2) attentions.append(imresize(att, [88, 200])) return attentions def _process_sensors(self, sensors): iteration = 0 for name, size in g_conf.SENSORS.items(): if self._carla_version == '0.9': sensor = sensors[name][g_conf.IMAGE_CUT[0]:g_conf.IMAGE_CUT[1], ...] else: sensor = sensors[name].data[ g_conf.IMAGE_CUT[0]:g_conf.IMAGE_CUT[1], ...] sensor = scipy.misc.imresize(sensor, (size[1], size[2])) self.latest_image = sensor sensor = np.swapaxes(sensor, 0, 1) sensor = np.transpose(sensor, (2, 1, 0)) sensor = torch.from_numpy(sensor / 255.0).type( torch.FloatTensor).cuda() if iteration == 0: image_input = sensor else: image_input = torch.cat((image_input, sensor), 0) iteration += 1 image_input = image_input.unsqueeze(0) self.latest_image_tensor = image_input return image_input def _process_model_outputs(self, outputs): """ A bit of heuristics in the control, to eventually make car faster, for instance. Returns: """ steer, throttle, brake = outputs[0], outputs[1], outputs[2] if brake < 0.05: brake = 0.0 if throttle > brake: brake = 0.0 return steer, throttle, brake def _process_model_outputs_wp(self, outputs): """ A bit of heuristics in the control, to eventually make car faster, for instance. Returns: """ wpa1, wpa2, throttle, brake = outputs[3], outputs[4], outputs[ 1], outputs[2] if brake < 0.2: brake = 0.0 if throttle > brake: brake = 0.0 steer = 0.7 * wpa2 if steer > 0: steer = min(steer, 1) else: steer = max(steer, -1) return steer, throttle, brake def _get_oracle_prediction(self, measurements, target): # For the oracle, the current version of sensor data is not really relevant. control, _, _, _, _ = self.control_agent.run_step( measurements, [], [], target) return control.steer, control.throttle, control.brake
class CoILAgent(object): def __init__(self, checkpoint, town_name, carla_version='0.84'): # Set the carla version that is going to be used by the interface self._carla_version = carla_version self.checkpoint = checkpoint # We save the checkpoint for some interesting future use. self._model = CoILModel(g_conf.MODEL_TYPE, g_conf.MODEL_CONFIGURATION) self.first_iter = True # Load the model and prepare set it for evaluation self._model.load_state_dict(checkpoint['state_dict']) self._model.cuda() self._model.eval() # this entire segment is for loading models for ensemble evaluation - take care for the paths and checkpoints ''' self.weights = [0.25, 0.25, 0.25, 0.25] # simple ensemble self.model_ids = ['660000', '670000', '1070000', '2640000'] # model checkpoints self.models_dir = '/is/sg2/aprakash/Projects/carla_autonomous_driving/code/coiltraine/_logs/ensemble' self._ensemble_model_list = [] for i in range(len(self.model_ids)): curr_checkpoint = torch.load(self.models_dir+'/resnet34imnet10S1/checkpoints/'+self.model_ids[i]+'.pth') self._ensemble_model_list.append(CoILModel(g_conf.MODEL_TYPE, g_conf.MODEL_CONFIGURATION)) self._ensemble_model_list[i].load_state_dict(curr_checkpoint['state_dict']) self._ensemble_model_list[i].cuda().eval() ''' self.latest_image = None self.latest_image_tensor = None # for image corruptions self.corruption_number = None self.severity = None if g_conf.USE_ORACLE or g_conf.USE_FULL_ORACLE: # for evaluating expert self.control_agent = CommandFollower(town_name) def run_step(self, measurements, sensor_data, directions, target, **kwargs): """ Run a step on the benchmark simulation Args: measurements: All the float measurements from CARLA ( Just speed is used) sensor_data: All the sensor data used on this benchmark directions: The directions, high level commands target: Final objective. Not used when the agent is predicting all outputs. Returns: Controls for the vehicle on the CARLA simulator. """ # only required if using corruptions module # self.corruption_number = kwargs.get('corruption_number', None) # self.severity = kwargs.get('severity', None) # Take the forward speed and normalize it for it to go from 0-1 norm_speed = measurements.player_measurements.forward_speed / g_conf.SPEED_FACTOR norm_speed = torch.cuda.FloatTensor([norm_speed]).unsqueeze(0) directions_tensor = torch.cuda.LongTensor([directions]) # Compute the forward pass processing the sensors got from CARLA. model_outputs = self._model.forward_branch(self._process_sensors(sensor_data), norm_speed, directions_tensor) # run forward pass using felipe model # model_outputs_felipe = self._model_felipe.forward_branch(self._process_sensors(sensor_data), norm_speed, # directions_tensor) # model_outputs[0] = torch.FloatTensor([(model_outputs[0][i].item()+model_outputs_felipe[0][i].item())/2.0 for i in range(3)]).cuda() steer, throttle, brake = self._process_model_outputs(model_outputs[0]) # steer_f, throttle_f, brake_f = self._process_model_outputs(model_outputs_felipe[0]) # ensemble ''' steer_c = [] throttle_c = [] brake_c = [] for i in range(len(self.model_ids)): mo = self._ensemble_model_list[i].forward_branch(self._process_sensors(sensor_data), norm_speed, directions_tensor) s, t, b = self._process_model_outputs(mo[0]) steer_c.append(s) throttle_c.append(t) brake_c.append(b) ''' if self._carla_version == '0.9': import carla control = carla.VehicleControl() else: control = VehicleControl() # single model control.steer = float(steer) control.throttle = float(throttle) control.brake = float(brake) # ensemble # control.steer = float(np.average(steer_c, weights=self.weights)) # control.throttle = float(np.average(throttle_c, weights=self.weights)) # control.brake = float(np.average(brake_c, weights=self.weights)) # There is the posibility to replace some of the predictions with oracle predictions. if g_conf.USE_ORACLE: control.steer, control.throttle, control.brake = self._get_oracle_prediction( measurements, sensor_data, target) if self.first_iter: coil_logger.add_message('Iterating', {"Checkpoint": self.checkpoint['iteration'], 'Agent': str(control.steer)}, self.checkpoint['iteration']) self.first_iter = False return control # define run step for carla 9 def run_step_carla9(self, observations): norm_speed = np.linalg.norm(observations['velocity'])/g_conf.SPEED_FACTOR norm_speed = torch.cuda.FloatTensor([norm_speed]).unsqueeze(0) directions_tensor = torch.cuda.LongTensor([int(observations['command'])]) # print ('rgb: ', observations['big_cam'].shape) # print ('velocity: ', observations['velocity']) # print ('norm velocity: ', np.linalg.norm(observations['velocity'])) # print ('norm_speed: ', norm_speed.shape, norm_speed.item()) # print ('directions_tensor: ', directions_tensor.shape, directions_tensor.item()) model_outputs = self._model.forward_branch(self._process_sensors(observations), norm_speed, directions_tensor) steer, throttle, brake = self._process_model_outputs(model_outputs[0]) if self._carla_version == '0.9': import carla control = carla.VehicleControl() else: control = VehicleControl() # single model control.steer = float(steer) control.throttle = float(throttle) control.brake = float(brake) return control def get_attentions(self, layers=None): """ Returns The activations obtained from the first layers of the latest iteration. """ if layers is None: layers = [0, 1, 2] if self.latest_image_tensor is None: raise ValueError('No step was ran yet. ' 'No image to compute the activations, Try Running ') all_layers = self._model.get_perception_layers(self.latest_image_tensor) cmap = plt.get_cmap('inferno') attentions = [] for layer in layers: y = all_layers[layer] att = torch.abs(y).mean(1)[0].data.cpu().numpy() att = att / att.max() att = cmap(att) att = np.delete(att, 3, 2) attentions.append(imresize(att, [88, 200])) # attentions.append(np.array(Image.fromarray(sensor).resize((200, 88)))) return attentions def _process_sensors(self, sensors): iteration = 0 for name, size in g_conf.SENSORS.items(): if self._carla_version == '0.9': sensor = sensors[name][g_conf.IMAGE_CUT[0]:g_conf.IMAGE_CUT[1], ...] else: sensor = sensors[name].data[g_conf.IMAGE_CUT[0]:g_conf.IMAGE_CUT[1], ...] sensor = scipy.misc.imresize(sensor, (size[1], size[2])) # depreciated # sensor = np.array(Image.fromarray(sensor).resize((size[2], size[1]))) # for running corruptions ''' # corrupt the image here # print ('out of corruption: ', self.corruption_number, self.severity) if self.corruption_number is not None and self.severity is not None: # print ('in corruption: ', self.corruption_number, self.severity) sensor = corrupt(sensor, corruption_number=self.corruption_number, severity=self.severity+1) ''' self.latest_image = sensor sensor = np.swapaxes(sensor, 0, 1) sensor = np.transpose(sensor, (2, 1, 0)) sensor = torch.from_numpy(sensor / 255.0).type(torch.FloatTensor).cuda() if iteration == 0: image_input = sensor else: image_input = torch.cat((image_input, sensor), 0) iteration += 1 image_input = image_input.unsqueeze(0) self.latest_image_tensor = image_input return image_input def _process_model_outputs(self, outputs): """ A bit of heuristics in the control, to eventually make car faster, for instance. Returns: """ steer, throttle, brake = outputs[0].item(), outputs[1].item(), outputs[2].item() # print ('steer: ', steer, 'throttle: ', throttle, 'brake: ', brake) # these heuristics are a part of the original benchmark, evaluation doesn't run properly without these if brake < 0.05: brake = 0.0 if throttle > brake: brake = 0.0 # print ('steer after heuristic: ', steer, 'throttle after heuristic: ', throttle, 'brake after heuristic: ', brake) return steer, throttle, brake def _process_model_outputs_wp(self, outputs): """ A bit of heuristics in the control, to eventually make car faster, for instance. Returns: """ wpa1, wpa2, throttle, brake = outputs[3], outputs[4], outputs[1], outputs[2] if brake < 0.2: brake = 0.0 if throttle > brake: brake = 0.0 steer = 0.7 * wpa2 if steer > 0: steer = min(steer, 1) else: steer = max(steer, -1) return steer, throttle, brake def _get_oracle_prediction(self, measurements, sensor_data, target): # For the oracle, the current version of sensor data is not really relevant. control, _ = self.control_agent.run_step(measurements, sensor_data, [], target) return control.steer, control.throttle, control.brake
class CoILBaselineCEXP(Agent): def setup(self, path_to_config_file): yaml_conf, checkpoint_number, agent_name, encoder_params = checkpoint_parse_configuration_file( path_to_config_file) # Take the checkpoint name and load it if encoder_params is not None: self.checkpoint = torch.load( os.path.join( '/', os.path.join(*os.path.realpath(__file__).split('/')[:-2]), '_logs', yaml_conf.split('/')[-2], yaml_conf.split('/')[-1].split('.')[-2] + '_' + str(encoder_params['encoder_checkpoint']), 'checkpoints', str(checkpoint_number) + '.pth')) # Once the ENCODER_MODEL_CONFIGURATION was defined, we use the pre-trained encoder model to extract bottleneck Z and drive the E-t-E agent self.encoder_checkpoint = torch.load( os.path.join( '/', os.path.join(*os.path.realpath(__file__).split('/')[:-2]), '_logs', encoder_params['encoder_folder'], encoder_params['encoder_exp'], 'checkpoints', str(encoder_params['encoder_checkpoint']) + '.pth')) self.encoder_model = CoILModel(g_conf.ENCODER_MODEL_TYPE, g_conf.ENCODER_MODEL_CONFIGURATION) self.encoder_model.load_state_dict( self.encoder_checkpoint['state_dict']) self.encoder_model.cuda() self.encoder_model.eval() else: self.checkpoint = torch.load( os.path.join( '/', os.path.join(*os.path.realpath(__file__).split('/')[:-2]), '_logs', yaml_conf.split('/')[-2], yaml_conf.split('/')[-1].split('.')[-2], 'checkpoints', str(checkpoint_number) + '.pth')) # do the merge here # TODO THE MERGE IS REQUIRED DEPENDING ON THE SITUATION g_conf.immutable(False) merge_with_yaml( os.path.join( '/', os.path.join(*os.path.realpath(__file__).split('/')[:-2]), yaml_conf), encoder_params) self._model = CoILModel(g_conf.MODEL_TYPE, g_conf.MODEL_CONFIGURATION, g_conf.ENCODER_MODEL_CONFIGURATION) self.first_iter = True logging.info("Setup Model") # Load the model and prepare set it for evaluation self._model.load_state_dict(self.checkpoint['state_dict']) self._model.cuda() self._model.eval() self.latest_image = None self.latest_image_tensor = None # We add more time to the curve commands self._expand_command_front = 5 self._expand_command_back = 3 # TODO: Merge with Felipe's code self._msn = None self._lat_ref = 0 self._lon_ref = 0 # Check the agent name self._name = agent_name self.count = 0 def sensors(self): sensors = [{ 'type': 'sensor.camera.rgb', 'x': 2.0, 'y': 0.0, 'z': 1.40, 'roll': 0.0, 'pitch': -15.0, 'yaw': 0.0, 'width': 800, 'height': 600, 'fov': 100, 'id': 'rgb' }, { 'type': 'sensor.can_bus', 'reading_frequency': 25, 'id': 'can_bus' }, { 'type': 'sensor.other.gnss', 'x': 0.7, 'y': -0.4, 'z': 1.60, 'id': 'GPS' }] return sensors """ def make_state(self, exp): # state is divided in three parts, the speed, the angle_error, the high level command # Get the closest waypoint #waypoint, _ = self._get_current_wp_direction(exp._ego_actor.get_transform().location, exp._route) #norm, angle = compute_magnitude_angle(waypoint.location, exp._ego_actor.get_transform().location, # exp._ego_actor.get_transform().rotation.yaw) #return np.array([_get_forward_speed(exp._ego_actor) / 12.0, # Normalize to by dividing by 12 # angle / 180.0]) self._global_plan = exp._route input_data = exp._sensor_interface.get_data() # TODO this should be capilarized #if 'scenario' in g_conf.MEASUREMENTS_INPUTS: # scenario = convert_scenario_name_number(exp._environment_data['exp_measurements']) # input_data.update({'scenario': scenario}) return input_data """ def make_state(self, exp): """ This function also do the necessary processing of the state for the run step function :param exp: :return: """ self._global_plan = exp._route # we also need to get the latitute longitude ref # TODO this needs to be adpated for a CARLA challenge submission self._lat_ref = exp._lat_ref self._lon_ref = exp._lon_ref input_data = exp._sensor_interface.get_data() self._vehicle_pos = exp._ego_actor.get_transform().location # TODO this should be capilarized input_data.update( {'sensor_input': self._process_sensors(input_data['rgb'][1])}) if self._msn is not None: input_data.update( {'scenario': self._msn(input_data['sensor_input'])}) #if 'scenario' in g_conf.MEASUREMENTS_INPUTS: # scenario = convert_scenario_name_number(exp._environment_data['exp_measurements']) # print (" SCENARIO NUMBER ", scenario) # input_data.update({'scenario': scenario}) return input_data def run_step(self, input_data): # Get the current directions for following the route directions = self._get_current_direction(self._vehicle_pos) logging.debug(" Current direction %f ", directions) # Take the forward speed and normalize it for it to go from 0-1 network_input = input_data['can_bus'][1]['speed'] / g_conf.SPEED_FACTOR network_input = torch.cuda.FloatTensor([network_input]).unsqueeze(0) # TODO remove ifs #if 'scenario' in g_conf.MEASUREMENTS_INPUTS: # network_input = torch.cat((torch.cuda.FloatTensor([input_data['scenario']]), # network_input), 1) # Compute the forward pass processing the sensors got from CARLA. # TODO we start with an if but we can build a class hierarquical ! if g_conf.MODEL_TYPE in [ 'coil-icra', 'coil-icra-KLD', 'separate-supervised' ]: directions_tensor = torch.cuda.LongTensor([directions]) #print(" Directions ", int(directions)) if False: save_path = os.path.join('temp', 'ete_baseline') if not os.path.exists(save_path): os.mkdir(save_path) save_image( input_data['sensor_input'], os.path.join( save_path, 'run_input_' + str(self.count).zfill(5) + ".png")) self.count += 1 model_outputs = self._model.forward_branch( input_data['sensor_input'], network_input, directions_tensor) elif g_conf.MODEL_TYPE in ['coil-icra-VAE']: directions_tensor = torch.cuda.LongTensor([directions]) if g_conf.ENCODER_MODEL_TYPE in ['VAE']: if g_conf.LABELS_SUPERVISED: input = torch.cat( (input_data['sensor_input'], torch.zeros( 1, 1, 88, 200).cuda()), dim=1) recon_x, mu, _, z = self.encoder_model(input) else: recon_x, mu, _, z = self.encoder_model( input_data['sensor_input']) elif g_conf.ENCODER_MODEL_TYPE in ['Affordances']: mu, _ = self.encoder_model(input_data['sensor_input']) if False: save_path = os.path.join('temp', 'affordances_upperbound') if not os.path.exists(save_path): os.mkdir(save_path) if g_conf.LABELS_SUPERVISED: save_image( input_data['sensor_input'], os.path.join( save_path, 'run_input_' + str(self.count).zfill(5) + ".png")) split = torch.split(torch.squeeze(recon_x, dim=1), [3, 1], dim=1) save_image( split[0], os.path.join( save_path, 'run_recon_rgb_' + str(self.count).zfill(5) + ".png")) save_image( split[1], os.path.join( save_path, 'run_recon_labels_' + str(self.count).zfill(5) + ".png")) else: save_image( input_data['sensor_input'], os.path.join( save_path, 'run_input_' + str(self.count).zfill(5) + ".png")) #save_image(recon_x, os.path.join(save_path, 'run_recon_' + str(self.count).zfill(5) + ".png")) self.count += 1 model_outputs = self._model.forward_branch(mu, network_input, directions_tensor) #print(' frame', self.count) #print(' direction', directions_tensor) #print(' branch output', model_outputs) elif g_conf.MODEL_TYPE in [ 'separate-supervised-NoSpeed', 'coil-icra-NoSpeed' ]: directions_tensor = torch.cuda.LongTensor([directions]) if False: save_path = os.path.join('temp', 'ETE_resnet34_6') if not os.path.exists(save_path): os.mkdir(save_path) save_image( input_data['sensor_input'], os.path.join( save_path, 'run_input_' + str(self.count).zfill(5) + ".png")) self.count += 1 model_outputs = self._model.forward_branch( input_data['sensor_input'], directions_tensor) else: directions_tensor = torch.cuda.FloatTensor( encode_directions(directions)) model_outputs = self._model.forward( self._process_sensors(input_data['rgb'][1]), network_input, directions_tensor)[0] steer, throttle, brake = self._process_model_outputs(model_outputs[0]) control = carla.VehicleControl() control.steer = float(steer) control.throttle = float(throttle) control.brake = float(brake) logging.debug("Output %f %f %f " % (control.steer, control.throttle, control.brake)) if self.first_iter: coil_logger.add_message('Iterating', { "Checkpoint": self.checkpoint['iteration'], 'Agent': str(steer) }, self.checkpoint['iteration']) # There is the posibility to replace some of the predictions with oracle predictions. self.first_iter = False #print(['steer: ', control.steer, 'throttle: ', control.throttle, 'brake: ', control.brake]) return control def get_attentions(self, layers=None): """ Returns The activations obtained from the first layers of the latest iteration. """ if layers is None: layers = [0, 1, 2] if self.latest_image_tensor is None: raise ValueError( 'No step was ran yet. ' 'No image to compute the activations, Try Running ') all_layers = self._model.get_perception_layers( self.latest_image_tensor) cmap = plt.get_cmap('inferno') attentions = [] for layer in layers: y = all_layers[layer] att = torch.abs(y).mean(1)[0].data.cpu().numpy() att = att / att.max() att = cmap(att) att = np.delete(att, 3, 2) attentions.append(imresize(att, [88, 200])) return attentions def _process_sensors(self, sensor): sensor = sensor[:, :, 0:3] # BGRA->BRG drop alpha channel sensor = sensor[g_conf.IMAGE_CUT[0]:g_conf.IMAGE_CUT[1], :, :] # crop sensor = scipy.misc.imresize(sensor, (g_conf.SENSORS['rgb_central'][1], g_conf.SENSORS['rgb_central'][2])) self.latest_image = sensor sensor = np.swapaxes(sensor, 0, 1) sensor = np.transpose(sensor, (2, 1, 0)) sensor = torch.from_numpy(sensor / 255.0).type( torch.FloatTensor).cuda() image_input = sensor.unsqueeze(0) self.latest_image_tensor = image_input return image_input def _get_current_direction(self, vehicle_position): #print(" number of waypoints in global plan:", len(self._global_plan)) # for the current position and orientation try to get the closest one from the waypoints closest_id = 0 min_distance = 100000 for index in range(len(self._global_plan)): waypoint = self._global_plan[index][0] computed_distance = distance_vehicle(waypoint, vehicle_position) if computed_distance < min_distance: min_distance = computed_distance closest_id = index #print(" closest waypoint", closest_id) logging.debug("Closest waypoint {} dist {}".format( closest_id, min_distance)) direction = self._global_plan[closest_id][1] if direction == RoadOption.LEFT: direction = 3.0 elif direction == RoadOption.RIGHT: direction = 4.0 elif direction == RoadOption.STRAIGHT: direction = 5.0 else: direction = 2.0 return direction def _process_model_outputs(self, outputs): """ A bit of heuristics in the control, to eventually make car faster, for instance. Returns: """ steer, throttle, brake = outputs[0], outputs[1], outputs[2] if brake < 0.05: brake = 0.0 if throttle > brake: brake = 0.0 return steer, throttle, brake def _expand_commands(self, topological_plan): """ The idea is to make the intersection indications to last longer""" # O(2*N) algorithm , probably it is possible to do in O(N) with queues. # Get the index where curves start and end curves_start_end = [] inside = False start = -1 current_curve = RoadOption.LANEFOLLOW for index in range(len(topological_plan)): command = topological_plan[index][1] if command != RoadOption.LANEFOLLOW and not inside: inside = True start = index current_curve = command if command == RoadOption.LANEFOLLOW and inside: inside = False # End now is the index. curves_start_end.append([start, index, current_curve]) if start == -1: raise ValueError("End of curve without start") start = -1 for start_end_index_command in curves_start_end: start_index = start_end_index_command[0] end_index = start_end_index_command[1] command = start_end_index_command[2] # Add the backwards curves ( Before the begginning) for index in range(1, self._expand_command_front + 1): changed_index = start_index - index if changed_index > 0: topological_plan[changed_index] = ( topological_plan[changed_index][0], command) # add the onnes after the end for index in range(0, self._expand_command_back): changed_index = end_index + index if changed_index < len(topological_plan): topological_plan[changed_index] = ( topological_plan[changed_index][0], command) return topological_plan
class CoILAgent(object): def __init__(self, checkpoint, town_name, carla_version='0.84'): # Set the carla version that is going to be used by the interface self._carla_version = carla_version self.checkpoint = checkpoint # We save the checkpoint for some interesting future use. self._model = CoILModel(g_conf.MODEL_TYPE, g_conf.MODEL_CONFIGURATION) self.first_iter = True # Load the model and prepare set it for evaluation self._model.load_state_dict(checkpoint['state_dict']) self._model.cuda() self._model.eval() self.latest_image = None self.latest_image_tensor = None if g_conf.USE_ORACLE or g_conf.USE_FULL_ORACLE: self.control_agent = CommandFollower(town_name) def run_step(self, measurements, sensor_data, directions, target): """ Run a step on the benchmark simulation Args: measurements: All the float measurements from CARLA ( Just speed is used) sensor_data: All the sensor data used on this benchmark directions: The directions, high level commands target: Final objective. Not used when the agent is predicting all outputs. Returns: Controls for the vehicle on the CARLA simulator. """ # Take the forward speed and normalize it for it to go from 0-1 norm_speed = measurements.player_measurements.forward_speed / g_conf.SPEED_FACTOR norm_speed = torch.cuda.FloatTensor([norm_speed]).unsqueeze(0) directions_tensor = torch.cuda.LongTensor([directions]) # Compute the forward pass processing the sensors got from CARLA. model_outputs = self._model.forward_branch( self._process_sensors(sensor_data), norm_speed, directions_tensor) steer, throttle, brake = self._process_model_outputs(model_outputs[0]) if self._carla_version == '0.9': import carla control = carla.VehicleControl() else: control = VehicleControl() control.steer = float(steer) control.throttle = float(throttle) control.brake = float(brake) # There is the posibility to replace some of the predictions with oracle predictions. if g_conf.USE_ORACLE: _, control.throttle, control.brake = self._get_oracle_prediction( measurements, target) if self.first_iter: coil_logger.add_message('Iterating', { "Checkpoint": self.checkpoint['iteration'], 'Agent': str(steer) }, self.checkpoint['iteration']) self.first_iter = False return control def get_attentions(self, layers=None): """ Returns The activations obtained from the first layers of the latest iteration. """ if layers is None: layers = [0, 1, 2] if self.latest_image_tensor is None: raise ValueError( 'No step was ran yet. ' 'No image to compute the activations, Try Running ') all_layers = self._model.get_perception_layers( self.latest_image_tensor) cmap = plt.get_cmap('inferno') attentions = [] for layer in layers: y = all_layers[layer] att = torch.abs(y).mean(1)[0].data.cpu().numpy() att = att / att.max() att = cmap(att) att = np.delete(att, 3, 2) attentions.append(imresize(att, [150, 200])) return attentions def _process_sensors(self, sensors): colors = [[0, 0, 0], [70, 70, 70], [190, 153, 153], [250, 170, 160], [220, 20, 60], [153, 153, 153], [157, 234, 50], [128, 64, 128], [244, 35, 232], [107, 142, 35], [0, 0, 142], [102, 102, 156], [220, 220, 0]] def label_to_color_0(e): return colors[e][0] def label_to_color_1(e): return colors[e][1] def label_to_color_2(e): return colors[e][2] iteration = 0 for name, size in g_conf.SENSORS.items(): sensor = sensors[name].data if (sensor.shape == (600, 800)): labels = sensor tmp = np.zeros((600, 800, 3)) f0 = np.vectorize(label_to_color_0) f1 = np.vectorize(label_to_color_1) f2 = np.vectorize(label_to_color_2) tmp[:, :, 0] = f0(sensor) tmp[:, :, 1] = f1(sensor) tmp[:, :, 2] = f2(sensor) sensor = tmp sensor = scipy.misc.imresize(sensor, (size[1], size[2])) ##### Save sensor self.latest_image = sensor sensor = np.swapaxes(sensor, 0, 1) sensor = np.transpose(sensor, (2, 1, 0)) sensor = torch.from_numpy(sensor / 255.0).type( torch.FloatTensor).cuda() if iteration == 0: image_input = sensor else: image_input = torch.cat((image_input, sensor), 0) iteration += 1 image_input = image_input.unsqueeze(0) self.latest_image_tensor = image_input return image_input def _process_model_outputs(self, outputs): """ A bit of heuristics in the control, to eventually make car faster, for instance. Returns: """ steer, throttle, brake = outputs[0], outputs[1], outputs[2] if brake < 0.05: brake = 0.0 if throttle > brake: brake = 0.0 return steer, throttle, brake def _process_model_outputs_wp(self, outputs): """ A bit of heuristics in the control, to eventually make car faster, for instance. Returns: """ wpa1, wpa2, throttle, brake = outputs[3], outputs[4], outputs[ 1], outputs[2] if brake < 0.2: brake = 0.0 if throttle > brake: brake = 0.0 steer = 0.7 * wpa2 if steer > 0: steer = min(steer, 1) else: steer = max(steer, -1) return steer, throttle, brake def _get_oracle_prediction(self, measurements, target): # For the oracle, the current version of sensor data is not really relevant. control, _, _, _, _ = self.control_agent.run_step( measurements, [], [], target) return control.steer, control.throttle, control.brake