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=400", "-ResY=300", "-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=400 -ResY=300" " -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 __init__(self, target=(158.08, 27.18)): # action space range: steer, throttle, brake min_steer, max_steer = -1, 1 min_throttle, max_throttle = 0, 1 min_brake, max_brake = 0, 1 self.action_space = spaces.Box(low=np.array([min_steer, -max_brake]), high=np.array([max_steer, max_throttle]), dtype=np.float32) # observation, 3 channel image self.observation_space = spaces.Box( low=0, high=1.0, shape=(3, carla_config.CARLA_IMG_HEIGHT, carla_config.CARLA_IMG_WIDTH), dtype=np.float32) self.viewer = ImageViewer() self.rng = np.random.RandomState() # used for random number generators self.target = np.array(target) self.cur_image = None # image with (H, W, C) self.cur_measurements = None self.carla_client = CarlaClient(carla_config.CARLA_HOST_ADDRESS, carla_config.CARLA_HOST_PORT, timeout=100) self.carla_client.connect() if self.carla_client.connected(): print("Successfully connected to", end=" ") else: print("Failed to connect", end=" ") print( f"Carla on {carla_config.CARLA_HOST_ADDRESS}:{carla_config.CARLA_HOST_PORT}" )
def execute(self): # Subscribe to control streams self.control = { 'steer': 0.0, 'throttle': 0.0, 'break': 0.0, 'hand_break': False, 'reverse': False } self.client = CarlaClient(self._flags.carla_host, self._flags.carla_port, timeout=10) self.client.connect() scene = self.client.load_settings(self.settings) # Choose one player start at random. number_of_player_starts = len(scene.player_start_spots) player_start = self._flags.carla_start_player_num if self._flags.carla_random_player_start: player_start = np.random.randint( 0, max(0, number_of_player_starts - 1)) self.client.start_episode(player_start) self.read_data_at_frequency() self.spin()
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=480", "-ResY=360", "-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") ) # ResourceWarning: unclosed file <_io.TextIOWrapper name='/dev/null' mode='w' encoding='UTF-8'> 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 start(self): if __CARLA_VERSION__ == '0.8.X': self.carla = CarlaClient(self._host, int(self._port), timeout=120) self.carla.connect() else: self.carla = CarlaClient(self._host, int(self._port)) self.carla.set_timeout(5000) wd = self.carla.get_world() self.wd = wd settings = wd.get_settings() settings.synchronous_mode = True wd.apply_settings(settings) self._reset() if not self._autopilot: pygame.joystick.init() joystick_count = pygame.joystick.get_count() if joystick_count > 1: print("Please Connect Just One Joystick") raise ValueError() self.joystick = pygame.joystick.Joystick(0) self.joystick.init()
def _setup_carla_client(self): carla_client = CarlaClient(self._params['host'], self._params['port'], timeout=None) carla_client.connect() ### create initial settings carla_settings = CarlaSettings() carla_settings.set( SynchronousMode=True, SendNonPlayerAgentsInfo=False, NumberOfVehicles=self._params['number_of_vehicles'], NumberOfPedestrians=self._params['number_of_pedestrians'], WeatherId=self._params['weather']) carla_settings.randomize_seeds() ### add cameras for camera_name in self._params['cameras']: camera_params = self._params[camera_name] camera_postprocessing = camera_params['postprocessing'] camera = Camera(camera_name, PostProcessing=camera_postprocessing) camera.set_image_size(CarlaCollSpeedEnv.CAMERA_HEIGHT * CarlaCollSpeedEnv.WIDTH_TO_HEIGHT_RATIO, CarlaCollSpeedEnv.CAMERA_HEIGHT) camera.set_position(*camera_params['position']) camera.set(**{'FOV': camera_params['fov']}) carla_settings.add_sensor(camera) ### load settings carla_scene = carla_client.load_settings(carla_settings) self._carla_client = carla_client self._carla_settings = carla_settings self._carla_scene = carla_scene
def setup_client_and_server(self, reconnect_client_only=False): # open the server if not reconnect_client_only: self.server, self.out = self._open_server() self.server_pid = self.server.pid # To kill incase child process gets lost print("setup server, out:", self.server_pid, "dockerid", self.out) while True: try: self.game = CarlaClient(self.host, self.port, timeout=99999999) self.game.connect( connection_attempts=100 ) # It's taking a very long time for the server process to spawn, so the client needs to wait or try sufficient no. of times lol self.game.load_settings(CarlaSettings()) self.game.start_episode(0) self.is_game_setup = self.server and self.game print("setup client") # self.out = shell("docker ps -q") # print("dockerid", self.out) return except TCPConnectionError as error: print(error) time.sleep(1)
def setup_client_and_server(self, reconnect_client_only=False, settings=None): # open the server if not reconnect_client_only: self.server = self._open_server() self.server_pid = self.server.pid # To kill incase child process gets lost # open the client self.game = CarlaClient(self.host, self.port, timeout=99999999) # It's taking a very long time for the server process to spawn, so the client needs to wait or try sufficient no. of times lol self.game.connect(connection_attempts=100) self.scene = self.game.load_settings(self.settings) # get available start positions positions = self.scene.player_start_spots start_loc = positions[self.start_loc_idx].location end_loc = positions[self.end_loc_idx].location self.end_loc = end_loc self.last_distance = self.cal_distance(start_loc, end_loc) self.curr_distance = self.last_distance self.end_loc_measurement = [end_loc.x, end_loc.y, end_loc.z] # self.num_pos = len(positions) # self.iterator_start_positions = 0 self.is_game_setup = self.server and self.game return
def __init__(self, host="localhost", num_vehicles=1, vehicles_seed=lambda: 200, player_starts=2, goals=0): self.num_vehicles = num_vehicles self.vehicles_seed = vehicles_seed self.starts = list_from_scalar(player_starts) self.goals = list_from_scalar(goals) self.port = get_open_port() self.host = host self.metadata = { 'render.modes': ['rgb_array'], #'video.frames_per_second': int(np.round(1.0 / self.dt)) } self.server = self.open_server() print(f"Connecting to CARLA Client on port {self.port}") self.client = CarlaClient(self.host, self.port, timeout=99999999) time.sleep(3) self.client.connect(connection_attempts=1000) print(f"Connected on port: {self.port}") self.action_space = gym.spaces.Box( -2, 2, (len(self._map_controls([1, 2, 3, 4, 5]).items()), ), dtype=np.float32) obs_size = len(self.reset()) self.observation_space = gym.spaces.Box(-float("inf"), float("inf"), (obs_size, ), dtype=np.float32)
def setup_client_and_server(self, reconnect_client_only=False): # open the server if not reconnect_client_only: self.server, self.out = self._open_server() self.server_pid = self.server.pid # To kill incase child process gets lost print("setup server, out:", self.out) while True: try: self.game = CarlaClient(self.host, self.port, timeout=99999999) self.game.connect( connection_attempts=100 ) # It's taking a very long time for the server process to spawn, so the client needs to wait or try sufficient no. of times lol scene = self.game.load_settings(self.settings) # get available start positions positions = scene.player_start_spots self.num_pos = len(positions) self.iterator_start_positions = 0 self.is_game_setup = self.server and self.game print("setup client") return except TCPConnectionError as error: print(error) time.sleep(1) return
def reset(self): print('reset!') print('=.' * 40) a = 0 log_level = logging.INFO logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) if os.path.exists('port.txt'): with open('port.txt', 'r') as f: a = int(f.readlines()[0].strip()) print(a) print('=.' * 40) with open('port.txt', 'w') as f: f.write(str(a + 1) + '\n') logging.info('listening to server %s:%s', 'localhost', 7899) while True: try: client = CarlaClient('localhost', 7899, timeout=1000) client.connect() self.carla_game = CarlaGame(client) self.carla_game.reset() obs = self.carla_game.get_obs() self.carla_game.display() return obs[0] except TCPConnectionError as error: logging.error(error) time.sleep(1)
def __init__(self, target): self.carla_client = CarlaClient(config.CARLA_HOST_ADDRESS, config.CARLA_HOST_PORT, timeout=100) self.carla_client.connect() self.target = target self.pre_image = None self.cur_image = None self.pre_measurements = None self.cur_measurements = None
def __init__(self, args): cmd.Cmd.__init__(self) self.args = args self.prompt = '\x1b[1;34m%s\x1b[0m ' % '(carla)' self.client = CarlaClient(args.host, args.port) self.settings = get_default_carla_settings(args) self.print_measurements = False self.control = _Control() self.thread = threading.Thread(target=self._agent_thread_worker) self.done = False self.thread.start()
def reset(self): # connect to server self._client = CarlaClient(self._host, self._port, self._timeout) self._client.connect() # settings index = random.randint(0, 4) # Last one is for testing # print(index) # index = 0 seed = npc_vehicle_seeds[index] start_list_index = start_list_indices[index] settings = CarlaSettings() settings.set(SynchronousMode=True, SendNonPlayerAgentsInfo=True, NumberOfVehicles=1, NumberOfPedestrians=0, SeedVehicles=seed, SeedPedestrians=123456789, WeatherId=weathers[index]) self._client.load_settings(settings) self._client.start_episode(start_list_index) # simulator init stage for i in range(10): measurements, _ = self._client.read_data() self._client.send_control(steer=0, throttle=0, brake=0, hand_brake=False, reverse=False) self._client.send_control(steer=0, throttle=0, brake=0, hand_brake=False, reverse=False) # Reset Flags self._reset = True self._history_path = np.zeros([self._history_path_num, 4]) self._cur_steer = 0 self._his_steer = 0 observation = self._observe() self._observation = observation return observation
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, "/Game/Maps/Town02", "-windowed", "-ResX=400", "-ResY=300", "-carla-server", "-carla-world-port={}".format( self.server_port) ], preexec_fn=os.setsid, stdout=open(os.devnull, "w")) self.client = CarlaClient("localhost", self.server_port) self.client.connect()
def _make_carla_client(self, host, port): while True: try: self.logger.info( "Trying to make client on port {}".format(port)) self._client = CarlaClient(host, port, timeout=100) self._client.connect() self._client.load_settings(CarlaSettings(QualityLevel='Low')) self._client.start_episode(0) self.logger.info( "Successfully made client on port {}".format(port)) break except TCPConnectionError as error: self.logger.debug('Got TCPConnectionError..sleeping for 1') self.logger.error(error) time.sleep(1)
def execute(self): # Connect to the simulator. self.client = CarlaClient(self._flags.carla_host, self._flags.carla_port, timeout=self._flags.carla_timeout) self.client.connect() scene = self.client.load_settings(self._settings) # Choose one player start at random. number_of_player_starts = len(scene.player_start_spots) player_start = self._flags.carla_start_player_num if self._flags.carla_random_player_start: player_start = np.random.randint( 0, max(0, number_of_player_starts - 1)) self.client.start_episode(player_start) self._tick_at = time.time() self._tick_simulator() self.spin()
def __init__( self, host='eceftl1.ces.clemson.edu', port=2000, image_filename_format='_images/episode_{:0>3d}/{:s}/image_{:0>5d}.png', save_images_to_disk=False): self.image_filename_format = image_filename_format self.save_images_to_disk = save_images_to_disk self.host = host self.port = port self.timeout = 60 self._Observation = collections.namedtuple('Observation', ['image', 'label']) self.frame = 0 self.episode_index = -1 self.client = CarlaClient(self.host, self.port, self.timeout)
def _make_carla_client(self, host, port): for _ in range( 20 ): # Try to connect for up to 20 times (usually connects first attempt if everything is correct) try: self.logger.info( "Trying to make client on port {}".format(port)) self._client = CarlaClient(host, port, timeout=100) self._client.connect() self._client.load_settings(CarlaSettings(QualityLevel='Low')) self._client.start_episode(0) self.logger.info( "Successfully made client on port {}".format(port)) break except TCPConnectionError as error: self.logger.debug('Got TCPConnectionError..sleeping for 1') self.logger.error(error) time.sleep(1)
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", "-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 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)
class CarlaLegacyOperator(Op): """ CarlaLegacyOperator initializes and controls the simulator. This operator connects to the simulator, spawns actors, gets and publishes ground info, and sends vehicle commands. The operator works with Carla 0.8.4. """ def __init__(self, name, flags, auto_pilot, camera_setups=[], lidar_setups=[], log_file_name=None, csv_file_name=None): super(CarlaLegacyOperator, self).__init__(name) self._flags = flags self._logger = setup_logging(self.name, log_file_name) self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name) self._auto_pilot = auto_pilot if self._flags.carla_high_quality: quality = 'Epic' else: quality = 'Low' self._settings = CarlaSettings() self._settings.set( SynchronousMode=self._flags.carla_synchronous_mode, SendNonPlayerAgentsInfo=True, NumberOfVehicles=self._flags.carla_num_vehicles, NumberOfPedestrians=self._flags.carla_num_pedestrians, WeatherId=self._flags.carla_weather, QualityLevel=quality) self._settings.randomize_seeds() self._transforms = {} # Add cameras to the simulation. for cs in camera_setups: self.__add_camera(cs) self._transforms[cs.name] = cs.get_transform() # Add lidars to the simulation. for ls in lidar_setups: self.__add_lidar(ls) self._transforms[ls.name] = ls.get_transform() self.agent_id_map = {} self.pedestrian_count = 0 # Initialize the control state. self.control = { 'steer': 0.0, 'throttle': 0.0, 'brake': 0.0, 'hand_brake': False, 'reverse': False } # Register custom serializers for Messages and WatermarkMessages ray.register_custom_serializer(Message, use_pickle=True) ray.register_custom_serializer(WatermarkMessage, use_pickle=True) @staticmethod def setup_streams(input_streams, camera_setups, lidar_setups): input_streams.add_callback(CarlaLegacyOperator.on_control_msg) camera_streams = [ pylot.utils.create_camera_stream(cs) for cs in camera_setups ] lidar_streams = [ pylot.utils.create_lidar_stream(ls) for ls in lidar_setups ] return [ pylot.utils.create_can_bus_stream(), pylot.utils.create_ground_traffic_lights_stream(), pylot.utils.create_ground_vehicles_stream(), pylot.utils.create_ground_pedestrians_stream(), pylot.utils.create_ground_speed_limit_signs_stream(), pylot.utils.create_ground_stop_signs_stream() ] + camera_streams + lidar_streams def __add_camera(self, camera_setup): """Adds a camera and a corresponding output stream. Args: camera_setup: A camera setup object. """ # Transform from Carla 0.9.x postprocessing strings to Carla 0.8.4. if camera_setup.camera_type == 'sensor.camera.rgb': postprocessing = 'SceneFinal' elif camera_setup.camera_type == 'sensor.camera.depth': postprocessing = 'Depth' elif camera_setup.camera_type == 'sensor.camera.semantic_segmentation': postprocessing = 'SemanticSegmentation' transform = camera_setup.get_transform() camera = Camera(name=camera_setup.name, PostProcessing=postprocessing, FOV=camera_setup.fov, ImageSizeX=camera_setup.width, ImageSizeY=camera_setup.height, PositionX=transform.location.x, PositionY=transform.location.y, PositionZ=transform.location.z, RotationPitch=transform.rotation.pitch, RotationRoll=transform.rotation.roll, RotationYaw=transform.rotation.yaw) self._settings.add_sensor(camera) def __add_lidar(self, lidar_setup): """Adds a LIDAR sensor and a corresponding output stream. Args: lidar_setup: A LidarSetup object.. """ transform = lidar_setup.get_transform() lidar = Lidar(lidar_setup.name, Channels=lidar_setup.channels, Range=lidar_setup.range, PointsPerSecond=lidar_setup.points_per_second, RotationFrequency=lidar_setup.rotation_frequency, UpperFovLimit=lidar_setup.upper_fov, LowerFovLimit=lidar_setup.lower_fov, PositionX=transform.location.x, PositionY=transform.location.y, PositionZ=transform.location.z, RotationPitch=transform.rotation.pitch, RotationYaw=transform.rotation.yaw, RotationRoll=transform.rotation.roll) self._settings.add_sensor(lidar) def publish_world_data(self): read_start_time = time.time() measurements, sensor_data = self.client.read_data() measure_time = time.time() self._logger.info( 'Got readings for game time {} and platform time {}'.format( measurements.game_timestamp, measurements.platform_timestamp)) timestamp = Timestamp(coordinates=[measurements.game_timestamp]) watermark = WatermarkMessage(timestamp) # Send player data on data streams. self.__send_player_data(measurements.player_measurements, timestamp, watermark) # Extract agent data from measurements. agents = self.__get_ground_agents(measurements) # Send agent data on data streams. self.__send_ground_agent_data(agents, timestamp, watermark) # Send sensor data on data stream. self.__send_sensor_data(sensor_data, timestamp, watermark) # Get Autopilot control data. if self._auto_pilot: self.control = measurements.player_measurements.autopilot_control end_time = time.time() measurement_runtime = (measure_time - read_start_time) * 1000 total_runtime = (end_time - read_start_time) * 1000 self._logger.info('Carla measurement time {}; total time {}'.format( measurement_runtime, total_runtime)) self._csv_logger.info('{},{},{},{}'.format(time_epoch_ms(), self.name, measurement_runtime, total_runtime)) def __send_player_data(self, player_measurements, timestamp, watermark): """ Sends hero vehicle information. It populates a CanBus tuple. """ location = pylot.simulation.utils.Location( carla_loc=player_measurements.transform.location) rotation = pylot.simulation.utils.Rotation( player_measurements.transform.rotation.pitch, player_measurements.transform.rotation.yaw, player_measurements.transform.rotation.roll) orientation = pylot.simulation.utils.Orientation( player_measurements.transform.orientation.x, player_measurements.transform.orientation.y, player_measurements.transform.orientation.z) vehicle_transform = pylot.simulation.utils.Transform( location, rotation, orientation=orientation) forward_speed = player_measurements.forward_speed * 3.6 can_bus = pylot.simulation.utils.CanBus(vehicle_transform, forward_speed) self.get_output_stream('can_bus').send(Message(can_bus, timestamp)) self.get_output_stream('can_bus').send(watermark) def __get_ground_agents(self, measurements): vehicles = [] pedestrians = [] traffic_lights = [] speed_limit_signs = [] for agent in measurements.non_player_agents: if agent.HasField('vehicle'): transform = pylot.simulation.utils.to_pylot_transform( agent.vehicle.transform) bb = pylot.simulation.utils.BoundingBox( agent.vehicle.bounding_box) forward_speed = agent.vehicle.forward_speed vehicle = pylot.simulation.utils.Vehicle( transform, bb, forward_speed) vehicles.append(vehicle) elif agent.HasField('pedestrian'): if not self.agent_id_map.get(agent.id): self.pedestrian_count += 1 self.agent_id_map[agent.id] = self.pedestrian_count pedestrian_index = self.agent_id_map[agent.id] transform = pylot.simulation.utils.to_pylot_transform( agent.pedestrian.transform) bb = pylot.simulation.utils.BoundingBox( agent.pedestrian.bounding_box) forward_speed = agent.pedestrian.forward_speed pedestrian = pylot.simulation.utils.Pedestrian( pedestrian_index, transform, bb, forward_speed) pedestrians.append(pedestrian) elif agent.HasField('traffic_light'): transform = pylot.simulation.utils.to_pylot_transform( agent.traffic_light.transform) if agent.traffic_light.state == 2: erdos_tl_state = TrafficLightColor.RED elif agent.traffic_light.state == 1: erdos_tl_state = TrafficLightColor.YELLOW elif agent.traffic_light.state == 0: erdos_tl_state = TrafficLightColor.GREEN else: erdos_tl_state = TrafficLightColor.OFF traffic_light = pylot.simulation.utils.TrafficLight( agent.id, transform, erdos_tl_state, None) traffic_lights.append(traffic_light) elif agent.HasField('speed_limit_sign'): transform = pylot.simulation.utils.to_pylot_transform( agent.speed_limit_sign.transform) speed_sign = pylot.simulation.utils.SpeedLimitSign( transform, agent.speed_limit_sign.speed_limit) speed_limit_signs.append(speed_sign) return vehicles, pedestrians, traffic_lights, speed_limit_signs def __send_ground_agent_data(self, agents, timestamp, watermark): vehicles, pedestrians, traffic_lights, speed_limit_signs = agents vehicles_msg = pylot.simulation.messages.GroundVehiclesMessage( vehicles, timestamp) self.get_output_stream('vehicles').send(vehicles_msg) self.get_output_stream('vehicles').send(watermark) pedestrians_msg = pylot.simulation.messages.GroundPedestriansMessage( pedestrians, timestamp) self.get_output_stream('pedestrians').send(pedestrians_msg) self.get_output_stream('pedestrians').send(watermark) traffic_lights_msg = pylot.simulation.messages.GroundTrafficLightsMessage( traffic_lights, timestamp) self.get_output_stream('traffic_lights').send(traffic_lights_msg) self.get_output_stream('traffic_lights').send(watermark) speed_limits_msg = pylot.simulation.messages.GroundSpeedSignsMessage( speed_limit_signs, timestamp) self.get_output_stream('speed_limit_signs').send(speed_limits_msg) self.get_output_stream('speed_limit_signs').send(watermark) # We do not have any stop signs. stop_signs_msg = pylot.simulation.messages.GroundStopSignsMessage( [], timestamp) self.get_output_stream('stop_signs').send(stop_signs_msg) self.get_output_stream('stop_signs').send(watermark) def __send_sensor_data(self, sensor_data, timestamp, watermark): for name, measurement in sensor_data.items(): data_stream = self.get_output_stream(name) if data_stream.get_label('camera_type') == 'sensor.camera.rgb': # Transform the Carla RGB images to BGR. data_stream.send( pylot.simulation.messages.FrameMessage( pylot.utils.bgra_to_bgr(to_bgra_array(measurement)), timestamp)) elif data_stream.get_label( 'camera_type') == 'sensor.camera.semantic_segmentation': frame = labels_to_array(measurement) data_stream.send(SegmentedFrameMessage(frame, 0, timestamp)) elif data_stream.get_label('camera_type') == 'sensor.camera.depth': # NOTE: depth_to_array flips the image. data_stream.send( pylot.simulation.messages.DepthFrameMessage( depth_to_array(measurement), self._transforms[name], measurement.fov, timestamp)) elif data_stream.get_label( 'sensor_type') == 'sensor.lidar.ray_cast': pc_msg = pylot.simulation.messages.PointCloudMessage( measurement.data, self._transforms[name], timestamp) data_stream.send(pc_msg) else: data_stream.send(Message(measurement, timestamp)) data_stream.send(watermark) def _tick_simulator(self): if (not self._flags.carla_synchronous_mode or self._flags.carla_step_frequency == -1): # Run as fast as possible. self.publish_world_data() return time_until_tick = self._tick_at - time.time() if time_until_tick > 0: time.sleep(time_until_tick) else: self._logger.error('Cannot tick Carla at frequency {}'.format( self._flags.carla_step_frequency)) self._tick_at += 1.0 / self._flags.carla_step_frequency self.publish_world_data() def on_control_msg(self, msg): """ Invoked when a ControlMessage is received. Args: msg: A control.messages.ControlMessage message. """ if not self._auto_pilot: # Update the control dict state. self.control['steer'] = msg.steer self.control['throttle'] = msg.throttle self.control['brake'] = msg.brake self.control['hand_brake'] = msg.hand_brake self.control['reverse'] = msg.reverse self.client.send_control(**self.control) else: self.client.send_control(self.control) self._tick_simulator() def execute(self): # Connect to the simulator. self.client = CarlaClient(self._flags.carla_host, self._flags.carla_port, timeout=self._flags.carla_timeout) self.client.connect() scene = self.client.load_settings(self._settings) # Choose one player start at random. number_of_player_starts = len(scene.player_start_spots) player_start = self._flags.carla_start_player_num if self._flags.carla_random_player_start: player_start = np.random.randint( 0, max(0, number_of_player_starts - 1)) self.client.start_episode(player_start) self._tick_at = time.time() self._tick_simulator() self.spin()
class CarlaEnv(object): ''' An OpenAI Gym Environment for CARLA. ''' def __init__(self, obs_converter, action_converter, env_id, random_seed=0, exp_suite_name='TrainingSuite', reward_class_name='RewardCarla', host='127.0.0.1', port=2000, city_name='Town01', subset=None, video_every=100, video_dir='./video/', distance_for_success=2.0, benchmark=False): self.logger = get_carla_logger() self.logger.info('Environment {} running in port {}'.format( env_id, port)) self.host, self.port = host, port self.id = env_id self._obs_converter = obs_converter self.observation_space = obs_converter.get_observation_space() self._action_converter = action_converter self.action_space = self._action_converter.get_action_space() if benchmark: self._experiment_suite = getattr(experiment_suites_benchmark, exp_suite_name)(city_name) else: self._experiment_suite = getattr(experiment_suites, exp_suite_name)(city_name, subset) self._reward = getattr(rewards, reward_class_name)() self._experiments = self._experiment_suite.get_experiments() self.subset = subset self._make_carla_client(host, port) self._distance_for_success = distance_for_success self._planner = Planner(city_name) self.done = False self.last_obs = None self.last_distance_to_goal = None self.last_direction = None self.last_measurements = None np.random.seed(random_seed) self.video_every = video_every self.video_dir = video_dir self.video_writer = None self._success = False self._failure_timeout = False self._failure_collision = False self.benchmark = benchmark self.benchmark_index = [0, 0, 0] try: if not os.path.isdir(self.video_dir): os.makedirs(self.video_dir) except OSError: pass self.steps = 0 self.num_episodes = 0 def step(self, action): if self.done: raise ValueError( 'self.done should always be False when calling step') while True: try: # Send control control = self._action_converter.action_to_control( action, self.last_measurements) self._client.send_control(control) # Gather the observations (including measurements, sensor and directions) measurements, sensor_data = self._client.read_data() self.last_measurements = measurements current_timestamp = measurements.game_timestamp distance_to_goal = self._get_distance_to_goal( measurements, self._target) self.last_distance_to_goal = distance_to_goal directions = self._get_directions( measurements.player_measurements.transform, self._target) self.last_direction = directions obs = self._obs_converter.convert(measurements, sensor_data, directions, self._target, self.id) if self.video_writer is not None and self.steps % 2 == 0: self._raster_frame(sensor_data, measurements, directions, obs) self.last_obs = obs except CameraException: self.logger.debug('Camera Exception in step()') obs = self.last_obs distance_to_goal = self.last_distance_to_goal current_timestamp = self.last_measurements.game_timestamp except TCPConnectionError as e: self.logger.debug( 'TCPConnectionError inside step(): {}'.format(e)) self.done = True return self.last_obs, 0.0, True, {'carla-reward': 0.0} break # Check if terminal state timeout = (current_timestamp - self._initial_timestamp) > (self._time_out * 1000) collision, _ = self._is_collision(measurements) success = distance_to_goal < self._distance_for_success if timeout: self.logger.debug('Timeout') self._failure_timeout = True if collision: self.logger.debug('Collision') self._failure_collision = True if success: self.logger.debug('Success') self.done = timeout or collision or success # Get the reward env_state = { 'timeout': timeout, 'collision': collision, 'success': success } reward = self._reward.get_reward(measurements, self._target, self.last_direction, control, env_state) # Additional information info = {'carla-reward': reward} self.steps += 1 return obs, reward, self.done, info def reset(self): # Loop forever due to TCPConnectionErrors while True: try: self._reward.reset_reward() self.done = False if self.video_writer is not None: try: self.video_writer.close() except Exception as e: self.logger.debug( 'Error when closing video writer in reset') self.logger.error(e) self.video_writer = None if self.benchmark: end_indicator = self._new_episode_benchmark() if end_indicator is False: return False else: self._new_episode() # Hack: Try sleeping so that the server is ready. Reduces the number of TCPErrors time.sleep(4) # measurements, sensor_data = self._client.read_data() self._client.send_control(VehicleControl()) measurements, sensor_data = self._client.read_data() self._initial_timestamp = measurements.game_timestamp self.last_measurements = measurements self.last_distance_to_goal = self._get_distance_to_goal( measurements, self._target) directions = self._get_directions( measurements.player_measurements.transform, self._target) self.last_direction = directions obs = self._obs_converter.convert(measurements, sensor_data, directions, self._target, self.id) self.last_obs = obs self.done = False self._success = False self._failure_timeout = False self._failure_collision = False return obs except CameraException: self.logger.debug('Camera Exception in reset()') continue except TCPConnectionError as e: self.logger.debug('TCPConnectionError in reset()') self.logger.error(e) # Disconnect and reconnect self.disconnect() time.sleep(5) self._make_carla_client(self.host, self.port) def disconnect(self): if self.video_writer is not None: try: self.video_writer.close() except Exception as e: self.logger.debug( 'Error when closing video writer in disconnect') self.logger.error(e) self.video_writer = None self._client.disconnect() def _raster_frame(self, sensor_data, measurements, directions, obs): frame = sensor_data['CameraRGB'].data.copy() cv2.putText(frame, text='Episode number: {:,}'.format(self.num_episodes - 1), org=(50, 50), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1.0, color=[0, 0, 0], thickness=2) cv2.putText(frame, text='Environment steps: {:,}'.format(self.steps), org=(50, 80), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1.0, color=[0, 0, 0], thickness=2) REACH_GOAL = 0.0 GO_STRAIGHT = 5.0 TURN_RIGHT = 4.0 TURN_LEFT = 3.0 LANE_FOLLOW = 2.0 if np.isclose(directions, REACH_GOAL): dir_str = 'REACH GOAL' elif np.isclose(directions, GO_STRAIGHT): dir_str = 'GO STRAIGHT' elif np.isclose(directions, TURN_RIGHT): dir_str = 'TURN RIGHT' elif np.isclose(directions, TURN_LEFT): dir_str = 'TURN LEFT' elif np.isclose(directions, LANE_FOLLOW): dir_str = 'LANE FOLLOW' else: raise ValueError(directions) cv2.putText(frame, text='Direction: {}'.format(dir_str), org=(50, 110), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1.0, color=[0, 0, 0], thickness=2) cv2.putText(frame, text='Speed: {:.02f}'.format( measurements.player_measurements.forward_speed * 3.6), org=(50, 140), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1.0, color=[0, 0, 0], thickness=2) cv2.putText(frame, text='rel_x: {:.02f}, rel_y: {:.02f}'.format( obs['v'][-2].item(), obs['v'][-1].item()), org=(50, 170), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1.0, color=[0, 0, 0], thickness=2) self.video_writer.writeFrame(frame) def _get_distance_to_goal(self, measurements, target): current_x = measurements.player_measurements.transform.location.x current_y = measurements.player_measurements.transform.location.y distance_to_goal = np.linalg.norm(np.array([current_x, current_y]) - \ np.array([target.location.x, target.location.y])) return distance_to_goal def _new_episode(self): experiment_idx = np.random.randint(0, len(self._experiments)) experiment = self._experiments[experiment_idx] exp_settings = experiment.conditions exp_settings.set(QualityLevel='Low') positions = self._client.load_settings(exp_settings).player_start_spots idx_pose = np.random.randint(0, len(experiment.poses)) pose = experiment.poses[idx_pose] self.logger.info('Env {} gets experiment {} with pose {}'.format( self.id, experiment_idx, idx_pose)) start_index = pose[0] end_index = pose[1] self._client.start_episode(start_index) self._time_out = self._experiment_suite.calculate_time_out( self._get_shortest_path(positions[start_index], positions[end_index])) self._target = positions[end_index] self._episode_name = str(experiment.Conditions.WeatherId) + '_' \ + str(experiment.task) + '_' + str(start_index) \ + '_' + str(end_index) if ((self.num_episodes % self.video_every) == 0) and (self.id == 0): video_path = os.path.join( self.video_dir, '{:08d}_'.format(self.num_episodes) + self._episode_name + '.mp4') self.logger.info('Writing video at {}'.format(video_path)) self.video_writer = skvideo.io.FFmpegWriter( video_path, inputdict={'-r': '30'}, outputdict={'-r': '30'}) else: self.video_writer = None self.num_episodes += 1 def _new_episode_benchmark(self): experiment_idx_past = self.benchmark_index[0] pose_idx_past = self.benchmark_index[1] repetition_idx_past = self.benchmark_index[2] experiment_past = self._experiments[experiment_idx_past] poses_past = experiment_past.poses[0:] repetition_past = experiment_past.repetitions if repetition_idx_past == repetition_past: if pose_idx_past == len(poses_past) - 1: if experiment_idx_past == len(self._experiments) - 1: return False else: experiment = self._experiments[experiment_idx_past + 1] pose = experiment.poses[0:][0] self.benchmark_index = [experiment_idx_past + 1, 0, 1] else: experiment = experiment_past pose = poses_past[pose_idx_past + 1] self.benchmark_index = [ experiment_idx_past, pose_idx_past + 1, 1 ] else: experiment = experiment_past pose = poses_past[pose_idx_past] self.benchmark_index = [ experiment_idx_past, pose_idx_past, repetition_idx_past + 1 ] exp_settings = experiment.Conditions exp_settings.set(QualityLevel='Low') positions = self._client.load_settings(exp_settings).player_start_spots start_index = pose[0] end_index = pose[1] self._client.start_episode(start_index) self._time_out = self._experiment_suite.calculate_time_out( self._get_shortest_path(positions[start_index], positions[end_index])) self._target = positions[end_index] self._episode_name = str(experiment.Conditions.WeatherId) + '_' \ + str(experiment.task) + '_' + str(start_index) \ + '_' + str(end_index) if ((self.num_episodes % self.video_every) == 0) and (self.id == 0): video_path = os.path.join( self.video_dir, '{:08d}_'.format(self.num_episodes) + self._episode_name + '.mp4') self.logger.info('Writing video at {}'.format(video_path)) self.video_writer = skvideo.io.FFmpegWriter( video_path, inputdict={'-r': '30'}, outputdict={'-r': '30'}) else: self.video_writer = None self.num_episodes += 1 def _get_directions(self, current_point, end_point): 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): 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 ]) @staticmethod def _is_collision(measurements): c = 0 c += measurements.player_measurements.collision_vehicles c += measurements.player_measurements.collision_pedestrians c += measurements.player_measurements.collision_other sidewalk_intersection = measurements.player_measurements.intersection_offroad otherlane_intersection = measurements.player_measurements.intersection_otherlane return (c > 1e-9) or (sidewalk_intersection > 0.01) or (otherlane_intersection > 0.9), c def _make_carla_client(self, host, port): while True: try: self.logger.info( "Trying to make client on port {}".format(port)) self._client = CarlaClient(host, port, timeout=100) self._client.connect() self._client.load_settings(CarlaSettings(QualityLevel='Low')) self._client.start_episode(0) self.logger.info( "Successfully made client on port {}".format(port)) break except TCPConnectionError as error: self.logger.debug('Got TCPConnectionError..sleeping for 1') self.logger.error(error) time.sleep(1)
class CarlaEnv(object): def __init__( self, host='eceftl1.ces.clemson.edu', port=2000, image_filename_format='_images/episode_{:0>3d}/{:s}/image_{:0>5d}.png', save_images_to_disk=False): self.image_filename_format = image_filename_format self.save_images_to_disk = save_images_to_disk self.host = host self.port = port self.timeout = 60 self._Observation = collections.namedtuple('Observation', ['image', 'label']) self.frame = 0 self.episode_index = -1 self.client = CarlaClient(self.host, self.port, self.timeout) def connect(self): self.client.connect() def __exit__(self, exc_type, exc_val, exc_tb): self.client.disconnect() def start_new_episode(self, player_index): settings = CarlaSettings() settings.set(SynchronousMode=True, SendNonPlayerAgentsInfo=True, NumberOfVehicles=20, NumberOfPedestrians=40, WeatherId=random.choice([1, 3, 7, 8, 14])) settings.randomize_seeds() camera0 = Camera('CameraRGB') camera0.set(CameraFOV=100) camera0.set_image_size(800, 400) camera0.set_position(120, 0, 130) settings.add_sensor(camera0) camera2 = Camera('CameraSeg', PostProcessing='SemanticSegmentation') camera2.set(CameraFOV=100) camera2.set_image_size(800, 400) camera2.set_position(120, 0, 130) settings.add_sensor(camera2) self.client.load_settings(settings) self.episode_index += 1 self.frame = 0 print('\nStarting episode {0}.'.format(self.episode_index)) self.client.start_episode(player_index) def _get_observation(self, sensor_data): camera_rgb_data = sensor_data['CameraRGB'] camera_rgb_img = PImage.frombytes(mode='RGBA', size=(camera_rgb_data.width, camera_rgb_data.height), data=camera_rgb_data.raw_data, decoder_name='raw') b, g, r, a = camera_rgb_img.split() camera_rgb_img = PImage.merge("RGB", (r, g, b)) camera_seg_data = sensor_data['CameraSeg'] camera_seg_gray = image_converter.labels_to_grayscale2( camera_seg_data).astype('uint8') camera_seg_img = PImage.fromarray(camera_seg_gray, 'L') return self._Observation(camera_rgb_img, camera_seg_img) def _gen_images(self, observation): camera_rgb_img_filename = self.image_filename_format.format( self.episode_index, 'CameraRGB', self.frame) camera_seg_img_filename = self.image_filename_format.format( self.episode_index, 'CameraSeg', self.frame) rgb_folder = os.path.dirname(camera_rgb_img_filename) if not os.path.isdir(rgb_folder): os.makedirs(rgb_folder) seg_folder = os.path.dirname(camera_seg_img_filename) if not os.path.isdir(seg_folder): os.makedirs(seg_folder) observation.image.save(camera_rgb_img_filename) observation.label.save(camera_seg_img_filename) def auto_drive(self): measurements, sensor_data = self.client.read_data() print_measurements(measurements) observation = self._get_observation(sensor_data) if self.save_images_to_disk: self._gen_images(self._get_observation(sensor_data)) control = measurements.player_measurements.autopilot_control control.steer += random.uniform(-0.1, 0.1) self.client.send_control(control) self.frame += 1 def step(self, *args, **kwargs): measurements, sensor_data = self.client.read_data() print_measurements(measurements) observation = self._get_observation(sensor_data) if self.save_images_to_disk: self._gen_images(observation) pm = measurements.player_measurements rewards = pm.collision_vehicles + pm.collision_pedestrians + pm.collision_other self.client.send_control(*args, **kwargs) self.frame += 1 return observation, rewards
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=800", "-ResY=600", "-carla-server", "-benchmark -fps=10", "-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(0.1, 0, 1.7) settings.add_sensor(camera1) camera2 = Camera("CameraRGB") camera2.set_image_size(self.config["render_x_res"], self.config["render_y_res"]) camera2.set_position(0.1, 0, 1.7) 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, # TODO can we figure some way using this 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
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 = 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 -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 ] 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: # screen.success('EPISODE IS DONE. GameTime: {}, Collision: {}'.format(str(measurements.game_timestamp), # str(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 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 = 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 CarlaEnv(gym.Env): def __init__(self, config=ENV_CONFIG, enable_autopilot=False): self.enable_autopilot = enable_autopilot self.config = config self.config["x_res"], self.config["y_res"] = IMG_SIZE self.city = self.config["server_map"].split("/")[-1] if self.config["enable_planner"]: self.planner = Planner(self.city) self.intersections_node = self.planner._city_track._map.get_intersection_nodes( ) self.intersections_pos = np.array([ self.planner._city_track._map.convert_to_world( intersection_node) for intersection_node in self.intersections_node ]) self.pre_intersection = np.array([0.0, 0.0]) # # Cartesian coordinates self.headings = np.array([[1, 0], [-1, 0], [0, 1], [0, -1]]) # self.lrs_matrix = {"GO_STRAIGHT": np.array([[1,0],[0,1]]),\ # "TURN_RIGHT": np.array([[0,-1],[1,0]]),\ # "TURN_LEFT": np.array([[0,1],[-1,0]])} # self.goal_heading = np.array([0.0,0.0]) # self.current_heading = None # self.pre_heading = None # self.angular_speed = None # Angular degree self.headings_degree = np.array( [0.0, 180.0, 90.0, -90.0]) # one-one mapping to self.headings self.lrs_degree = { "GO_STRAIGHT": 0.0, "TURN_LEFT": -90.0, "TURN_RIGHT": 90.0 } self.goal_heading_degree = 0.0 self.current_heading_degree = None self.pre_heading_degree = None self.angular_speed_degree = np.array(0.0) # 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_sensor"] == 'use_semantic': image_space = Box(0.0, 255.0, shape=(config["y_res"], config["x_res"], 1 * config["framestack"]), dtype=np.float32) elif config["use_sensor"] in ['use_depth', 'use_logdepth']: image_space = Box(0.0, 255.0, shape=(config["y_res"], config["x_res"], 1 * config["framestack"]), dtype=np.float32) elif config["use_sensor"] == 'use_rgb': image_space = Box(0, 255, shape=(config["y_res"], config["x_res"], 3 * config["framestack"]), dtype=np.uint8) elif config["use_sensor"] == 'use_2rgb': image_space = Box(0, 255, shape=(config["y_res"], config["x_res"], 2 * 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 self.cnt1 = None self.displacement = None self.next_command = "LANE_FOLLOW" 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=480", "-ResY=360", "-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") ) # ResourceWarning: unclosed file <_io.TextIOWrapper name='/dev/null' mode='w' encoding='UTF-8'> 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 # @set_timeout(15) 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 self.pre_intersection = np.array([0.0, 0.0]) # 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_sensor"] == 'use_semantic': camera0 = Camera("CameraSemantic", PostProcessing="SemanticSegmentation") camera0.set_image_size(self.config["render_x_res"], self.config["render_y_res"]) # camera0.set_position(30, 0, 130) camera0.set(FOV=120) camera0.set_position(2.0, 0.0, 1.4) camera0.set_rotation(0.0, 0.0, 0.0) settings.add_sensor(camera0) if self.config["use_sensor"] in ['use_depth', 'use_logdepth']: 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(FOV=120) camera1.set_position(2.0, 0.0, 1.4) camera1.set_rotation(0.0, 0.0, 0.0) settings.add_sensor(camera1) if self.config["use_sensor"] == 'use_rgb': 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(FOV=120) camera2.set_position(2.0, 0.0, 1.4) camera2.set_rotation(0.0, 0.0, 0.0) settings.add_sensor(camera2) if self.config["use_sensor"] == 'use_2rgb': camera_l = Camera("CameraRGB_L") camera_l.set_image_size(self.config["render_x_res"], self.config["render_y_res"]) camera_l.set(FOV=120) camera_l.set_position(2.0, -0.1, 1.4) camera_l.set_rotation(0.0, 0.0, 0.0) settings.add_sensor(camera_l) camera_r = Camera("CameraRGB_R") camera_r.set_image_size(self.config["render_x_res"], self.config["render_y_res"]) camera_r.set(FOV=120) camera_r.set_position(2.0, 0.1, 1.4) camera_r.set_rotation(0.0, 0.0, 0.0) settings.add_sensor(camera_r) # Setup start and end positions scene = self.client.load_settings(settings) self.positions = positions = scene.player_start_spots self.start_pos = positions[self.scenario["start_pos_id"]] self.pre_pos = self.start_pos.location self.cnt1 = 0 self.displacement = 1000.0 self.end_pos = positions[self.scenario["end_pos_id"]] self.start_coord = [ self.start_pos.location.x // 1, self.start_pos.location.y // 1 ] self.end_coord = [ self.end_pos.location.x // 1, self.end_pos.location.y // 1 ] 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"]) # remove the vehicle dropping when starting a new episode. cnt = 1 z1 = 0 zero_control = VehicleControl() while (cnt < 3): self.client.send_control( zero_control ) # VehicleControl().steer = 0, VehicleControl().throttle = 0, VehicleControl().reverse = False z0 = z1 z1 = self.client.read_data( )[0].player_measurements.transform.location.z print(z1) if z0 - z1 == 0: cnt += 1 print('Starting new episode done.\n') # Process observations: self._read_observation() returns image and py_measurements. image, py_measurements = self._read_observation() self.prev_measurement = py_measurements # self.current_heading = self.pre_heading = np.array([py_measurements["x_orient"], py_measurements["y_orient"]]) # self.angular_speed = 0.0 self.pre_heading_degree = self.current_heading_degree = py_measurements[ "current_heading_degree"] self.angular_speed_degree = np.array(0.0) return self.encode_obs(self.preprocess_image(image), py_measurements), 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"] # ]) obs = image 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, self.prev_measurement) def delta_degree(self, deltaxy): return deltaxy if np.abs( deltaxy) < 180 else deltaxy - np.sign(deltaxy) * 360 # image, py_measurements = self._read_observation() ---> self.preprocess_image(image) ---> step observation output # @set_timeout(10) 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.enable_autopilot: action[ 0] = self.autopilot.brake if self.autopilot.brake < 0 else self.autopilot.throttle action[1] = self.autopilot.steer 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) # print(self.client) 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"]) print("Next command", py_measurements["next_command"]) print("dist_to_intersection:", py_measurements["dist_to_intersection"]) 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 angular_speed self.current_heading_degree = py_measurements["current_heading_degree"] self.angular_speed_degree = np.array( self.delta_degree(self.current_heading_degree - self.pre_heading_degree)) self.pre_heading_degree = py_measurements["current_heading_degree"] # 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 = False # done = self.cnt1 > 50 and (py_measurements["collision_vehicles"] or py_measurements["collision_pedestrians"] or py_measurements["collision_other"] or self.displacement < 0.5) done = self.cnt1 > 50 and self.displacement < 0.2 # done = (self.num_steps > self.scenario["max_steps"] # or py_measurements["next_command"] == "REACH_GOAL" or py_measurements["intersection_offroad"] or py_measurements["intersection_otherlane"] # 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_sensor"] == 'use_semantic': # image.data: uint8(0 ~ 12) data = image.data * 21 # data: uint8(0 ~ 255) 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) # data: uint8(0 ~ 255), shape(y_res, x_res, 1) elif self.config["use_sensor"] == 'use_depth': # depth: float64(0 ~ 1) # data = (image.data - 0.5) * 2 data = image.data * 255 # data: float64(0 ~ 255) data = data.reshape(self.config["render_y_res"], self.config["render_x_res"], 1) # shape(render_y_res,render_x_res,1) data = cv2.resize( data, (self.config["x_res"], self.config["y_res"]), interpolation=cv2.INTER_AREA) # shape(y_res, x_res) data = np.expand_dims( data, 2) # data: float64(0 ~ 255), shape(y_res, x_res, 1) elif self.config[ "use_sensor"] == 'use_logdepth': # depth: float64(0 ~ 1) data = (np.log(image.data) + 7.0) * 255.0 / 7.0 # data: float64(0 ~ 255) data = data.reshape(self.config["render_y_res"], self.config["render_x_res"], 1) # shape(render_y_res,render_x_res,1) data = cv2.resize( data, (self.config["x_res"], self.config["y_res"]), interpolation=cv2.INTER_AREA) # shape(y_res, x_res) data = np.expand_dims( data, 2) # data: float64(0 ~ 255), shape(y_res, x_res, 1) elif self.config["use_sensor"] == 'use_rgb': 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"] ), # data: uint8(0 ~ 255), shape(y_res, x_res, 3) interpolation=cv2.INTER_AREA) # data = (data.astype(np.float32) - 128) / 128 elif self.config["use_sensor"] == 'use_2rgb': data_l, data_r = image[0].data, image[0].data data_l = data_l.reshape(self.config["render_y_res"], self.config["render_x_res"], 3) data_r = data_r.reshape(self.config["render_y_res"], self.config["render_x_res"], 3) data_l = cv2.resize(data_l, (self.config["x_res"], self.config["y_res"]), interpolation=cv2.INTER_AREA) data_r = cv2.resize(data_r, (self.config["x_res"], self.config["y_res"]), interpolation=cv2.INTER_AREA) data = np.concatenate( (data_l, data_r), axis=2) # data: uint8(0 ~ 255), shape(y_res, x_res, 6) return data def _read_observation(self): # Read the data produced by the server this frame. measurements, sensor_data = self.client.read_data() if self.enable_autopilot: self.autopilot = measurements.player_measurements.autopilot_control # Print some of the measurements. if self.config["verbose"]: print_measurements(measurements) observation = None if self.config["use_sensor"] == 'use_semantic': camera_name = "CameraSemantic" elif self.config["use_sensor"] in ['use_depth', 'use_logdepth']: camera_name = "CameraDepth" elif self.config["use_sensor"] == 'use_rgb': camera_name = "CameraRGB" elif self.config["use_sensor"] == 'use_2rgb': camera_name = ["CameraRGB_L", "CameraRGB_R"] # for name, image in sensor_data.items(): # if name == camera_name: # observation = image if camera_name == ["CameraRGB_L", "CameraRGB_R"]: observation = [ sensor_data["CameraRGB_L"], sensor_data["CameraRGB_R"] ] else: observation = sensor_data[camera_name] cur = measurements.player_measurements if self.config["enable_planner"]: # modify next_command current_pos = np.array([cur.transform.location.x, cur.transform.location.y])\ + np.array([cur.transform.orientation.x, cur.transform.orientation.y]) * 5.0 dist_intersection_current_pos = np.linalg.norm( self.intersections_pos[:, :2] - current_pos, axis=1) is_near_intersection = np.min(dist_intersection_current_pos) < 18.0 if not is_near_intersection: self.next_command = "LANE_FOLLOW" # goal_heading if is_near_intersection: cur_intersection = self.intersections_pos[ dist_intersection_current_pos.argmin(), :2] self.dist_to_intersection = np.linalg.norm(cur_intersection - current_pos) else: self.goal_heading_degree = 0.0 self.dist_to_intersection = 1000.0 if is_near_intersection and np.linalg.norm(self.pre_intersection - cur_intersection) > 0.1: self.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] \ )] if self.next_command == "LANE_FOLLOW": self.next_command = random.choice( ["GO_STRAIGHT", "TURN_LEFT", "TURN_RIGHT"]) cur_heading0 = cur_intersection - current_pos cur_heading_1 = cur_heading0 / np.linalg.norm(cur_heading0) cur_heading_degree = self.headings_degree[np.linalg.norm( cur_heading_1 - self.headings, axis=1).argmin()] self.goal_heading_degree = self.delta_degree( cur_heading_degree + self.lrs_degree[self.next_command]) self.pre_intersection = cur_intersection else: self.next_command = "LANE_FOLLOW" if self.next_command == "REACH_GOAL": distance_to_goal = 0.0 # avoids crash in planner self.end_pos = self.positions[random.choice( self.config["scenarios"])['end_pos_id']] # 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 # ]) else: distance_to_goal = -1 distance_to_goal_euclidean = float( np.linalg.norm([ # default norm: L2 norm cur.transform.location.x - self.end_pos.location.x, cur.transform.location.y - self.end_pos.location.y ])) # displacement if self.cnt1 > 70 and self.cnt1 % 30 == 0: self.displacement = float( np.linalg.norm([ cur.transform.location.x - self.pre_pos.x, cur.transform.location.y - self.pre_pos.y ])) self.pre_pos = cur.transform.location self.cnt1 += 1 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": np.array(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, "collided": collision_(cur), ### "intersection_offroad": np.array(cur.intersection_offroad), ### "intersection_otherlane": np.array(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": self.next_command, "next_command_id": COMMAND_ORDINAL[self.next_command], "displacement": self.displacement, "is_near_intersection": is_near_intersection, "goal_heading_degree": self.goal_heading_degree, "angular_speed_degree": self.angular_speed_degree, ### "current_heading_degree": cur.transform.rotation.yaw, # left-, right+, (-180 ~ 180) degrees "dist_to_intersection": self.dist_to_intersection } 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) # image.data without preprocess. assert observation is not None, sensor_data return observation, py_measurements
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 self.viewer = None def render(self, mode=None): filename = f'images/id_{self._spec.id}_ep_{self.episode_id}_step_{self.num_steps}' self.frame_image.save_to_disk(filename) # from gym.envs.classic_control import rendering # if self.viewer is None: # self.viewer = rq # endering.SimpleImageViewer() # self.viewer.imshow(self.frame_image) # return self.viewer.isopen 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=400", "-ResY=300", "-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=400 -ResY=300" " -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(0.30, 0, 1.30) settings.add_sensor(camera1) camera2 = Camera("CameraRGB") camera2.set_image_size( self.config["render_x_res"], self.config["render_y_res"]) camera2.set_position(0.30, 0, 1.30) 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 self.frame_image = observation 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 CarlaClientConsole(cmd.Cmd): def __init__(self, args): cmd.Cmd.__init__(self) self.args = args self.prompt = '\x1b[1;34m%s\x1b[0m ' % '(carla)' self.client = CarlaClient(args.host, args.port) self.settings = get_default_carla_settings(args) self.print_measurements = False self.control = _Control() self.thread = threading.Thread(target=self._agent_thread_worker) self.done = False self.thread.start() def cleanup(self): self.do_disconnect() self.done = True if self.thread.is_alive(): self.thread.join() def default(self, line): logging.error('unknown command \'%s\'!', line) def emptyline(self): pass def precmd(self, line): return line.strip().lower() def can_exit(self): return True def do_exit(self, line=None): """Exit the console.""" return True def do_eof(self, line=None): """Exit the console.""" print('exit') return self.do_exit(line) def help_help(self): print('usage: help [topic]') def do_disconnect(self, line=None): """Disconnect from the server.""" self.client.disconnect() def do_new_episode(self, line=None): """Request a new episode. Connect to the server if not connected.""" try: self.control = _Control() if not self.client.connected(): self.client.connect() self.client.load_settings(self.settings) self.client.start_episode(0) logging.info('new episode started') except Exception as exception: logging.error(exception) def do_control(self, line=None): """Set control message:\nusage: control [reset|<param> <value>]\n(e.g., control throttle 0.5)""" try: if line == 'reset': self.control = _Control() else: self.control.set(line) logging.debug('control: %s', self.control.kwargs()) except Exception as exception: logging.error(exception) def complete_control(self, text, *args, **kwargs): options = self.control.action_list() options.append('reset') return [x + ' ' for x in options if x.startswith(text)] def do_settings(self, line=None): """Open a text editor to edit CARLA settings.""" result = edit_text(self.settings) if result is not None: self.settings = result def do_print_measurements(self, line): """Print received measurements to console.\nusage: print_measurements [t/f]""" self.print_measurements = True if not line else _Control._parse(bool, line) def _agent_thread_worker(self): filename = '_images/console/camera_{:0>3d}/image_{:0>8d}.png' while not self.done: try: measurements, sensor_data = self.client.read_data() if self.print_measurements: print(measurements) if self.args.images_to_disk: images = [x for x in sensor_data.values() if isinstance(x, Image)] for n, image in enumerate(images): path = filename.format(n, measurements.game_timestamp) image.save_to_disk(path) self.client.send_control(**self.control.kwargs()) except Exception as exception: # logging.exception(exception) time.sleep(1)
class CarlaEnvironmentWrapper(EnvironmentWrapper): def __init__(self, num_speedup_steps = 30, require_explicit_reset=True, is_render_enabled=False, early_termination_enabled=False, run_offscreen=False, cameras=['SceneFinal'], save_screens=False): EnvironmentWrapper.__init__(self, is_render_enabled, save_screens) self.episode_max_time = 1000000 self.allow_braking = True self.log_path = 'logs/CarlaLogs.txt' self.num_speedup_steps = num_speedup_steps self.is_game_ready_for_input = False self.run_offscreen = run_offscreen self.kill_when_connection_lost = True # server configuration self.port = get_open_port() self.host = 'localhost' self.level = 'town2' #Why town2: https://github.com/carla-simulator/carla/issues/10#issuecomment-342483829 self.map = CarlaLevel().get(self.level) # client configuration self.verbose = True self.observation = None self.camera_settings = dict( ImageSizeX=carla_config.server_width, ImageSizeY=carla_config.server_height, FOV=110.0, PositionX=2.0, # 200 for Carla 0.7 PositionY=0.0, PositionZ=1.40, # 140 for Carla 0.7 RotationPitch = 0.0, RotationRoll = 0.0, RotationYaw = 0.0, ) self.rgb_camera_name = 'CameraRGB' self.segment_camera_name = 'CameraSegment' self.depth_camera_name = 'CameraDepth' self.rgb_camera = 'SceneFinal' in cameras self.segment_camera = 'SemanticSegmentation' in cameras self.depth_camera = 'Depth' in cameras self.class_grouping = carla_config.class_grouping or [(i, ) for i in range(carla_config.no_of_classes)] self.autocolor_the_segments = False self.color_the_depth_map = False self.enable_coalesced_output = False self.max_depth_value = 1.0 #255.0 for CARLA 7.0 self.min_depth_value = 0.0 self.config = None #'Environment/CarlaSettings.ini' if self.config: # load settings from file with open(self.config, 'r') as fp: self.settings = CarlaSettings(fp.read()) else: # hard coded settings #print("CarlaSettings.ini not found; using default settings") self.settings = CarlaSettings() self.settings.set( SynchronousMode=True, SendNonPlayerAgentsInfo=False, NumberOfVehicles=15, NumberOfPedestrians=30, WeatherId=1, SeedVehicles = 123456789, SeedPedestrians = 123456789) #self.settings.randomize_seeds() # add cameras if self.rgb_camera: self.settings.add_sensor(self.create_camera(self.rgb_camera_name, 'SceneFinal')) if self.segment_camera: self.settings.add_sensor(self.create_camera(self.segment_camera_name, 'SemanticSegmentation')) if self.depth_camera: self.settings.add_sensor(self.create_camera(self.depth_camera_name, 'Depth')) self.car_speed = 0 self.is_game_setup = False # Will be true only when setup_client_and_server() is called, either explicitly, or by reset() # action space self.discrete_controls = True self.action_space_size = 2 self.action_space_high = [1, 1] self.action_space_low = [-1, -1] self.action_space_abs_range = np.maximum(np.abs(self.action_space_low), np.abs(self.action_space_high)) self.steering_strength = 0.35 self.gas_strength = 1.0 self.brake_strength = 0.6 self.actions = {0: [0., 0.], 1: [0., -self.steering_strength], 2: [0., self.steering_strength], 3: [self.gas_strength-0.15, 0.], 4: [-self.brake_strength, 0], 5: [self.gas_strength-0.3, -self.steering_strength], 6: [self.gas_strength-0.3, self.steering_strength], 7: [-self.brake_strength, -self.steering_strength], 8: [-self.brake_strength, self.steering_strength]} self.actions_description = ['NO-OP', 'TURN_LEFT', 'TURN_RIGHT', 'GAS', 'BRAKE', 'GAS_AND_TURN_LEFT', 'GAS_AND_TURN_RIGHT', 'BRAKE_AND_TURN_LEFT', 'BRAKE_AND_TURN_RIGHT'] for idx, action in enumerate(self.actions_description): for key in key_map.keys(): if action == key: self.key_to_action[key_map[key]] = idx # measurements self.measurements_size = (1,) self.autopilot = None self.kill_if_unmoved_for_n_steps = 15 self.unmoved_steps = 0.0 self.early_termination_enabled = early_termination_enabled if self.early_termination_enabled: self.max_neg_steps = 70 self.cur_neg_steps = 0 self.early_termination_punishment = 20.0 # env initialization if not require_explicit_reset: self.reset(True) # render if self.automatic_render: self.init_renderer() if self.save_screens: create_dir(self.images_path) self.rgb_img_path = self.images_path+"/rgb/" create_dir(self.rgb_img_path) self.segmented_img_path = self.images_path+"/segmented/" create_dir(self.segmented_img_path) self.depth_img_path = self.images_path+"/depth/" create_dir(self.depth_img_path) def create_camera(self, camera_name, PostProcessing): #camera = Camera('CameraRGB') #camera.set_image_size(carla_config.server_width, carla_config.server_height) #camera.set_position(200, 0, 140) #camera.set_rotation(0, 0, 0) #self.settings.add_sensor(camera) camera = Camera(camera_name, **dict(self.camera_settings, PostProcessing=PostProcessing)) return camera def setup_client_and_server(self, reconnect_client_only = False): # open the server if not reconnect_client_only: self.server = self._open_server() self.server_pid = self.server.pid # To kill incase child process gets lost # open the client self.game = CarlaClient(self.host, self.port, timeout=99999999) self.game.connect(connection_attempts=100) #It's taking a very long time for the server process to spawn, so the client needs to wait or try sufficient no. of times lol scene = self.game.load_settings(self.settings) # get available start positions positions = scene.player_start_spots self.num_pos = len(positions) self.iterator_start_positions = 0 self.is_game_setup = self.server and self.game return def close_client_and_server(self): self._close_server() print("Disconnecting the client") self.game.disconnect() self.game = None self.server = None self.is_game_setup = False return def _open_server(self): # Note: There is no way to disable rendering in CARLA as of now # https://github.com/carla-simulator/carla/issues/286 # decrease the window resolution if you want to see if performance increases # Command: $CARLA_ROOT/CarlaUE4.sh /Game/Maps/Town02 -benchmark -carla-server -fps=15 -world-port=9876 -windowed -ResX=480 -ResY=360 -carla-no-hud # To run off_screen: SDL_VIDEODRIVER=offscreen SDL_HINT_CUDA_DEVICE=0 <command> #https://github.com/carla-simulator/carla/issues/225 my_env = None if self.run_offscreen: my_env = {**os.environ, 'SDL_VIDEODRIVER': 'offscreen', 'SDL_HINT_CUDA_DEVICE':'0'} with open(self.log_path, "wb") as out: cmd = [path.join(environ.get('CARLA_ROOT'), 'CarlaUE4.sh'), self.map, "-benchmark", "-carla-server", "-fps=20", "-world-port={}".format(self.port), "-windowed -ResX={} -ResY={}".format(carla_config.server_width, carla_config.server_height), "-carla-no-hud", "-quality-level=Low"] if self.config: cmd.append("-carla-settings={}".format(self.config)) p = subprocess.Popen(cmd, stdout=out, stderr=out, env=my_env) return p def _close_server(self): if self.kill_when_connection_lost: os.killpg(os.getpgid(self.server.pid), signal.SIGKILL) return no_of_attempts = 0 while is_process_alive(self.server_pid): print("Trying to close Carla server with pid %d" % self.server_pid) if no_of_attempts<5: self.server.terminate() elif no_of_attempts<10: self.server.kill() elif no_of_attempts<15: os.kill(self.server_pid, signal.SIGTERM) else: os.kill(self.server_pid, signal.SIGKILL) time.sleep(10) no_of_attempts += 1 def check_early_stop(self, player_measurements, immediate_reward): if player_measurements.intersection_offroad>0.95 or immediate_reward < -1 or (self.control.throttle == 0.0 and player_measurements.forward_speed < 0.1 and self.control.brake != 0.0): self.cur_neg_steps += 1 early_done = (self.cur_neg_steps > self.max_neg_steps) if early_done: print("Early kill the mad car") return early_done, self.early_termination_punishment else: self.cur_neg_steps /= 2 #Exponentially decay return False, 0.0 def _update_state(self): # get measurements and observations measurements = [] while type(measurements) == list: try: measurements, sensor_data = self.game.read_data() except: # Connection between cli and server lost; reconnect if self.kill_when_connection_lost: raise print("Connection to server lost while reading state. Reconnecting...........") self.close_client_and_server() self.setup_client_and_server(reconnect_client_only=False) self.done = True self.location = (measurements.player_measurements.transform.location.x, measurements.player_measurements.transform.location.y, measurements.player_measurements.transform.location.z) is_collision = measurements.player_measurements.collision_vehicles != 0 \ or measurements.player_measurements.collision_pedestrians != 0 \ or measurements.player_measurements.collision_other != 0 # CARLA doesn't recognize if collision occured and colliding speed is less than 5 km/h (Around 0.7 m/s) # Ref: https://github.com/carla-simulator/carla/issues/13 # Recognize that as a collision self.car_speed = measurements.player_measurements.forward_speed if self.control.throttle > 0 and self.car_speed < 0.75 and self.control.brake==0.0 and self.is_game_ready_for_input: self.unmoved_steps += 1.0 if self.unmoved_steps > self.kill_if_unmoved_for_n_steps: is_collision = True print("Car stuck somewhere lol") elif self.unmoved_steps>0: self.unmoved_steps -= 0.50 #decay slowly, since it may be stuck and not accelerate few times if is_collision: print("Collision occured") speed_reward = self.car_speed - 1 if speed_reward > 30.: speed_reward = 30. self.reward = speed_reward*1.2 \ - (measurements.player_measurements.intersection_otherlane * (self.car_speed+1.5)*1.2) \ - (measurements.player_measurements.intersection_offroad * (self.car_speed+2.5)*1.5) \ - is_collision * 250 \ - np.abs(self.control.steer) * 2 # Scale down the reward by a factor self.reward /= 10 if self.early_termination_enabled: early_done, punishment = self.check_early_stop(measurements.player_measurements, self.reward) if early_done: self.done = True self.reward -= punishment # update measurements self.observation = { #'observation': sensor_data['CameraRGB'].data, 'acceleration': measurements.player_measurements.acceleration, 'forward_speed': measurements.player_measurements.forward_speed, 'intersection_otherlane': measurements.player_measurements.intersection_otherlane, 'intersection_offroad': measurements.player_measurements.intersection_offroad } if self.rgb_camera: self.observation['rgb_image'] = sensor_data[self.rgb_camera_name].data if self.segment_camera: self.observation['segmented_image'] = sensor_data[self.segment_camera_name].data if self.depth_camera: self.observation['depth_map'] = sensor_data[self.depth_camera_name].data if self.segment_camera and self.depth_camera and self.enable_coalesced_output: self.observation['coalesced_data'] = coalesce_depth_and_segmentation( self.observation['segmented_image'], self.class_grouping, self.observation['depth_map'], self.max_depth_value) if self.segment_camera and (self.autocolor_the_segments or self.is_render_enabled): self.observation['colored_segmented_image'] = convert_segmented_to_rgb(carla_config.colors_segment, self.observation['segmented_image']) self.autopilot = measurements.player_measurements.autopilot_control # action_p = ['%.2f' % member for member in [self.control.throttle, self.control.steer]] # screen.success('REWARD: %.2f, ACTIONS: %s' % (self.reward, action_p)) if (measurements.game_timestamp >= self.episode_max_time) or is_collision: # screen.success('EPISODE IS DONE. GameTime: {}, Collision: {}'.format(str(measurements.game_timestamp), # str(is_collision))) self.done = True def _take_action(self, action_idx): if not self.is_game_setup: print("Reset the environment before calling step") sys.exit(1) if type(action_idx) == int: action = self.actions[action_idx] else: action = action_idx self.control = VehicleControl() if self.car_speed>35.0 and action[0]>0: action[0] -= 0.20*(self.car_speed/35.0) self.control.throttle = np.clip(action[0], 0, 1) self.control.steer = np.clip(action[1], -1, 1) self.control.brake = np.abs(np.clip(action[0], -1, 0)) if not self.allow_braking: self.control.brake = 0 self.control.hand_brake = False self.control.reverse = False controls_sent = False while not controls_sent: try: self.game.send_control(self.control) controls_sent = True except: if self.kill_when_connection_lost: raise print("Connection to server lost while sending controls. Reconnecting...........") self.close_client_and_server() self.setup_client_and_server(reconnect_client_only=False) self.done = True return def init_renderer(self): self.num_cameras = 0 if self.rgb_camera: self.num_cameras += 1 if self.segment_camera: self.num_cameras += 1 if self.depth_camera: self.num_cameras += 1 self.renderer.create_screen(carla_config.render_width, carla_config.render_height*self.num_cameras) def _restart_environment_episode(self, force_environment_reset=True): if not force_environment_reset and not self.done and self.is_game_setup: print("Can't reset dude, episode ain't over yet") return None #User should handle this self.is_game_ready_for_input = False if not self.is_game_setup: self.setup_client_and_server() if self.is_render_enabled: self.init_renderer() else: self.iterator_start_positions += 1 if self.iterator_start_positions >= self.num_pos: self.iterator_start_positions = 0 try: self.game.start_episode(self.iterator_start_positions) except: self.game.connect() self.game.start_episode(self.iterator_start_positions) self.unmoved_steps = 0.0 if self.early_termination_enabled: self.cur_neg_steps = 0 # start the game with some initial speed self.car_speed = 0 observation = None for i in range(self.num_speedup_steps): observation, reward, done, _ = self.step([1.0, 0]) self.observation = observation self.is_game_ready_for_input = True return observation def get_rendered_image(self): temp = [] if self.rgb_camera: temp.append(self.observation['rgb_image']) if self.segment_camera: temp.append(self.observation['colored_segmented_image']) if self.depth_camera: if self.color_the_depth_map: temp.append(depthmap_to_rgb(self.observation['depth_map'])) else: temp.append(depthmap_to_grey(self.observation['depth_map'])) return np.vstack((img for img in temp)) def save_screenshots(self): if not self.save_screens: print("save_screens is set False") return filename = str(int(time.time()*100)) if self.rgb_camera: save_image(self.rgb_img_path+filename+".png", self.observation['rgb_image']) if self.segment_camera: np.save(self.segmented_img_path+filename, self.observation['segmented_image']) if self.depth_camera: save_depthmap_as_16bit_png(self.depth_img_path+filename+".png",self.observation['depth_map'],self.max_depth_value,0.95) #Truncating sky as 0
class CarlaClientConsole(cmd.Cmd): def __init__(self, args): cmd.Cmd.__init__(self) self.args = args self.prompt = '\x1b[1;34m%s\x1b[0m ' % '(carla)' self.client = CarlaClient(args.host, args.port) self.settings = get_default_carla_settings(args) self.print_measurements = False self.control = _Control() self.thread = threading.Thread(target=self._agent_thread_worker) self.done = False self.thread.start() def cleanup(self): self.do_disconnect() self.done = True if self.thread.is_alive(): self.thread.join() def default(self, line): logging.error('unknown command \'%s\'!', line) def emptyline(self): pass def precmd(self, line): return line.strip().lower() def can_exit(self): return True def do_exit(self, line=None): """Exit the console.""" return True def do_eof(self, line=None): """Exit the console.""" print('exit') return self.do_exit(line) def help_help(self): print('usage: help [topic]') def do_disconnect(self, line=None): """Disconnect from the server.""" self.client.disconnect() def do_new_episode(self, line=None): """Request a new episode. Connect to the server if not connected.""" try: self.control = _Control() if not self.client.connected(): self.client.connect() self.client.load_settings(self.settings) self.client.start_episode(0) logging.info('new episode started') except Exception as exception: logging.error(exception) def do_control(self, line=None): """Set control message:\nusage: control [reset|<param> <value>]\n(e.g., control throttle 0.5)""" try: if line == 'reset': self.control = _Control() else: self.control.set(line) logging.debug('control: %s', self.control.kwargs()) except Exception as exception: logging.error(exception) def complete_control(self, text, *args, **kwargs): options = self.control.action_list() options.append('reset') return [x + ' ' for x in options if x.startswith(text)] def do_settings(self, line=None): """Open a text editor to edit CARLA settings.""" result = edit_text(self.settings) if result is not None: self.settings = result def do_print_measurements(self, line): """Print received measurements to console.\nusage: print_measurements [t/f]""" self.print_measurements = True if not line else _Control._parse( bool, line) def _agent_thread_worker(self): filename = '_images/console/camera_{:0>3d}/image_{:0>8d}.png' while not self.done: try: measurements, sensor_data = self.client.read_data() if self.print_measurements: print(measurements) if self.args.images_to_disk: images = [ x for x in sensor_data.values() if isinstance(x, Image) ] for n, image in enumerate(images): path = filename.format(n, measurements.game_timestamp) image.save_to_disk(path) self.client.send_control(**self.control.kwargs()) except Exception as exception: # logging.exception(exception) time.sleep(1)