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 CarlaEnv(gym.Env): 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 dist_from_goal(self, measurements): x = array_from_loc(measurements.player_measurements.transform.location) y = array_from_loc(self.scene.player_start_spots[self.goal].location) return np.sqrt(np.sum((x - y)**2)) def open_server(self): #os.devnull log_file = f"./logs/carla_logs/carla_{self.port}.txt" os.makedirs(os.path.dirname(log_file), exist_ok=True) with open(log_file, "wb+") as out: """ true_script_name = os.path.join(CARLA_PATH, 'CarlaUE4.sh') if os.path.islink(true_script_name): true_script_name = os.readlink(execute_path) project_root = os.path.dirname(true_script_name) execute_path = f"{project_root}/CarlaUE4/Binaries/Linux/CarlaUE4" # chmod +x st = os.stat(execute_path) os.chmod(execute_path, st.st_mode | stat.S_IEXEC) """ cmd = [ os.path.join(CARLA_PATH, 'CarlaUE4.sh'), #execute_path, "CarlaUE4", "-carla-server", "-fps=60", f"-world-port={self.port}", f"-windowed -ResX={500} -ResY={500}", "-carla-no-hud" ] # - benchmark p = subprocess.Popen(cmd, stdout=out, stderr=out, stdin=subprocess.PIPE, preexec_fn=os.setpgrp) return p def close_server(self): #self.server.terminate() pid = self.server.pid no_of_attempts = 0 def is_process_alive(pid): ## Source: https://stackoverflow.com/questions/568271/how-to-check-if-there-exists-a-process-with-a-given-pid-in-python try: os.kill(pid, 0) except OSError: return False return True while is_process_alive(pid): pgroup = os.getpgid(self.server.pid) self.server.terminate() os.killpg(pgroup, signal.SIGTERM) _, _ = os.waitpid(pid, os.WNOHANG) time.sleep(5) def _add_settings(self): self.settings.set(SynchronousMode=True, SendNonPlayerAgentsInfo=True, NumberOfVehicles=self.num_vehicles, NumberOfPedestrians=0, WeatherId=1, QualityLevel="Low", SeedVehicles=self.vehicles_seed()) #settings.randomize_seeds() def _add_sensors(self): camera0 = Camera('RenderCamera0') # Set image resolution in pixels. camera0.set_image_size(800, 600) # Set its position relative to the car in meters. camera0.set_position(-4.30, 0, 2.60) camera0.set_rotation(pitch=-25, yaw=0, roll=0) self.settings.add_sensor(camera0) def _map_controls(self, a): return dict(steer=a[0], throttle=a[1]) def _process_observation(self, measurements, sensor_data): return array_from_measurements(measurements) def _get_reward_and_termination(self): measurements, sensor_data = self.current_state collision_penalty = ( measurements.player_measurements.collision_vehicles + measurements.player_measurements.collision_other) is_collided = collision_penalty > 2 offroad_penalty = measurements.player_measurements.intersection_offroad is_offroad = offroad_penalty > 0.5 dist_from_goal = self.dist_from_goal(measurements) is_at_target = dist_from_goal < 5 is_done = is_collided or is_at_target or is_offroad distance_reward = 300 - dist_from_goal collision_cost = collision_penalty / 300 offroad_cost = (offroad_penalty * 10) reward = distance_reward - collision_cost - offroad_cost reward = reward / 20 return reward, is_done def get_new_start_goal(self): start = np.random.choice(self.starts) goal = np.random.choice(self.goals) while goal == start: start = np.random.choice(self.starts) goal = np.random.choice(self.goals) return start, goal def reset(self): self.settings = CarlaSettings() self._add_settings() self._add_sensors() self.scene = self.client.load_settings(self.settings) start, goal = self.get_new_start_goal() self.goal = goal for i in range(100): try: self.client.start_episode(start) self.current_state = self.client.read_data() if i > 0: print("Reconnected.") break except carla.tcp.TCPConnectionError: if i % 10 == 0: print(f"There was a TCP Error (Attempt {i}). Retrying. ") time.sleep(3) measurements, sensor_data = self.current_state return self._process_observation(measurements, sensor_data) def step(self, a): control = self._map_controls(a) for i in range(100): try: self.client.send_control(**control) break except carla.tcp.TCPConnectionError: time.sleep(0.5) self.current_state = self.client.read_data() measurements, sensor_data = self.current_state reward, is_done = self._get_reward_and_termination() obs = self._process_observation(measurements, sensor_data) return obs, reward, is_done, {} def render(self, mode='human', **kwargs): if mode == 'rgb_array': return np.concatenate([ sensor.data for name, sensor in self.current_state[1].items() if "Render" in name ], axis=1) super().render(mode=mode, **kwargs) def _close(self): self.close() def close(self): print( f"Disconnecting from CARLA Client (port: {self.port}, pid: {self.server.pid})" ) self.close_server() time.sleep(3) while self.client.connected(): self.client.disconnect() print( f"Disconnected from CARLA Client (port: {self.port}, pid: {self.server.pid})." )
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 CarlaEnv(gym.Env): metadata = {'render.modes': ['human', 'rgb_array']} reward_range = (-np.inf, np.inf) spec = None action_space = None observation_space = None 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 reset(self): """ Resets the state of the environment and returns an initial observation. :return: observation (object): the initial observation of the space. """ # load Carla settings settings = CarlaSettings() settings = self._load_settings(settings) scene = self.carla_client.load_settings(settings) # define a random starting point of the agent for the appropriate training number_of_player_starts = len(scene.player_start_spots) player_start = self.rng.randint(0, max(0, number_of_player_starts - 1)) self.carla_client.start_episode(player_start) print(f'Starting new episode at {scene.map_name}, {player_start}...') # read and return status after reset self.cur_measurements, self.cur_image = self._read_data() state = self._state( self.cur_image, self.cur_image ) #possibly revise the state to be some operation of several images. meas = self.cur_measurements return state, meas def render(self, mode='human'): """Renders the environment. :param mode: - human: render to the current display or terminal and return nothing. Usually for human consumption. - rgb_array: Return an numpy.ndarray with shape (x, y, 3), representing RGB values for an x-by-y pixel image, suitable for turning into a video. :return: """ if mode == 'rgb_array': return self.cur_image elif mode == 'human': self.viewer.imshow(arr=self.cur_image) return self.viewer.isopen else: super(CarlaEnv, self).render(mode=mode) # just raise an exception def step(self, action): """ Run one time step of the environment's dynamics. When end of episode is reached, you are responsible for calling `reset()` to reset this environment's state. Accepts an action and returns a tuple (observation, reward, done, info). :param action: an action provided by the environment :return: observation (object): agent's observation of the current environment reward (float) : amount of reward returned after previous action done (boolean): whether the episode has ended, in which case further step() calls will return undefined results info (dict): contains auxiliary diagnostic information (helpful for debugging, and sometimes learning) """ if isinstance(action, dict): self.carla_client.send_control(**action) elif isinstance(action, np.ndarray): # parse control from array throttle = max(0, action[1]) brake = max(0, -action[1]) self.carla_client.send_control(steer=action[0], throttle=throttle, brake=brake) else: self.carla_client.send_control(action) # read measurements and image from Carla measurements, image = self._read_data() # calculate reward reward = self._reward(self.cur_measurements, measurements) done = self._done(measurements) state = self._state(self.cur_image, image) # save current measurements for next use self.cur_measurements = measurements self.cur_image = image return state, reward, done, measurements def close(self): """ Close connection to Carla :return: """ self.carla_client.disconnect() self.viewer.close() def seed(self, seed=None): """Set seed for random number generators used in the code Set seed so that results are consistent in multi runs :param seed: seed number, defaults to None """ self.rng = np.random.RandomState(seed) def _state(self, pre_image, cur_image): def image_proc(img): img = img.transpose(2, 0, 1) img = np.ascontiguousarray(img, dtype=np.float32) / 255 return img # pre_image = image_proc(pre_image) cur_image = image_proc(cur_image) # CHW, use current image as state return cur_image def _load_settings(self, settings): """Load Carla settings Override to customize settings :param settings: default settings :return: new settings """ settings.set(SynchronousMode=True, SendNonPlayerAgentsInfo=True, NumberOfVehicles=20, NumberOfPedestrians=40, WeatherId=self.rng.choice([1, 3, 7, 8, 14]), QualityLevel='Low') # CAMERA camera0 = Camera('CameraRGB', PostProcessing='SceneFinal') # Set image resolution in pixels. camera0.set_image_size(carla_config.CARLA_IMG_HEIGHT, carla_config.CARLA_IMG_WIDTH) # Set its position relative to the car in meters. camera0.set_position(0.30, 0, 1.30) settings.add_sensor(camera0) return settings def _get_sensor_data(self, sensor_data): """Extract sensor data from multi sensor dict Override to customize which sensor (Camera/LiDar) to use :param sensor_data: multi sensor dict :type sensor_data: extracted sensor data """ return sensor_data['CameraRGB'].data def _read_data(self, ): """ read data from carla :return: custom measurement data dict, camera image """ measurements, sensor_data = self.carla_client.read_data() p_meas = measurements.player_measurements pos_x = p_meas.transform.location.x pos_y = p_meas.transform.location.y speed = p_meas.forward_speed * 3.6 # m/s -> km/h col_cars = p_meas.collision_vehicles col_ped = p_meas.collision_pedestrians col_other = p_meas.collision_other other_lane = 100 * p_meas.intersection_otherlane if other_lane: print('Intersection into other lane %.2f' % other_lane) offroad = 100 * p_meas.intersection_offroad if offroad: print('offroad %.2f' % offroad) agents_num = len(measurements.non_player_agents) distance = np.linalg.norm(np.array([pos_x, pos_y]) - self.target) meas = { 'pos_x': pos_x, 'pos_y': pos_y, 'speed': speed, 'col_damage': col_cars + col_ped + col_other, 'other_lane': other_lane, 'offroad': offroad, 'agents_num': agents_num, 'distance': distance, 'autopilot_control': p_meas.autopilot_control } return meas, self._get_sensor_data(sensor_data) def _reward(self, pre_measurements, cur_measurements): """ Calculate reward :param pre_measurements: previous measurement :param cur_measurements: latest measurement :return: reward """ def delta(key): return cur_measurements[key] - pre_measurements[key] if pre_measurements is None: rwd = 0.0 else: rwd = 0.15 * delta('speed') - 0.002 * delta('col_damage') \ - 2 * delta('offroad') - 2 * delta('other_lane') return rwd def _done(self, cur_measurements): """ Check done or not :param cur_measurements: latest measurement :return: """ # check distance to target d = cur_measurements['distance'] < 1 # final state arrived or not return d