def __init__(self, config=ENV_CONFIG): self.config = config self.city = self.config["server_map"].split("/")[-1] if self.config["enable_planner"]: self.planner = Planner(self.city) if config["discrete_actions"]: self.action_space = Discrete(len(DISCRETE_ACTIONS)) else: self.action_space = Box(-1.0, 1.0, shape=(2, ), dtype=np.float32) if config["use_depth_camera"]: image_space = Box( -1.0, 1.0, shape=(config["y_res"], config["x_res"], 1 * config["framestack"]), dtype=np.float32) else: image_space = Box( 0, 255, shape=(config["y_res"], config["x_res"], 3 * config["framestack"]), dtype=np.uint8) self.observation_space = Tuple( # forward_speed, dist to goal [ image_space, Discrete(len(COMMANDS_ENUM)), # next_command Box(-128.0, 128.0, shape=(2, ), dtype=np.float32) ]) # TODO(ekl) this isn't really a proper gym spec self._spec = lambda: None self._spec.id = "Carla-v0" self.server_port = None self.server_process = None self.client = None self.num_steps = 0 self.total_reward = 0 self.prev_measurement = None self.prev_image = None self.episode_id = None self.measurements_file = None self.weather = None self.scenario = None self.start_pos = None self.end_pos = None self.start_coord = None self.end_coord = None self.last_obs = None
def __init__( self, city_name='Town01', name_to_save='Test', continue_experiment=False, save_images=False, distance_for_success=2.0 ): self.__metaclass__ = abc.ABCMeta self._city_name = city_name self._base_name = name_to_save # The minimum distance for arriving into the goal point in # order to consider ir a success self._distance_for_success = distance_for_success # The object used to record the benchmark and to able to continue after self._recording = Recording(name_to_save=name_to_save, continue_experiment=continue_experiment, save_images=save_images ) # We have a default planner instantiated that produces high level commands self._planner = Planner(city_name)
class CarlaEnv(gym.Env): def __init__(self, config=ENV_CONFIG): self.config = config self.city = self.config["server_map"].split("/")[-1] if self.config["enable_planner"]: self.planner = Planner(self.city) if config["discrete_actions"]: self.action_space = Discrete(len(DISCRETE_ACTIONS)) else: self.action_space = Box(-1.0, 1.0, shape=(2, ), dtype=np.float32) if config["use_depth_camera"]: image_space = Box( -1.0, 1.0, shape=(config["y_res"], config["x_res"], 1 * config["framestack"]), dtype=np.float32) else: image_space = Box( 0, 255, shape=(config["y_res"], config["x_res"], 3 * config["framestack"]), dtype=np.uint8) self.observation_space = Tuple( # forward_speed, dist to goal [ image_space, Discrete(len(COMMANDS_ENUM)), # next_command Box(-128.0, 128.0, shape=(2, ), dtype=np.float32) ]) # TODO(ekl) this isn't really a proper gym spec self._spec = lambda: None self._spec.id = "Carla-v0" self.server_port = None self.server_process = None self.client = None self.num_steps = 0 self.total_reward = 0 self.prev_measurement = None self.prev_image = None self.episode_id = None self.measurements_file = None self.weather = None self.scenario = None self.start_pos = None self.end_pos = None self.start_coord = None self.end_coord = None self.last_obs = None def init_server(self): print("Initializing new Carla server...") # Create a new server process and start the client. self.server_port = random.randint(10000, 60000) self.server_process = subprocess.Popen( [ SERVER_BINARY, self.config["server_map"], "-windowed", "-ResX=400", "-ResY=300", "-carla-server", "-carla-world-port={}".format(self.server_port) ], preexec_fn=os.setsid, stdout=open(os.devnull, "w")) live_carla_processes.add(os.getpgid(self.server_process.pid)) for i in range(RETRIES_ON_ERROR): try: self.client = CarlaClient("localhost", self.server_port) return self.client.connect() except Exception as e: print("Error connecting: {}, attempt {}".format(e, i)) time.sleep(2) def clear_server_state(self): print("Clearing Carla server state") try: if self.client: self.client.disconnect() self.client = None except Exception as e: print("Error disconnecting client: {}".format(e)) pass if self.server_process: pgid = os.getpgid(self.server_process.pid) os.killpg(pgid, signal.SIGKILL) live_carla_processes.remove(pgid) self.server_port = None self.server_process = None def __del__(self): self.clear_server_state() def reset(self): error = None for _ in range(RETRIES_ON_ERROR): try: if not self.server_process: self.init_server() return self._reset() except Exception as e: print("Error during reset: {}".format(traceback.format_exc())) self.clear_server_state() error = e raise error def _reset(self): self.num_steps = 0 self.total_reward = 0 self.prev_measurement = None self.prev_image = None self.episode_id = datetime.today().strftime("%Y-%m-%d_%H-%M-%S_%f") self.measurements_file = None # Create a CarlaSettings object. This object is a wrapper around # the CarlaSettings.ini file. Here we set the configuration we # want for the new episode. settings = CarlaSettings() self.scenario = random.choice(self.config["scenarios"]) assert self.scenario["city"] == self.city, (self.scenario, self.city) self.weather = random.choice(self.scenario["weather_distribution"]) settings.set( SynchronousMode=True, SendNonPlayerAgentsInfo=True, NumberOfVehicles=self.scenario["num_vehicles"], NumberOfPedestrians=self.scenario["num_pedestrians"], WeatherId=self.weather) settings.randomize_seeds() if self.config["use_depth_camera"]: camera1 = Camera("CameraDepth", PostProcessing="Depth") camera1.set_image_size(self.config["render_x_res"], self.config["render_y_res"]) camera1.set_position(30, 0, 130) settings.add_sensor(camera1) camera2 = Camera("CameraRGB") camera2.set_image_size(self.config["render_x_res"], self.config["render_y_res"]) camera2.set_position(30, 0, 130) settings.add_sensor(camera2) # Setup start and end positions scene = self.client.load_settings(settings) positions = scene.player_start_spots self.start_pos = positions[self.scenario["start_pos_id"]] self.end_pos = positions[self.scenario["end_pos_id"]] self.start_coord = [ self.start_pos.location.x // 100, self.start_pos.location.y // 100 ] self.end_coord = [ self.end_pos.location.x // 100, self.end_pos.location.y // 100 ] print("Start pos {} ({}), end {} ({})".format( self.scenario["start_pos_id"], self.start_coord, self.scenario["end_pos_id"], self.end_coord)) # Notify the server that we want to start the episode at the # player_start index. This function blocks until the server is ready # to start the episode. print("Starting new episode...") self.client.start_episode(self.scenario["start_pos_id"]) image, py_measurements = self._read_observation() self.prev_measurement = py_measurements return self.encode_obs(self.preprocess_image(image), py_measurements) def encode_obs(self, image, py_measurements): assert self.config["framestack"] in [1, 2] prev_image = self.prev_image self.prev_image = image if prev_image is None: prev_image = image if self.config["framestack"] == 2: image = np.concatenate([prev_image, image], axis=2) obs = (image, COMMAND_ORDINAL[py_measurements["next_command"]], [ py_measurements["forward_speed"], py_measurements["distance_to_goal"] ]) self.last_obs = obs return obs def step(self, action): try: obs = self._step(action) return obs except Exception: print("Error during step, terminating episode early", traceback.format_exc()) self.clear_server_state() return (self.last_obs, 0.0, True, {}) def _step(self, action): if self.config["discrete_actions"]: action = DISCRETE_ACTIONS[int(action)] assert len(action) == 2, "Invalid action {}".format(action) if self.config["squash_action_logits"]: forward = 2 * float(sigmoid(action[0]) - 0.5) throttle = float(np.clip(forward, 0, 1)) brake = float(np.abs(np.clip(forward, -1, 0))) steer = 2 * float(sigmoid(action[1]) - 0.5) else: throttle = float(np.clip(action[0], 0, 1)) brake = float(np.abs(np.clip(action[0], -1, 0))) steer = float(np.clip(action[1], -1, 1)) reverse = False hand_brake = False if self.config["verbose"]: print("steer", steer, "throttle", throttle, "brake", brake, "reverse", reverse) self.client.send_control( steer=steer, throttle=throttle, brake=brake, hand_brake=hand_brake, reverse=reverse) # Process observations image, py_measurements = self._read_observation() if self.config["verbose"]: print("Next command", py_measurements["next_command"]) if type(action) is np.ndarray: py_measurements["action"] = [float(a) for a in action] else: py_measurements["action"] = action py_measurements["control"] = { "steer": steer, "throttle": throttle, "brake": brake, "reverse": reverse, "hand_brake": hand_brake, } reward = compute_reward(self, self.prev_measurement, py_measurements) self.total_reward += reward py_measurements["reward"] = reward py_measurements["total_reward"] = self.total_reward done = (self.num_steps > self.scenario["max_steps"] or py_measurements["next_command"] == "REACH_GOAL" or (self.config["early_terminate_on_collision"] and collided_done(py_measurements))) py_measurements["done"] = done self.prev_measurement = py_measurements # Write out measurements to file if CARLA_OUT_PATH: if not self.measurements_file: self.measurements_file = open( os.path.join( CARLA_OUT_PATH, "measurements_{}.json".format(self.episode_id)), "w") self.measurements_file.write(json.dumps(py_measurements)) self.measurements_file.write("\n") if done: self.measurements_file.close() self.measurements_file = None if self.config["convert_images_to_video"]: self.images_to_video() self.num_steps += 1 image = self.preprocess_image(image) return (self.encode_obs(image, py_measurements), reward, done, py_measurements) def images_to_video(self): videos_dir = os.path.join(CARLA_OUT_PATH, "Videos") if not os.path.exists(videos_dir): os.makedirs(videos_dir) ffmpeg_cmd = ( "ffmpeg -loglevel -8 -r 60 -f image2 -s {x_res}x{y_res} " "-start_number 0 -i " "{img}_%04d.jpg -vcodec libx264 {vid}.mp4 && rm -f {img}_*.jpg " ).format( x_res=self.config["render_x_res"], y_res=self.config["render_y_res"], vid=os.path.join(videos_dir, self.episode_id), img=os.path.join(CARLA_OUT_PATH, "CameraRGB", self.episode_id)) print("Executing ffmpeg command", ffmpeg_cmd) subprocess.call(ffmpeg_cmd, shell=True) def preprocess_image(self, image): if self.config["use_depth_camera"]: assert self.config["use_depth_camera"] data = (image.data - 0.5) * 2 data = data.reshape(self.config["render_y_res"], self.config["render_x_res"], 1) data = cv2.resize( data, (self.config["x_res"], self.config["y_res"]), interpolation=cv2.INTER_AREA) data = np.expand_dims(data, 2) else: data = image.data.reshape(self.config["render_y_res"], self.config["render_x_res"], 3) data = cv2.resize( data, (self.config["x_res"], self.config["y_res"]), interpolation=cv2.INTER_AREA) data = (data.astype(np.float32) - 128) / 128 return data def _read_observation(self): # Read the data produced by the server this frame. measurements, sensor_data = self.client.read_data() # Print some of the measurements. if self.config["verbose"]: print_measurements(measurements) observation = None if self.config["use_depth_camera"]: camera_name = "CameraDepth" else: camera_name = "CameraRGB" for name, image in sensor_data.items(): if name == camera_name: observation = image cur = measurements.player_measurements if self.config["enable_planner"]: next_command = COMMANDS_ENUM[self.planner.get_next_command( [cur.transform.location.x, cur.transform.location.y, GROUND_Z], [ cur.transform.orientation.x, cur.transform.orientation.y, GROUND_Z ], [self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [ self.end_pos.orientation.x, self.end_pos.orientation.y, GROUND_Z ])] else: next_command = "LANE_FOLLOW" if next_command == "REACH_GOAL": distance_to_goal = 0.0 # avoids crash in planner elif self.config["enable_planner"]: distance_to_goal = self.planner.get_shortest_path_distance([ cur.transform.location.x, cur.transform.location.y, GROUND_Z ], [ cur.transform.orientation.x, cur.transform.orientation.y, GROUND_Z ], [self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [ self.end_pos.orientation.x, self.end_pos.orientation.y, GROUND_Z ]) / 100 else: distance_to_goal = -1 distance_to_goal_euclidean = float( np.linalg.norm([ cur.transform.location.x - self.end_pos.location.x, cur.transform.location.y - self.end_pos.location.y ]) / 100) py_measurements = { "episode_id": self.episode_id, "step": self.num_steps, "x": cur.transform.location.x, "y": cur.transform.location.y, "x_orient": cur.transform.orientation.x, "y_orient": cur.transform.orientation.y, "forward_speed": cur.forward_speed, "distance_to_goal": distance_to_goal, "distance_to_goal_euclidean": distance_to_goal_euclidean, "collision_vehicles": cur.collision_vehicles, "collision_pedestrians": cur.collision_pedestrians, "collision_other": cur.collision_other, "intersection_offroad": cur.intersection_offroad, "intersection_otherlane": cur.intersection_otherlane, "weather": self.weather, "map": self.config["server_map"], "start_coord": self.start_coord, "end_coord": self.end_coord, "current_scenario": self.scenario, "x_res": self.config["x_res"], "y_res": self.config["y_res"], "num_vehicles": self.scenario["num_vehicles"], "num_pedestrians": self.scenario["num_pedestrians"], "max_steps": self.scenario["max_steps"], "next_command": next_command, } if CARLA_OUT_PATH and self.config["log_images"]: for name, image in sensor_data.items(): out_dir = os.path.join(CARLA_OUT_PATH, name) if not os.path.exists(out_dir): os.makedirs(out_dir) out_file = os.path.join( out_dir, "{}_{:>04}.jpg".format(self.episode_id, self.num_steps)) scipy.misc.imsave(out_file, image.data) assert observation is not None, sensor_data return observation, py_measurements
def __init__(self, level: LevelSelection, seed: int, frame_skip: int, human_control: bool, custom_reward_threshold: Union[int, float], visualization_parameters: VisualizationParameters, server_height: int, server_width: int, camera_height: int, camera_width: int, verbose: bool, experiment_suite: ExperimentSuite, config: str, episode_max_time: int, allow_braking: bool, quality: CarlaEnvironmentParameters.Quality, cameras: List[CameraTypes], weather_id: List[int], experiment_path: str, separate_actions_for_throttle_and_brake: bool, num_speedup_steps: int, max_speed: float, **kwargs): super().__init__(level, seed, frame_skip, human_control, custom_reward_threshold, visualization_parameters) # server configuration self.server_height = server_height self.server_width = server_width self.port = 9003 #get_open_port() self.host = 'localhost' self.map_name = CarlaLevel[level.upper()].value['map_name'] self.map_path = CarlaLevel[level.upper()].value['map_path'] self.experiment_path = experiment_path # client configuration self.verbose = verbose self.quality = quality self.cameras = cameras self.weather_id = weather_id self.episode_max_time = episode_max_time self.allow_braking = allow_braking self.separate_actions_for_throttle_and_brake = separate_actions_for_throttle_and_brake self.camera_width = camera_width self.camera_height = camera_height # setup server settings self.experiment_suite = experiment_suite self.config = config if self.config: # load settings from file with open(self.config, 'r') as fp: self.settings = fp.read() else: # hard coded settings self.settings = CarlaSettings() self.settings.set(SynchronousMode=True, SendNonPlayerAgentsInfo=False, NumberOfVehicles=15, NumberOfPedestrians=30, WeatherId=random.choice( force_list(self.weather_id)), QualityLevel=self.quality.value, SeedVehicles=seed, SeedPedestrians=seed) if seed is None: self.settings.randomize_seeds() self.settings = self._add_cameras(self.settings, self.cameras, self.camera_width, self.camera_height) # open the server self.server = self._open_server() logging.disable(40) # open the client self.game = CarlaClient(self.host, self.port, timeout=99999999) self.game.connect() if self.experiment_suite: self.current_experiment_idx = 0 self.current_experiment = self.experiment_suite.get_experiments()[ self.current_experiment_idx] self.scene = self.game.load_settings( self.current_experiment.conditions) else: self.scene = self.game.load_settings(self.settings) # get available start positions self.positions = self.scene.player_start_spots self.num_positions = len(self.positions) self.current_start_position_idx = 0 self.current_pose = 0 # state space self.state_space = StateSpace({ "measurements": VectorObservationSpace( 4, measurements_names=["forward_speed", "x", "y", "z"]) }) for camera in self.scene.sensors: self.state_space[camera.name] = ImageObservationSpace( shape=np.array([self.camera_height, self.camera_width, 3]), high=255) # action space if self.separate_actions_for_throttle_and_brake: self.action_space = BoxActionSpace( shape=3, low=np.array([-1, 0, 0]), high=np.array([1, 1, 1]), descriptions=["steer", "gas", "brake"]) else: self.action_space = BoxActionSpace( shape=2, low=np.array([-1, -1]), high=np.array([1, 1]), descriptions=["steer", "gas_and_brake"]) # human control if self.human_control: # convert continuous action space to discrete self.steering_strength = 0.5 self.gas_strength = 1.0 self.brake_strength = 0.5 # TODO: reverse order of actions self.action_space = PartialDiscreteActionSpaceMap( target_actions=[[0., 0.], [0., -self.steering_strength], [0., self.steering_strength], [self.gas_strength, 0.], [-self.brake_strength, 0], [self.gas_strength, -self.steering_strength], [self.gas_strength, self.steering_strength], [self.brake_strength, -self.steering_strength], [self.brake_strength, self.steering_strength]], descriptions=[ 'NO-OP', 'TURN_LEFT', 'TURN_RIGHT', 'GAS', 'BRAKE', 'GAS_AND_TURN_LEFT', 'GAS_AND_TURN_RIGHT', 'BRAKE_AND_TURN_LEFT', 'BRAKE_AND_TURN_RIGHT' ]) # map keyboard keys to actions for idx, action in enumerate(self.action_space.descriptions): for key in key_map.keys(): if action == key: self.key_to_action[key_map[key]] = idx self.num_speedup_steps = num_speedup_steps self.max_speed = max_speed # measurements self.autopilot = None self.planner = Planner(self.map_name) # env initialization self.reset_internal_state(True) # render if self.is_rendered: image = self.get_rendered_image() self.renderer.create_screen(image.shape[1], image.shape[0])
class DrivingBenchmark(object): """ The Benchmark class, controls the execution of the benchmark interfacing an Agent class with a set Suite. The benchmark class must be inherited with a class that defines the all the experiments to be run by the agent """ def __init__( self, city_name='Town01', name_to_save='Test', continue_experiment=False, save_images=False, distance_for_success=2.0 ): self.__metaclass__ = abc.ABCMeta self._city_name = city_name self._base_name = name_to_save # The minimum distance for arriving into the goal point in # order to consider ir a success self._distance_for_success = distance_for_success # The object used to record the benchmark and to able to continue after self._recording = Recording(name_to_save=name_to_save, continue_experiment=continue_experiment, save_images=save_images ) # We have a default planner instantiated that produces high level commands self._planner = Planner(city_name) def benchmark_agent(self, experiment_suite, agent, client): """ Function to benchmark the agent. It first check the log file for this benchmark. if it exist it continues from the experiment where it stopped. Args: experiment_suite agent: an agent object with the run step class implemented. client: Return: A dictionary with all the metrics computed from the agent running the set of experiments. """ # Instantiate a metric object that will be used to compute the metrics for # the benchmark afterwards. metrics_object = Metrics(experiment_suite.metrics_parameters, experiment_suite.dynamic_tasks) # Function return the current pose and task for this benchmark. start_pose, start_experiment = self._recording.get_pose_and_experiment( experiment_suite.get_number_of_poses_task()) logging.info('START') for experiment in experiment_suite.get_experiments()[int(start_experiment):]: positions = client.load_settings( experiment.conditions).player_start_spots self._recording.log_start(experiment.task) for pose in experiment.poses[start_pose:]: for rep in range(experiment.repetitions): start_index = pose[0] end_index = pose[1] client.start_episode(start_index) # Print information on logging.info('======== !!!! ==========') logging.info(' Start Position %d End Position %d ', start_index, end_index) self._recording.log_poses(start_index, end_index, experiment.Conditions.WeatherId) # Calculate the initial distance for this episode initial_distance = \ sldist( [positions[start_index].location.x, positions[start_index].location.y], [positions[end_index].location.x, positions[end_index].location.y]) time_out = experiment_suite.calculate_time_out( self._get_shortest_path(positions[start_index], positions[end_index])) # running the agent (result, reward_vec, control_vec, final_time, remaining_distance) = \ self._run_navigation_episode( agent, client, time_out, positions[end_index], str(experiment.Conditions.WeatherId) + '_' + str(experiment.task) + '_' + str(start_index) + '.' + str(end_index)) # Write the general status of the just ran episode self._recording.write_summary_results( experiment, pose, rep, initial_distance, remaining_distance, final_time, time_out, result) # Write the details of this episode. self._recording.write_measurements_results(experiment, rep, pose, reward_vec, control_vec) if result > 0: logging.info('+++++ Target achieved in %f seconds! +++++', final_time) else: logging.info('----- Timeout! -----') start_pose = 0 self._recording.log_end() return metrics_object.compute(self._recording.path) def get_path(self): """ Returns the path were the log was saved. """ return self._recording.path def _get_directions(self, current_point, end_point): """ Class that should return the directions to reach a certain goal """ directions = self._planner.get_next_command( (current_point.location.x, current_point.location.y, 0.22), (current_point.orientation.x, current_point.orientation.y, current_point.orientation.z), (end_point.location.x, end_point.location.y, 0.22), (end_point.orientation.x, end_point.orientation.y, end_point.orientation.z)) return directions def _get_shortest_path(self, start_point, end_point): """ Calculates the shortest path between two points considering the road netowrk """ return self._planner.get_shortest_path_distance( [ start_point.location.x, start_point.location.y, 0.22], [ start_point.orientation.x, start_point.orientation.y, 0.22], [ end_point.location.x, end_point.location.y, end_point.location.z], [ end_point.orientation.x, end_point.orientation.y, end_point.orientation.z]) def _run_navigation_episode( self, agent, client, time_out, target, episode_name): """ Run one episode of the benchmark (Pose) for a certain agent. Args: agent: the agent object client: an object of the carla client to communicate with the CARLA simulator time_out: the time limit to complete this episode target: the target to reach episode_name: The name for saving images of this episode """ # Send an initial command. measurements, sensor_data = client.read_data() client.send_control(VehicleControl()) initial_timestamp = measurements.game_timestamp current_timestamp = initial_timestamp # The vector containing all measurements produced on this episode measurement_vec = [] # The vector containing all controls produced on this episode control_vec = [] frame = 0 distance = 10000 success = False while (current_timestamp - initial_timestamp) < (time_out * 1000) and not success: # Read data from server with the client measurements, sensor_data = client.read_data() # The directions to reach the goal are calculated. directions = self._get_directions(measurements.player_measurements.transform, target) # Agent process the data. control = agent.run_step(measurements, sensor_data, directions, target) # Send the control commands to the vehicle client.send_control(control) # save images if the flag is activated self._recording.save_images(sensor_data, episode_name, frame) current_x = measurements.player_measurements.transform.location.x current_y = measurements.player_measurements.transform.location.y logging.info("Controller is Inputting:") logging.info('Steer = %f Throttle = %f Brake = %f ', control.steer, control.throttle, control.brake) current_timestamp = measurements.game_timestamp # Get the distance travelled until now distance = sldist([current_x, current_y], [target.location.x, target.location.y]) # Write status of the run on verbose mode logging.info('Status:') logging.info( '[d=%f] c_x = %f, c_y = %f ---> t_x = %f, t_y = %f', float(distance), current_x, current_y, target.location.x, target.location.y) # Check if reach the target if distance < self._distance_for_success: success = True # Increment the vectors and append the measurements and controls. frame += 1 measurement_vec.append(measurements.player_measurements) control_vec.append(control) if success: return 1, measurement_vec, control_vec, float( current_timestamp - initial_timestamp) / 1000.0, distance return 0, measurement_vec, control_vec, time_out, distance
class CarlaEnvironment(Environment): def __init__(self, level: LevelSelection, seed: int, frame_skip: int, human_control: bool, custom_reward_threshold: Union[int, float], visualization_parameters: VisualizationParameters, server_height: int, server_width: int, camera_height: int, camera_width: int, verbose: bool, experiment_suite: ExperimentSuite, config: str, episode_max_time: int, allow_braking: bool, quality: CarlaEnvironmentParameters.Quality, cameras: List[CameraTypes], weather_id: List[int], experiment_path: str, separate_actions_for_throttle_and_brake: bool, num_speedup_steps: int, max_speed: float, **kwargs): super().__init__(level, seed, frame_skip, human_control, custom_reward_threshold, visualization_parameters) # server configuration self.server_height = server_height self.server_width = server_width self.port = 9003 #get_open_port() self.host = 'localhost' self.map_name = CarlaLevel[level.upper()].value['map_name'] self.map_path = CarlaLevel[level.upper()].value['map_path'] self.experiment_path = experiment_path # client configuration self.verbose = verbose self.quality = quality self.cameras = cameras self.weather_id = weather_id self.episode_max_time = episode_max_time self.allow_braking = allow_braking self.separate_actions_for_throttle_and_brake = separate_actions_for_throttle_and_brake self.camera_width = camera_width self.camera_height = camera_height # setup server settings self.experiment_suite = experiment_suite self.config = config if self.config: # load settings from file with open(self.config, 'r') as fp: self.settings = fp.read() else: # hard coded settings self.settings = CarlaSettings() self.settings.set(SynchronousMode=True, SendNonPlayerAgentsInfo=False, NumberOfVehicles=15, NumberOfPedestrians=30, WeatherId=random.choice( force_list(self.weather_id)), QualityLevel=self.quality.value, SeedVehicles=seed, SeedPedestrians=seed) if seed is None: self.settings.randomize_seeds() self.settings = self._add_cameras(self.settings, self.cameras, self.camera_width, self.camera_height) # open the server self.server = self._open_server() logging.disable(40) # open the client self.game = CarlaClient(self.host, self.port, timeout=99999999) self.game.connect() if self.experiment_suite: self.current_experiment_idx = 0 self.current_experiment = self.experiment_suite.get_experiments()[ self.current_experiment_idx] self.scene = self.game.load_settings( self.current_experiment.conditions) else: self.scene = self.game.load_settings(self.settings) # get available start positions self.positions = self.scene.player_start_spots self.num_positions = len(self.positions) self.current_start_position_idx = 0 self.current_pose = 0 # state space self.state_space = StateSpace({ "measurements": VectorObservationSpace( 4, measurements_names=["forward_speed", "x", "y", "z"]) }) for camera in self.scene.sensors: self.state_space[camera.name] = ImageObservationSpace( shape=np.array([self.camera_height, self.camera_width, 3]), high=255) # action space if self.separate_actions_for_throttle_and_brake: self.action_space = BoxActionSpace( shape=3, low=np.array([-1, 0, 0]), high=np.array([1, 1, 1]), descriptions=["steer", "gas", "brake"]) else: self.action_space = BoxActionSpace( shape=2, low=np.array([-1, -1]), high=np.array([1, 1]), descriptions=["steer", "gas_and_brake"]) # human control if self.human_control: # convert continuous action space to discrete self.steering_strength = 0.5 self.gas_strength = 1.0 self.brake_strength = 0.5 # TODO: reverse order of actions self.action_space = PartialDiscreteActionSpaceMap( target_actions=[[0., 0.], [0., -self.steering_strength], [0., self.steering_strength], [self.gas_strength, 0.], [-self.brake_strength, 0], [self.gas_strength, -self.steering_strength], [self.gas_strength, self.steering_strength], [self.brake_strength, -self.steering_strength], [self.brake_strength, self.steering_strength]], descriptions=[ 'NO-OP', 'TURN_LEFT', 'TURN_RIGHT', 'GAS', 'BRAKE', 'GAS_AND_TURN_LEFT', 'GAS_AND_TURN_RIGHT', 'BRAKE_AND_TURN_LEFT', 'BRAKE_AND_TURN_RIGHT' ]) # map keyboard keys to actions for idx, action in enumerate(self.action_space.descriptions): for key in key_map.keys(): if action == key: self.key_to_action[key_map[key]] = idx self.num_speedup_steps = num_speedup_steps self.max_speed = max_speed # measurements self.autopilot = None self.planner = Planner(self.map_name) # env initialization self.reset_internal_state(True) # render if self.is_rendered: image = self.get_rendered_image() self.renderer.create_screen(image.shape[1], image.shape[0]) def _add_cameras(self, settings, cameras, camera_width, camera_height): # add a front facing camera if CameraTypes.FRONT in cameras: camera = Camera(CameraTypes.FRONT.value) camera.set(FOV=100) camera.set_image_size(camera_width, camera_height) camera.set_position(2.0, 0, 1.4) camera.set_rotation(-15.0, 0, 0) settings.add_sensor(camera) # add a left facing camera if CameraTypes.LEFT in cameras: camera = Camera(CameraTypes.LEFT.value) camera.set(FOV=100) camera.set_image_size(camera_width, camera_height) camera.set_position(2.0, 0, 1.4) camera.set_rotation(-15.0, -30, 0) settings.add_sensor(camera) # add a right facing camera if CameraTypes.RIGHT in cameras: camera = Camera(CameraTypes.RIGHT.value) camera.set(FOV=100) camera.set_image_size(camera_width, camera_height) camera.set_position(2.0, 0, 1.4) camera.set_rotation(-15.0, 30, 0) settings.add_sensor(camera) # add a front facing depth camera if CameraTypes.DEPTH in cameras: camera = Camera(CameraTypes.DEPTH.value) camera.set_image_size(camera_width, camera_height) camera.set_position(0.2, 0, 1.3) camera.set_rotation(8, 30, 0) camera.PostProcessing = 'Depth' settings.add_sensor(camera) # add a front facing semantic segmentation camera if CameraTypes.SEGMENTATION in cameras: camera = Camera(CameraTypes.SEGMENTATION.value) camera.set_image_size(camera_width, camera_height) camera.set_position(0.2, 0, 1.3) camera.set_rotation(8, 30, 0) camera.PostProcessing = 'SemanticSegmentation' settings.add_sensor(camera) return settings def _get_directions(self, current_point, end_point): """ Class that should return the directions to reach a certain goal """ directions = self.planner.get_next_command( (current_point.location.x, current_point.location.y, 0.22), (current_point.orientation.x, current_point.orientation.y, current_point.orientation.z), (end_point.location.x, end_point.location.y, 0.22), (end_point.orientation.x, end_point.orientation.y, end_point.orientation.z)) return directions def _open_server(self): log_path = path.join( self.experiment_path if self.experiment_path is not None else '.', 'logs', "CARLA_LOG_{}.txt".format(self.port)) if not os.path.exists(os.path.dirname(log_path)): os.makedirs(os.path.dirname(log_path)) with open(log_path, "wb") as out: cmd = [ path.join(environ.get('CARLA_ROOT'), 'CarlaUE4.sh'), self.map_path, "-benchmark", "-carla-server", "-fps={}".format(30 / self.frame_skip), "-world-port={}".format(self.port), "-windowed -nocore -ResX={} -ResY={}".format( self.server_width, self.server_height), "-carla-no-hud" ] if self.config: cmd.append("-carla-settings={}".format(self.config)) p = subprocess.Popen(cmd, stdout=out, stderr=out) return p def _close_server(self): os.killpg(os.getpgid(self.server.pid), signal.SIGKILL) def _update_state(self): # get measurements and observations measurements = [] while type(measurements) == list: measurements, sensor_data = self.game.read_data() self.state = {} for camera in self.scene.sensors: self.state[camera.name] = sensor_data[camera.name].data self.location = [ measurements.player_measurements.transform.location.x, measurements.player_measurements.transform.location.y, measurements.player_measurements.transform.location.z ] self.distance_from_goal = np.linalg.norm( np.array(self.location[:2]) - [self.current_goal.location.x, self.current_goal.location.y]) is_collision = measurements.player_measurements.collision_vehicles != 0 \ or measurements.player_measurements.collision_pedestrians != 0 \ or measurements.player_measurements.collision_other != 0 speed_reward = measurements.player_measurements.forward_speed - 1 if speed_reward > 30.: speed_reward = 30. self.reward = speed_reward \ - (measurements.player_measurements.intersection_otherlane * 5) \ - (measurements.player_measurements.intersection_offroad * 5) \ - is_collision * 100 \ - np.abs(self.control.steer) * 10 # update measurements self.measurements = [measurements.player_measurements.forward_speed ] + self.location self.autopilot = measurements.player_measurements.autopilot_control # The directions to reach the goal (0 Follow lane, 1 Left, 2 Right, 3 Straight) directions = int( self._get_directions(measurements.player_measurements.transform, self.current_goal) - 2) self.state['high_level_command'] = directions if (measurements.game_timestamp >= self.episode_max_time) or is_collision: self.done = True self.state['measurements'] = np.array(self.measurements) def _take_action(self, action): self.control = VehicleControl() if self.separate_actions_for_throttle_and_brake: self.control.steer = np.clip(action[0], -1, 1) self.control.throttle = np.clip(action[1], 0, 1) self.control.brake = np.clip(action[2], 0, 1) else: # transform the 2 value action (steer, throttle - brake) into a 3 value action (steer, throttle, brake) self.control.steer = np.clip(action[0], -1, 1) self.control.throttle = np.clip(action[1], 0, 1) self.control.brake = np.abs(np.clip(action[1], -1, 0)) # prevent braking if not self.allow_braking or self.control.brake < 0.1 or self.control.throttle > self.control.brake: self.control.brake = 0 # prevent over speeding if hasattr(self, 'measurements') and self.measurements[ 0] * 3.6 > self.max_speed and self.control.brake == 0: self.control.throttle = 0.0 self.control.hand_brake = False self.control.reverse = False self.game.send_control(self.control) def _load_experiment(self, experiment_idx): self.current_experiment = self.experiment_suite.get_experiments( )[experiment_idx] self.scene = self.game.load_settings( self.current_experiment.conditions) self.positions = self.scene.player_start_spots self.num_positions = len(self.positions) self.current_start_position_idx = 0 self.current_pose = 0 def _restart_environment_episode(self, force_environment_reset=False): # select start and end positions if self.experiment_suite: # if an expeirent suite is available, follow its given poses if self.current_pose >= len(self.current_experiment.poses): # load a new experiment self.current_experiment_idx = ( self.current_experiment_idx + 1) % len( self.experiment_suite.get_experiments()) self._load_experiment(self.current_experiment_idx) self.current_start_position_idx = self.current_experiment.poses[ self.current_pose][0] self.current_goal = self.positions[self.current_experiment.poses[ self.current_pose][1]] self.current_pose += 1 else: # go over all the possible positions in a cyclic manner self.current_start_position_idx = ( self.current_start_position_idx + 1) % self.num_positions # choose a random goal destination self.current_goal = random.choice(self.positions) try: self.game.start_episode(self.current_start_position_idx) except: self.game.connect() self.game.start_episode(self.current_start_position_idx) # start the game with some initial speed for i in range(self.num_speedup_steps): self.control = VehicleControl(throttle=1.0, brake=0, steer=0, hand_brake=False, reverse=False) self.game.send_control(VehicleControl()) def get_rendered_image(self) -> np.ndarray: """ Return a numpy array containing the image that will be rendered to the screen. This can be different from the observation. For example, mujoco's observation is a measurements vector. :return: numpy array containing the image that will be rendered to the screen """ image = [self.state[camera.name] for camera in self.scene.sensors] image = np.vstack(image) return image
class DrivingBenchmark(object): """ The Benchmark class, controls the execution of the benchmark interfacing an Agent class with a set Suite. The benchmark class must be inherited with a class that defines the all the experiments to be run by the agent """ def __init__(self, city_name='Town01', name_to_save='Test', continue_experiment=False, save_images=False, distance_for_success=2.0): self.__metaclass__ = abc.ABCMeta self._city_name = city_name self._base_name = name_to_save # The minimum distance for arriving into the goal point in # order to consider ir a success self._distance_for_success = distance_for_success # The object used to record the benchmark and to able to continue after self._recording = Recording(name_to_save=name_to_save, continue_experiment=continue_experiment, save_images=save_images) # We have a default planner instantiated that produces high level commands self._planner = Planner(city_name) def benchmark_agent(self, experiment_suite, agent, client): """ Function to benchmark the agent. It first check the log file for this benchmark. if it exist it continues from the experiment where it stopped. Args: experiment_suite agent: an agent object with the run step class implemented. client: Return: A dictionary with all the metrics computed from the agent running the set of experiments. """ # Instantiate a metric object that will be used to compute the metrics for # the benchmark afterwards. metrics_object = Metrics(experiment_suite.metrics_parameters, experiment_suite.dynamic_tasks) # Function return the current pose and task for this benchmark. start_pose, start_experiment = self._recording.get_pose_and_experiment( experiment_suite.get_number_of_poses_task()) logging.info('START') experiments = experiment_suite.get_experiments()[int(start_experiment ):] for e, experiment in enumerate(experiments): positions = client.load_settings( experiment.conditions).player_start_spots self._recording.log_start(experiment.task) poses = experiment.poses[start_pose:] print( 'Benchmarking experiment {} out of {}, which contains {} poses' .format(e + 1, len(experiments), len(poses))) for pose in poses: for rep in range(experiment.repetitions): start_index = pose[0] end_index = pose[1] client.start_episode(start_index) # Print information on logging.info('======== !!!! ==========') logging.info(' Start Position %d End Position %d ', start_index, end_index) self._recording.log_poses(start_index, end_index, experiment.Conditions.WeatherId) # Calculate the initial distance for this episode initial_distance = \ sldist( [positions[start_index].location.x, positions[start_index].location.y], [positions[end_index].location.x, positions[end_index].location.y]) time_out = experiment_suite.calculate_time_out( self._get_shortest_path(positions[start_index], positions[end_index])) # running the agent (result, reward_vec, control_vec, final_time, remaining_distance) = \ self._run_navigation_episode( agent, client, time_out, positions[end_index], str(experiment.Conditions.WeatherId) + '_' + str(experiment.task) + '_' + str(start_index) + '.' + str(end_index)) # Write the general status of the just ran episode self._recording.write_summary_results( experiment, pose, rep, initial_distance, remaining_distance, final_time, time_out, result) # Write the details of this episode. self._recording.write_measurements_results( experiment, rep, pose, reward_vec, control_vec) if result > 0: logging.info( '+++++ Target achieved in %f seconds! +++++', final_time) else: logging.info('----- Timeout! -----') start_pose = 0 self._recording.log_end() return metrics_object.compute(self._recording.path) def get_path(self): """ Returns the path were the log was saved. """ return self._recording.path def _get_directions(self, current_point, end_point): """ Class that should return the directions to reach a certain goal """ directions = self._planner.get_next_command( (current_point.location.x, current_point.location.y, 0.22), (current_point.orientation.x, current_point.orientation.y, current_point.orientation.z), (end_point.location.x, end_point.location.y, 0.22), (end_point.orientation.x, end_point.orientation.y, end_point.orientation.z)) return directions def _get_shortest_path(self, start_point, end_point): """ Calculates the shortest path between two points considering the road netowrk """ return self._planner.get_shortest_path_distance( [start_point.location.x, start_point.location.y, 0.22], [start_point.orientation.x, start_point.orientation.y, 0.22], [end_point.location.x, end_point.location.y, end_point.location.z], [ end_point.orientation.x, end_point.orientation.y, end_point.orientation.z ]) def _run_navigation_episode(self, agent, client, time_out, target, episode_name): """ Run one episode of the benchmark (Pose) for a certain agent. Args: agent: the agent object client: an object of the carla client to communicate with the CARLA simulator time_out: the time limit to complete this episode target: the target to reach episode_name: The name for saving images of this episode """ # Send an initial command. measurements, sensor_data = client.read_data() client.send_control(VehicleControl()) initial_timestamp = measurements.game_timestamp current_timestamp = initial_timestamp # The vector containing all measurements produced on this episode measurement_vec = [] # The vector containing all controls produced on this episode control_vec = [] frame = 0 distance = 10000 success = False while (current_timestamp - initial_timestamp) < (time_out * 1000) and not success: # Read data from server with the client measurements, sensor_data = client.read_data() # The directions to reach the goal are calculated. directions = self._get_directions( measurements.player_measurements.transform, target) # Agent process the data. control = agent.run_step(measurements, sensor_data, directions, target) # Send the control commands to the vehicle client.send_control(control) # save images if the flag is activated self._recording.save_images(sensor_data, episode_name, frame) current_x = measurements.player_measurements.transform.location.x current_y = measurements.player_measurements.transform.location.y logging.info("Controller is Inputting:") logging.info('Steer = %f Throttle = %f Brake = %f ', control.steer, control.throttle, control.brake) current_timestamp = measurements.game_timestamp # Get the distance travelled until now distance = sldist([current_x, current_y], [target.location.x, target.location.y]) # Write status of the run on verbose mode logging.info('Status:') logging.info('[d=%f] c_x = %f, c_y = %f ---> t_x = %f, t_y = %f', float(distance), current_x, current_y, target.location.x, target.location.y) # Check if reach the target if distance < self._distance_for_success: success = True # Increment the vectors and append the measurements and controls. frame += 1 measurement_vec.append(measurements.player_measurements) control_vec.append(control) if success: return 1, measurement_vec, control_vec, float( current_timestamp - initial_timestamp) / 1000.0, distance return 0, measurement_vec, control_vec, time_out, distance
class CarlaEnv(gym.Env): def __init__(self, config=ENV_CONFIG): """ Carla Gym Environment class implementation. Creates an OpenAI Gym compatible driving environment based on Carla driving simulator. :param config: A dictionary with environment configuration keys and values """ self.config = config self.city = self.config["server_map"].split("/")[-1] if self.config["enable_planner"]: self.planner = Planner(self.city) if config["discrete_actions"]: self.action_space = Discrete(len(DISCRETE_ACTIONS)) else: self.action_space = Box(-1.0, 1.0, shape=(2, ), dtype=np.uint8) if config["use_depth_camera"]: image_space = Box(-1.0, 1.0, shape=(config["y_res"], config["x_res"], 1 * config["framestack"]), dtype=np.float32) else: image_space = Box(0.0, 255.0, shape=(config["y_res"], config["x_res"], 3 * config["framestack"]), dtype=np.float32) if self.config["use_image_only_observations"]: self.observation_space = image_space else: self.observation_space = Tuple([ image_space, Discrete(len(COMMANDS_ENUM)), # next_command Box(-128.0, 128.0, shape=(2, ), dtype=np.float32) ]) # forward_speed, dist to goal self._spec = lambda: None self._spec.id = "Carla-v0" self._seed = ENV_CONFIG["seed"] self.server_port = None self.server_process = None self.client = None self.num_steps = 0 self.total_reward = 0 self.prev_measurement = None self.prev_image = None self.episode_id = None self.measurements_file = None self.weather = None self.scenario = None self.start_pos = None self.end_pos = None self.start_coord = None self.end_coord = None self.last_obs = None def init_server(self): print("Initializing new Carla server...") # Create a new server process and start the client. self.server_port = random.randint(10000, 60000) if self.config["render"]: self.server_process = subprocess.Popen([ SERVER_BINARY, self.config["server_map"], "-windowed", "-ResX=1024", "-ResY=768", "-carla-server", "-carla-world-port={}".format(self.server_port) ], preexec_fn=os.setsid, stdout=open( os.devnull, "w")) else: self.server_process = subprocess.Popen( ("SDL_VIDEODRIVER=offscreen SDL_HINT_CUDA_DEVICE={} {} " + self.config["server_map"] + " -windowed -ResX=1024 -ResY=768" " -carla-server -carla-world-port={}").format( 0, SERVER_BINARY, self.server_port), shell=True, preexec_fn=os.setsid, stdout=open(os.devnull, "w")) live_carla_processes.add(os.getpgid(self.server_process.pid)) for i in range(RETRIES_ON_ERROR): try: self.client = CarlaClient("localhost", self.server_port) return self.client.connect() except Exception as e: print("Error connecting: {}, attempt {}".format(e, i)) time.sleep(2) def clear_server_state(self): print("Clearing Carla server state") try: if self.client: self.client.disconnect() self.client = None except Exception as e: print("Error disconnecting client: {}".format(e)) pass if self.server_process: pgid = os.getpgid(self.server_process.pid) os.killpg(pgid, signal.SIGKILL) live_carla_processes.remove(pgid) self.server_port = None self.server_process = None def __del__(self): self.clear_server_state() def reset(self): error = None for _ in range(RETRIES_ON_ERROR): try: if not self.server_process: self.init_server() return self.reset_env() except Exception as e: print("Error during reset: {}".format(traceback.format_exc())) self.clear_server_state() error = e raise error def reset_env(self): self.num_steps = 0 self.total_reward = 0 self.prev_measurement = None self.prev_image = None self.episode_id = datetime.today().strftime("%Y-%m-%d_%H-%M-%S_%f") self.measurements_file = None # Create a CarlaSettings object. This object is a wrapper around # the CarlaSettings.ini file. Here we set the configuration we # want for the new episode. settings = CarlaSettings() # If config["scenarios"] is a single scenario, then use it if it's an array of scenarios, randomly choose one and init if isinstance(self.config["scenarios"], dict): self.scenario = self.config["scenarios"] else: #isinstance array of dict self.scenario = random.choice(self.config["scenarios"]) assert self.scenario["city"] == self.city, (self.scenario, self.city) self.weather = random.choice(self.scenario["weather_distribution"]) settings.set(SynchronousMode=True, SendNonPlayerAgentsInfo=True, NumberOfVehicles=self.scenario["num_vehicles"], NumberOfPedestrians=self.scenario["num_pedestrians"], WeatherId=self.weather) settings.randomize_seeds() if self.config["use_depth_camera"]: camera1 = Camera("CameraDepth", PostProcessing="Depth") camera1.set_image_size(self.config["render_x_res"], self.config["render_y_res"]) camera1.set_position(30, 0, 130) settings.add_sensor(camera1) camera2 = Camera("CameraRGB") camera2.set_image_size(self.config["render_x_res"], self.config["render_y_res"]) camera2.set_position(30, 0, 130) settings.add_sensor(camera2) # Setup start and end positions scene = self.client.load_settings(settings) positions = scene.player_start_spots self.start_pos = positions[self.scenario["start_pos_id"]] self.end_pos = positions[self.scenario["end_pos_id"]] self.start_coord = [ self.start_pos.location.x // 100, self.start_pos.location.y // 100 ] self.end_coord = [ self.end_pos.location.x // 100, self.end_pos.location.y // 100 ] print("Start pos {} ({}), end {} ({})".format( self.scenario["start_pos_id"], self.start_coord, self.scenario["end_pos_id"], self.end_coord)) # Notify the server that we want to start the episode at the # player_start index. This function blocks until the server is ready # to start the episode. print("Starting new episode...") self.client.start_episode(self.scenario["start_pos_id"]) image, py_measurements = self._read_observation() self.prev_measurement = py_measurements return self.encode_obs(self.preprocess_image(image), py_measurements) def encode_obs(self, image, py_measurements): assert self.config["framestack"] in [1, 2] prev_image = self.prev_image self.prev_image = image if prev_image is None: prev_image = image if self.config["framestack"] == 2: image = np.concatenate([prev_image, image], axis=2) if self.config["use_image_only_observations"]: obs = image else: obs = (image, COMMAND_ORDINAL[py_measurements["next_command"]], [ py_measurements["forward_speed"], py_measurements["distance_to_goal"] ]) self.last_obs = obs return obs def step(self, action): try: obs = self.step_env(action) return obs except Exception: print("Error during step, terminating episode early", traceback.format_exc()) self.clear_server_state() return (self.last_obs, 0.0, True, {}) def step_env(self, action): if self.config["discrete_actions"]: action = DISCRETE_ACTIONS[int(action)] assert len(action) == 2, "Invalid action {}".format(action) throttle = float(np.clip(action[0], 0, 1)) brake = float(np.abs(np.clip(action[0], -1, 0))) steer = float(np.clip(action[1], -1, 1)) reverse = False hand_brake = False if self.config["verbose"]: print("steer", steer, "throttle", throttle, "brake", brake, "reverse", reverse) self.client.send_control(steer=steer, throttle=throttle, brake=brake, hand_brake=hand_brake, reverse=reverse) # Process observations image, py_measurements = self._read_observation() if self.config["verbose"]: print("Next command", py_measurements["next_command"]) if type(action) is np.ndarray: py_measurements["action"] = [float(a) for a in action] else: py_measurements["action"] = action py_measurements["control"] = { "steer": steer, "throttle": throttle, "brake": brake, "reverse": reverse, "hand_brake": hand_brake, } reward = self.calculate_reward(py_measurements) self.total_reward += reward py_measurements["reward"] = reward py_measurements["total_reward"] = self.total_reward done = (self.num_steps > self.scenario["max_steps"] or py_measurements["next_command"] == "REACH_GOAL" or (self.config["early_terminate_on_collision"] and check_collision(py_measurements))) py_measurements["done"] = done self.prev_measurement = py_measurements self.num_steps += 1 image = self.preprocess_image(image) return (self.encode_obs(image, py_measurements), reward, done, py_measurements) def preprocess_image(self, image): if self.config["use_depth_camera"]: assert self.config["use_depth_camera"] data = (image.data - 0.5) * 2 data = data.reshape(self.config["render_y_res"], self.config["render_x_res"], 1) data = cv2.resize(data, (self.config["x_res"], self.config["y_res"]), interpolation=cv2.INTER_AREA) data = np.expand_dims(data, 2) else: data = image.data.reshape(self.config["render_y_res"], self.config["render_x_res"], 3) data = cv2.resize(data, (self.config["x_res"], self.config["y_res"]), interpolation=cv2.INTER_AREA) data = (data.astype(np.float32) - 128) / 128 return data def _read_observation(self): # Read the data produced by the server this frame. measurements, sensor_data = self.client.read_data() # Print some of the measurements. if self.config["verbose"]: print_measurements(measurements) observation = None if self.config["use_depth_camera"]: camera_name = "CameraDepth" else: camera_name = "CameraRGB" for name, image in sensor_data.items(): if name == camera_name: observation = image cur = measurements.player_measurements if self.config["enable_planner"]: next_command = COMMANDS_ENUM[self.planner.get_next_command( [cur.transform.location.x, cur.transform.location.y, GROUND_Z], [ cur.transform.orientation.x, cur.transform.orientation.y, GROUND_Z ], [self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [ self.end_pos.orientation.x, self.end_pos.orientation.y, GROUND_Z ])] else: next_command = "LANE_FOLLOW" if next_command == "REACH_GOAL": distance_to_goal = 0.0 # avoids crash in planner elif self.config["enable_planner"]: distance_to_goal = self.planner.get_shortest_path_distance([ cur.transform.location.x, cur.transform.location.y, GROUND_Z ], [ cur.transform.orientation.x, cur.transform.orientation.y, GROUND_Z ], [self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [ self.end_pos.orientation.x, self.end_pos.orientation.y, GROUND_Z ]) / 100 else: distance_to_goal = -1 distance_to_goal_euclidean = float( np.linalg.norm([ cur.transform.location.x - self.end_pos.location.x, cur.transform.location.y - self.end_pos.location.y ]) / 100) py_measurements = { "episode_id": self.episode_id, "step": self.num_steps, "x": cur.transform.location.x, "y": cur.transform.location.y, "x_orient": cur.transform.orientation.x, "y_orient": cur.transform.orientation.y, "forward_speed": cur.forward_speed, "distance_to_goal": distance_to_goal, "distance_to_goal_euclidean": distance_to_goal_euclidean, "collision_vehicles": cur.collision_vehicles, "collision_pedestrians": cur.collision_pedestrians, "collision_other": cur.collision_other, "intersection_offroad": cur.intersection_offroad, "intersection_otherlane": cur.intersection_otherlane, "weather": self.weather, "map": self.config["server_map"], "start_coord": self.start_coord, "end_coord": self.end_coord, "current_scenario": self.scenario, "x_res": self.config["x_res"], "y_res": self.config["y_res"], "num_vehicles": self.scenario["num_vehicles"], "num_pedestrians": self.scenario["num_pedestrians"], "max_steps": self.scenario["max_steps"], "next_command": next_command, } assert observation is not None, sensor_data return observation, py_measurements def calculate_reward(self, current_measurement): """ Calculate the reward based on the effect of the action taken using the previous and the current measurements :param current_measurement: The measurement obtained from the Carla engine after executing the current action :return: The scalar reward """ reward = 0.0 cur_dist = current_measurement["distance_to_goal"] prev_dist = self.prev_measurement["distance_to_goal"] if self.config["verbose"]: print("Cur dist {}, prev dist {}".format(cur_dist, prev_dist)) # Distance travelled toward the goal in m reward += np.clip(prev_dist - cur_dist, -10.0, 10.0) # Change in speed (km/hr) reward += 0.05 * (current_measurement["forward_speed"] - self.prev_measurement["forward_speed"]) # New collision damage reward -= .00002 * (current_measurement["collision_vehicles"] + current_measurement["collision_pedestrians"] + current_measurement["collision_other"] - self.prev_measurement["collision_vehicles"] - self.prev_measurement["collision_pedestrians"] - self.prev_measurement["collision_other"]) # New sidewalk intersection reward -= 2 * (current_measurement["intersection_offroad"] - self.prev_measurement["intersection_offroad"]) # New opposite lane intersection reward -= 2 * (current_measurement["intersection_otherlane"] - self.prev_measurement["intersection_otherlane"]) return reward
class CarlaEnv(gym.Env): def __init__(self, config=ENV_CONFIG): self.config = config self.city = self.config["server_map"].split("/")[-1] if self.config["enable_planner"]: self.planner = Planner(self.city) # The Action Space if config["discrete_actions"]: self.action_space = Discrete(len(DISCRETE_ACTIONS)) # It will be transformed to continuous 2D action. else: self.action_space = Box(-1.0, 1.0, shape=(2, ), dtype=np.float32) # 2D action. if config["use_depth_camera"]: image_space = Box( -1.0, 1.0, shape=(config["y_res"], config["x_res"], 1 * config["framestack"]), dtype=np.float32) else: image_space = Box( 0, 255, shape=(config["y_res"], config["x_res"], 3 * config["framestack"]), dtype=np.uint8) # The Observation Space self.observation_space = Tuple( [ image_space, Discrete(len(COMMANDS_ENUM)), # next_command Box(-128.0, 128.0, shape=(2, ), dtype=np.float32) # forward_speed, dist to goal ]) # TODO(ekl) this isn't really a proper gym spec self._spec = lambda: None self._spec.id = "Carla-v0" self.server_port = None self.server_process = None self.client = None self.num_steps = 0 self.total_reward = 0 self.prev_measurement = None self.prev_image = None self.episode_id = None self.measurements_file = None self.weather = None self.scenario = None self.start_pos = None self.end_pos = None self.start_coord = None self.end_coord = None self.last_obs = None def init_server(self): print("Initializing new Carla server...") # Create a new server process and start the client. self.server_port = random.randint(10000, 60000) self.server_process = subprocess.Popen( [ SERVER_BINARY, self.config["server_map"], "-windowed", "-ResX=800", "-ResY=600", "-carla-server", "-benchmark -fps=10", # "-benchmark -fps=10": to run the simulation at a fixed time-step of 0.1 seconds "-carla-world-port={}".format(self.server_port) ], preexec_fn=os.setsid, stdout=open(os.devnull, "w")) live_carla_processes.add(os.getpgid(self.server_process.pid)) for i in range(RETRIES_ON_ERROR): try: self.client = CarlaClient("localhost", self.server_port) return self.client.connect() except Exception as e: print("Error connecting: {}, attempt {}".format(e, i)) time.sleep(2) def clear_server_state(self): print("Clearing Carla server state") try: if self.client: self.client.disconnect() self.client = None except Exception as e: print("Error disconnecting client: {}".format(e)) pass if self.server_process: pgid = os.getpgid(self.server_process.pid) os.killpg(pgid, signal.SIGKILL) live_carla_processes.remove(pgid) self.server_port = None self.server_process = None def __del__(self): # the __del__ method will be called when the instance of the class is deleted.(memory is freed.) self.clear_server_state() def reset(self): error = None for _ in range(RETRIES_ON_ERROR): try: if not self.server_process: self.init_server() return self._reset() except Exception as e: print("Error during reset: {}".format(traceback.format_exc())) self.clear_server_state() error = e raise error def _reset(self): self.num_steps = 0 self.total_reward = 0 self.prev_measurement = None self.prev_image = None self.episode_id = datetime.today().strftime("%Y-%m-%d_%H-%M-%S_%f") self.measurements_file = None # Create a CarlaSettings object. This object is a wrapper around # the CarlaSettings.ini file. Here we set the configuration we # want for the new episode. settings = CarlaSettings() self.scenario = random.choice(self.config["scenarios"]) assert self.scenario["city"] == self.city, (self.scenario, self.city) self.weather = random.choice(self.scenario["weather_distribution"]) settings.set( SynchronousMode=True, # ServerTimeOut=10000, # CarlaSettings: no key named 'ServerTimeOut' SendNonPlayerAgentsInfo=True, NumberOfVehicles=self.scenario["num_vehicles"], NumberOfPedestrians=self.scenario["num_pedestrians"], WeatherId=self.weather) settings.randomize_seeds() if self.config["use_depth_camera"]: camera1 = Camera("CameraDepth", PostProcessing="Depth") camera1.set_image_size(self.config["render_x_res"], self.config["render_y_res"]) # camera1.set_position(30, 0, 130) camera1.set_position(0.5, 0.0, 1.6) # camera1.set_rotation(0.0, 0.0, 0.0) settings.add_sensor(camera1) camera2 = Camera("CameraRGB") camera2.set_image_size(self.config["render_x_res"], self.config["render_y_res"]) # camera2.set_position(30, 0, 130) # camera2.set_position(0.3, 0.0, 1.3) # camera2.set_position(2.0, 0.0, 1.4) # camera2.set_rotation(0.0, 0.0, 0.0) camera2.set_position(0.5, 0.0, 1.6) settings.add_sensor(camera2) # Setup start and end positions scene = self.client.load_settings(settings) positions = scene.player_start_spots self.start_pos = positions[self.scenario["start_pos_id"]] self.end_pos = positions[self.scenario["end_pos_id"]] self.start_coord = [ self.start_pos.location.x // 100, self.start_pos.location.y // 100 ] self.end_coord = [ self.end_pos.location.x // 100, self.end_pos.location.y // 100 ] print("Start pos {} ({}), end {} ({})".format( self.scenario["start_pos_id"], self.start_coord, self.scenario["end_pos_id"], self.end_coord)) # Notify the server that we want to start the episode at the # player_start index. This function blocks until the server is ready # to start the episode. print("Starting new episode...") self.client.start_episode(self.scenario["start_pos_id"]) # Process observations: self._read_observation() returns image and py_measurements. image, py_measurements = self._read_observation() self.prev_measurement = py_measurements return self.encode_obs(self.preprocess_image(image), py_measurements) def encode_obs(self, image, py_measurements): assert self.config["framestack"] in [1, 2] prev_image = self.prev_image self.prev_image = image if prev_image is None: prev_image = image if self.config["framestack"] == 2: # image = np.concatenate([prev_image, image], axis=2) '''this works when use depth camera and frame 2''' image = np.concatenate([image, np.zeros_like(image)+py_measurements["forward_speed"]/30], axis=2) # obs = (image, COMMAND_ORDINAL[py_measurements["next_command"]], [ # py_measurements["forward_speed"], # py_measurements["distance_to_goal"] # ]) obs = image # can we encode speed into imagine self.last_obs = obs return obs def step(self, action): try: obs = self._step(action) return obs except Exception: print("Error during step, terminating episode early", traceback.format_exc()) self.clear_server_state() return (self.last_obs, 0.0, True, {}) def _step(self, action): if self.config["discrete_actions"]: action = DISCRETE_ACTIONS[int(action)] # Carla action is 2D. assert len(action) == 2, "Invalid action {}".format(action) if self.config["squash_action_logits"]: forward = 2 * float(sigmoid(action[0]) - 0.5) throttle = float(np.clip(forward, 0, 1)) brake = float(np.abs(np.clip(forward, -1, 0))) steer = 2 * float(sigmoid(action[1]) - 0.5) else: throttle = float(np.clip(action[0], 0, 1)) brake = float(np.abs(np.clip(action[0], -1, 0))) steer = float(np.clip(action[1], -1, 1)) # reverse and hand_brake are disabled. reverse = False hand_brake = False if self.config["verbose"]: print("steer", steer, "throttle", throttle, "brake", brake, "reverse", reverse) self.client.send_control( steer=steer, throttle=throttle, brake=brake, hand_brake=hand_brake, reverse=reverse) # Process observations: self._read_observation() returns image and py_measurements. image, py_measurements = self._read_observation() if self.config["verbose"]: print("Next command", py_measurements["next_command"]) if type(action) is np.ndarray: py_measurements["action"] = [float(a) for a in action] else: py_measurements["action"] = action py_measurements["control"] = { "steer": steer, "throttle": throttle, "brake": brake, "reverse": reverse, "hand_brake": hand_brake, } # compute reward reward = compute_reward(self, self.prev_measurement, py_measurements) self.total_reward += reward py_measurements["reward"] = reward py_measurements["total_reward"] = self.total_reward # done or not done = (self.num_steps > self.scenario["max_steps"] or py_measurements["next_command"] == "REACH_GOAL" or (self.config["early_terminate_on_collision"] and collided_done(py_measurements))) py_measurements["done"] = done self.prev_measurement = py_measurements # Write out measurements to file if self.config["verbose"] and CARLA_OUT_PATH: if not self.measurements_file: self.measurements_file = open( os.path.join( CARLA_OUT_PATH, "measurements_{}.json".format(self.episode_id)), "w") self.measurements_file.write(json.dumps(py_measurements)) self.measurements_file.write("\n") if done: self.measurements_file.close() self.measurements_file = None if self.config["convert_images_to_video"]: self.images_to_video() self.num_steps += 1 image = self.preprocess_image(image) return (self.encode_obs(image, py_measurements), reward, done, py_measurements) def images_to_video(self): videos_dir = os.path.join(CARLA_OUT_PATH, "Videos") if not os.path.exists(videos_dir): os.makedirs(videos_dir) ffmpeg_cmd = ( "ffmpeg -loglevel -8 -r 60 -f image2 -s {x_res}x{y_res} " "-start_number 0 -i " "{img}_%04d.jpg -vcodec libx264 {vid}.mp4 && rm -f {img}_*.jpg " ).format( x_res=self.config["render_x_res"], y_res=self.config["render_y_res"], vid=os.path.join(videos_dir, self.episode_id), img=os.path.join(CARLA_OUT_PATH, "CameraRGB", self.episode_id)) print("Executing ffmpeg command", ffmpeg_cmd) subprocess.call(ffmpeg_cmd, shell=True) def preprocess_image(self, image): if self.config["use_depth_camera"]: assert self.config["use_depth_camera"] data = (image.data - 0.5) * 2 data = data.reshape(self.config["render_y_res"], self.config["render_x_res"], 1) data = cv2.resize( data, (self.config["x_res"], self.config["y_res"]), interpolation=cv2.INTER_AREA) data = np.expand_dims(data, 2) else: data = image.data.reshape(self.config["render_y_res"], self.config["render_x_res"], 3) data = cv2.resize( data, (self.config["x_res"], self.config["y_res"]), interpolation=cv2.INTER_AREA) data = (data.astype(np.float32) - 128) / 128 return data def _read_observation(self): # Read the data produced by the server this frame. measurements, sensor_data = self.client.read_data() # Print some of the measurements. if self.config["verbose"]: print_measurements(measurements) observation = None if self.config["use_depth_camera"]: camera_name = "CameraDepth" else: camera_name = "CameraRGB" for name, image in sensor_data.items(): if name == camera_name: observation = image cur = measurements.player_measurements if self.config["enable_planner"]: next_command = COMMANDS_ENUM[self.planner.get_next_command( [cur.transform.location.x, cur.transform.location.y, GROUND_Z], [ cur.transform.orientation.x, cur.transform.orientation.y, GROUND_Z ], [self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [ self.end_pos.orientation.x, self.end_pos.orientation.y, GROUND_Z ])] else: next_command = "LANE_FOLLOW" if next_command == "REACH_GOAL": distance_to_goal = 0.0 # avoids crash in planner elif self.config["enable_planner"]: distance_to_goal = self.planner.get_shortest_path_distance([ cur.transform.location.x, cur.transform.location.y, GROUND_Z ], [ cur.transform.orientation.x, cur.transform.orientation.y, GROUND_Z ], [self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [ self.end_pos.orientation.x, self.end_pos.orientation.y, GROUND_Z ]) / 100 else: distance_to_goal = -1 distance_to_goal_euclidean = float( np.linalg.norm([ cur.transform.location.x - self.end_pos.location.x, cur.transform.location.y - self.end_pos.location.y ]) / 100) py_measurements = { "episode_id": self.episode_id, "step": self.num_steps, "x": cur.transform.location.x, "y": cur.transform.location.y, "x_orient": cur.transform.orientation.x, "y_orient": cur.transform.orientation.y, "forward_speed": cur.forward_speed, "distance_to_goal": distance_to_goal, "distance_to_goal_euclidean": distance_to_goal_euclidean, "collision_vehicles": cur.collision_vehicles, "collision_pedestrians": cur.collision_pedestrians, "collision_other": cur.collision_other, "intersection_offroad": cur.intersection_offroad, "intersection_otherlane": cur.intersection_otherlane, "weather": self.weather, "map": self.config["server_map"], "start_coord": self.start_coord, "end_coord": self.end_coord, "current_scenario": self.scenario, "x_res": self.config["x_res"], "y_res": self.config["y_res"], "num_vehicles": self.scenario["num_vehicles"], "num_pedestrians": self.scenario["num_pedestrians"], "max_steps": self.scenario["max_steps"], "next_command": next_command, } if CARLA_OUT_PATH and self.config["log_images"]: for name, image in sensor_data.items(): out_dir = os.path.join(CARLA_OUT_PATH, name) if not os.path.exists(out_dir): os.makedirs(out_dir) out_file = os.path.join( out_dir, "{}_{:>04}.jpg".format(self.episode_id, self.num_steps)) scipy.misc.imsave(out_file, image.data) assert observation is not None, sensor_data return observation, py_measurements
def main(): argparser = argparse.ArgumentParser( description='CARLA Manual Control Client') argparser.add_argument('-v', '--verbose', action='store_true', dest='debug', help='print debug information') argparser.add_argument('--host', metavar='H', default='localhost', help='IP of the host server (default: localhost)') argparser.add_argument('-p', '--port', metavar='P', default=2000, type=int, help='TCP port to listen to (default: 2000)') argparser.add_argument( '-m', '--map-name', metavar='M', default='Town01', help='plot the map of the current city (needs to match active map in ' 'server, options: Town01 or Town02)') argparser.add_argument( '-c', '--city-name', metavar='C', default='Town01', help='The town that is going to be used on benchmark ' '(needs to match active town in server, options: Town01 or Town02)') argparser.add_argument('-n', '--enable-noise', action='store_true', help='Add noise to player commands') argparser.add_argument( '-mo', '--collection-mode', default='keyboard', help= 'For data collection: keyboard = use keyboard, steering = use steering wheel, auto = use autopilot' ) argparser.add_argument('-s', '--save-data', action='store_true', help='Store the demonstrated data to disk') argparser.add_argument('-l', '--player-name', metavar='L', default='AshishMehta', help='Name of the demonstrator') args = argparser.parse_args() log_level = logging.DEBUG if args.debug else logging.INFO logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) logging.info('listening to server %s:%s', args.host, args.port) print(__doc__) if args.save_data: file_count = 0 if not os.path.exists('./Data_Set/' + args.player_name): os.makedirs('./Data_Set/' + args.player_name) for _ in glob.glob('./Data_Set/' + args.player_name + '/*'): file_count += 1 file_count = int(file_count / 2) f = h5py.File( "./Data_Set/" + args.player_name + "/Training_Data{}.h5".format(file_count), "w") if not os.path.exists('./Data_Set/' + args.player_name + '/Training_Data{}/'.format(file_count)): os.makedirs('./Data_Set/' + args.player_name + '/Training_Data{}/'.format(file_count)) json_folder = './Data_Set/' + args.player_name + '/Training_Data{}/'.format( file_count) dset_target = f.create_dataset('targets', (1, 34), maxshape=(None, 50), dtype=np.float128) dset_im_front = f.create_dataset('Camera/RGB_front', (1, WINDOW_HEIGHT, WINDOW_WIDTH, 3), maxshape=(None, WINDOW_HEIGHT, WINDOW_WIDTH, 3), dtype=np.uint8) dset_im_left = f.create_dataset( 'Camera/RGB_left', (1, MINI_WINDOW_HEIGHT, MINI_WINDOW_WIDTH, 3), maxshape=(None, MINI_WINDOW_HEIGHT, MINI_WINDOW_WIDTH, 3), dtype=np.uint8) dset_im_right = f.create_dataset( 'Camera/RGB_right', (1, MINI_WINDOW_HEIGHT, MINI_WINDOW_WIDTH, 3), maxshape=(None, MINI_WINDOW_HEIGHT, MINI_WINDOW_WIDTH, 3), dtype=np.uint8) dset_im = [dset_im_front, dset_im_left, dset_im_right] else: dset_target = None dset_im = None json_folder = None while True: try: with make_carla_client(args.host, args.port) as client: planner_obj = Planner(args.city_name) game = CarlaGame(client, planner_obj, args.map_name, args.city_name, args.save_data, args.enable_noise, args.collection_mode) game.execute(dset_target, dset_im, json_folder) break except TCPConnectionError as error: logging.error(error) time.sleep(1)