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, save_screens=False, port=2000, gpu=0, discrete_control=True, kill_when_connection_lost=True, city_name="Town01", channel_last=True): EnvironmentWrapper.__init__(self, is_render_enabled, save_screens) print("port:", port) self.episode_max_time = 1000000 self.allow_braking = True self.log_path = os.path.join(DEFAULT_CARLA_LOG_DIR, "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 = kill_when_connection_lost # server configuration self.port = port self.gpu = gpu self.host = 'localhost' self.level = 'town1' self.map = CarlaLevel().get(self.level) # experiment = basic_experiment_suite.BasicExperimentSuite(city_name) experiment = CoRL2017(city_name) self.experiments = experiment.get_experiments() self.experiment_type = 0 self.planner = Planner(city_name) 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 = discrete_control self.action_space_size = 3 self.action_space_high = np.array([1, 1, 1]) self.action_space_low = np.array([-1, -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' ] if discrete_control: self.action_space = Discrete(len(self.actions)) else: self.action_space = Box(low=self.action_space_low, high=self.action_space_high) self.observation_space = Box(low=-np.inf, high=np.inf, shape=[88, 200, 3]) # measurements self.measurements_size = (1, ) self.pre_image = None self.first_debug = True self.channel_last = channel_last 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 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: #docker <19.03 # p = subprocess.Popen(['docker', 'run', '--rm', '-d', '-p', # str(self.port) + '-' + str(self.port + 2) + ':' + str(self.port) + '-' + str(self.port + 2), # '--runtime=nvidia', '-e', 'NVIDIA_VISIBLE_DEVICES='+str(self.gpu), "carlasim/carla:0.8.4", # '/bin/bash', 'CarlaUE4.sh', self.map, '-windowed', # '-benchmark', '-fps=10', '-world-port=' + str(self.port)], shell=False, # stdout=subprocess.PIPE) #docker=19.03 p = subprocess.Popen([ 'docker', 'run', '--rm', '-d', '-p', str(self.port) + '-' + str(self.port + 2) + ':' + str(self.port) + '-' + str(self.port + 2), '--gpus=' + str(self.gpu), "carlasim/carla:0.8.4", '/bin/bash', 'CarlaUE4.sh', self.map, '-windowed', '-benchmark', '-fps=10', '-world-port=' + str(self.port) ], shell=False, stdout=subprocess.PIPE) # docker_id = shell("docker ps -q") docker_out, err = p.communicate() # cmd = [path.join(environ.get('CARLA_ROOT'), 'CarlaUE4.sh'), self.map, # "-benchmark", "-carla-server", "-fps=10", "-world-port={}".format(self.port), # "-windowed -ResX={} -ResY={}".format(carla_config.server_width, carla_config.server_height), # "-carla-no-hud"] # p = subprocess.Popen(cmd, stdout=out, stderr=out, env=my_env, preexec_fn=os.setsid) # p = subprocess.Popen(cmd, env=my_env, preexec_fn=os.setsid) return p, docker_out def _close_server(self): if self.kill_when_connection_lost: print("kill before") # os.kill(os.getpgid(self.server.pid), signal.SIGKILL) # self.server.kill() print(self.out) subprocess.call(['docker', 'stop', self.out[:-1]]) while True: cmd = shell("docker ps -q") if cmd == "": break print("kill after") return # no_of_attempts = 0 # while is_process_alive(self.server_pid): # try: # self.server.terminate() # self.server.wait() # os.killpg(self.server.pid, signal.SIGTERM) # time.sleep(10) # except Exception as error: # print(error) 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 _update_state(self): # get measurements and observations measurements = [] while type(measurements) == list: try: measurements, sensor_data = self.game.read_data() except: print( "Connection to server lost while reading state. Reconnecting..........." ) # self.game.disconnect() # self.setup_client_and_server(reconnect_client_only=True) import psutil info = psutil.virtual_memory() print("memory used", str(info.percent)) self.close_client_and_server() self.setup_client_and_server(reconnect_client_only=False) scene = self.game.load_settings(self.cur_experiment.conditions) self.positions = scene.player_start_spots self.start_point = self.positions[self.pose[0]] self.end_point = self.positions[self.pose[1]] self.game.start_episode(self.pose[0]) self.done = True current_point = measurements.player_measurements.transform direction = self._get_directions(current_point, self.end_point) speed = measurements.player_measurements.forward_speed self.reward = 0 dist = sldist([current_point.location.x, current_point.location.y], [self.end_point.location.x, self.end_point.location.y]) if direction == 5: #go straight if abs(self.control.steer) > 0.2: self.reward -= 20 self.reward += min(35, speed * 3.6) elif direction == 2: #follow lane self.reward += min(35, speed * 3.6) elif direction == 3: #turn left ,steer negtive if self.control.steer > 0: self.reward -= 15 if speed * 3.6 <= 20: self.reward += speed * 3.6 else: self.reward += 40 - speed * 3.6 elif direction == 4: #turn right if self.control.steer < 0: self.reward -= 15 if speed * 3.6 <= 20: self.reward += speed * 3.6 else: self.reward += 40 - speed * 3.6 elif direction == 0: self.done = True self.reward += 100 direction = 2 print("success", dist) else: print("error direction") direction = 5 direction -= 2 direction = int(direction) intersection_offroad = measurements.player_measurements.intersection_offroad intersection_otherlane = measurements.player_measurements.intersection_otherlane collision_veh = measurements.player_measurements.collision_vehicles collision_ped = measurements.player_measurements.collision_pedestrians collision_other = measurements.player_measurements.collision_other if intersection_otherlane > 0 or intersection_offroad > 0 or collision_ped > 0 or collision_veh > 0: self.reward -= 100 elif collision_other > 0: self.reward -= 50 if collision_other > 0 or collision_veh > 0 or collision_ped > 0: self.done = True if intersection_offroad > 0.2 or intersection_otherlane > 0.2: self.done = True if speed * 3.6 <= 1: self.nospeed_time += 1 if self.nospeed_time > 100: self.done = True self.reward -= 1 else: self.nospeed_time = 0 # speed = min(1, speed/10.0) speed = speed / 25 # print(measurements) # print(sensor_data) self.observation = { "img": self.process_image(sensor_data['CameraRGB'].data), "speed": speed, "direction": direction } return self.observation, self.reward, self.done def encode_obs(self, image): if self.pre_image is None: self.pre_image = image img = np.concatenate([self.pre_image, image], axis=2) self.pre_image = image return img def process_image(self, data): rgb_img = data[115:510, :] rgb_img = cv2.resize(rgb_img, (200, 88), interpolation=cv2.INTER_AREA) if not self.channel_last: rgb_img = np.transpose(rgb_img, (2, 0, 1)) # return cv2.resize( # data, (80, 80), # interpolation=cv2.INTER_AREA) return rgb_img def _take_action(self, action_idx): if not self.is_game_setup: print("Reset the environment duh by reset() before calling step()") sys.exit(1) if type(action_idx) == np.int64 or type(action_idx) == np.int: action = self.actions[action_idx] else: action = action_idx self.control = VehicleControl() if len(action) == 3: self.control.throttle = np.clip(action[1], 0, 1) self.control.steer = np.clip(action[0], -1, 1) self.control.brake = np.clip(action[2], 0, 1) else: 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 # print(self.control) # # # rand = random.randint(0, 4) # if rand == 0 and self.first_debug: # self.first_debug = False # raise Exception except: print( "Connection to server lost while sending controls. Reconnecting..........." ) import psutil info = psutil.virtual_memory() print("memory used", str(info.percent)) self.close_client_and_server() self.setup_client_and_server(reconnect_client_only=False) scene = self.game.load_settings(self.cur_experiment.conditions) self.positions = scene.player_start_spots self.start_point = self.positions[self.pose[0]] self.end_point = self.positions[self.pose[1]] self.game.start_episode(self.pose[0]) self.done = True controls_sent = False return 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() experiment_type = random.randint(0, 0) self.cur_experiment = self.experiments[experiment_type] self.pose = random.choice(self.cur_experiment.poses) # self.cur_experiment = self.experiments[0] # self.pose = self.cur_experiment.poses[0] scene = self.game.load_settings(self.cur_experiment.conditions) self.positions = scene.player_start_spots self.start_point = self.positions[self.pose[0]] self.end_point = self.positions[self.pose[1]] # print("start episode") while True: try: self.game.start_episode(self.pose[0]) # start the game with some initial speed self.car_speed = 0 self.nospeed_time = 0 observation = None for i in range(self.num_speedup_steps): observation, reward, done, _ = self.step([0, 1.0, 0]) self.observation = observation self.is_game_ready_for_input = True break except Exception as error: print(error) self.game.connect() self.game.start_episode(self.pose[0]) return observation
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
class CarlaEnv(gym.Env): def __init__(self, config=ENV_CONFIG): self.config = config self.city = self.config["server_map"].split("/")[-1] if self.config["enable_planner"]: self.planner = Planner(self.city) # The Action Space if config["discrete_actions"]: self.action_space = Discrete( len(DISCRETE_ACTIONS )) # It will be transformed to continuous 2D action. else: self.action_space = Box(-1.0, 1.0, shape=(2, ), dtype=np.float32) # 2D action. if config["use_depth_camera"]: image_space = Box(-1.0, 1.0, shape=(config["y_res"], config["x_res"], 1 * config["framestack"]), dtype=np.float32) else: image_space = Box(0, 255, shape=(config["y_res"], config["x_res"], 3 * config["framestack"]), dtype=np.uint8) # encode_measure ---> rgb + depth + pre_rgb + measurement_encode # 3 + 1 + 3 + 1 = 8 if config["encode_measurement"]: image_space = Box(0, 255, shape=(config["y_res"], config["x_res"], 8), dtype=np.float32) # The Observation Space self.observation_space = Tuple([ image_space, Discrete(len(COMMANDS_ENUM)), # next_command Box(-128.0, 128.0, shape=(2, ), dtype=np.float32) # forward_speed, dist to goal ]) # TODO(ekl) this isn't really a proper gym spec self._spec = lambda: None self._spec.id = "Carla-v0" self.server_port = None self.server_process = None self.client = None self.num_steps = 0 self.total_reward = 0 self.prev_measurement = None self.prev_image = None self.episode_id = None self.measurements_file = None self.weather = None self.scenario = None self.start_pos = None self.end_pos = None self.start_coord = None self.end_coord = None self.last_obs = None def init_server(self): print("Initializing new Carla server...") # Create a new server process and start the client. self.server_port = random.randint(1000, 60000) self.server_process = subprocess.Popen( [ SERVER_BINARY, self.config["server_map"], "-windowed", "-ResX=800", "-ResY=600", "-carla-server", "-benchmark -fps=10", #: to run the simulation at a fixed time-step of 0.1 seconds "-carla-world-port={}".format(self.server_port) ], preexec_fn=os.setsid, stdout=open(os.devnull, "w")) live_carla_processes.add(os.getpgid(self.server_process.pid)) for i in range(RETRIES_ON_ERROR): try: self.client = CarlaClient("localhost", self.server_port) return self.client.connect() except Exception as e: print("Error connecting: {}, attempt {}".format(e, i)) time.sleep(2) def clear_server_state(self): print("Clearing Carla server state") try: if self.client: self.client.disconnect() self.client = None except Exception as e: print("Error disconnecting client: {}".format(e)) pass if self.server_process: pgid = os.getpgid(self.server_process.pid) os.killpg(pgid, signal.SIGKILL) live_carla_processes.remove(pgid) self.server_port = None self.server_process = None def __del__( self ): # the __del__ method will be called when the instance of the class is deleted.(memory is freed.) self.clear_server_state() def reset(self): error = None for _ in range(RETRIES_ON_ERROR): try: if not self.server_process: self.init_server() return self._reset() except Exception as e: print("Error during reset: {}".format(traceback.format_exc())) self.clear_server_state() error = e raise error def _reset(self): self.num_steps = 0 self.total_reward = 0 self.prev_measurement = None self.prev_image = None self.episode_id = datetime.today().strftime("%Y-%m-%d_%H-%M-%S_%f") self.measurements_file = None # Create a CarlaSettings object. This object is a wrapper around # the CarlaSettings.ini file. Here we set the configuration we # want for the new episode. settings = CarlaSettings() self.scenario = random.choice(self.config["scenarios"]) assert self.scenario["city"] == self.city, (self.scenario, self.city) self.weather = random.choice(self.scenario["weather_distribution"]) settings.set( SynchronousMode=True, # ServerTimeOut=10000, # CarlaSettings: no key named 'ServerTimeOut' SendNonPlayerAgentsInfo=True, NumberOfVehicles=self.scenario["num_vehicles"], NumberOfPedestrians=self.scenario["num_pedestrians"], WeatherId=self.weather) settings.randomize_seeds() if self.config["use_depth_camera"]: camera1 = Camera("CameraDepth", PostProcessing="Depth") camera1.set_image_size(self.config["render_x_res"], self.config["render_y_res"]) # camera1.set_position(30, 0, 170) camera1.set_position(0.5, 0.0, 1.6) # camera1.set_rotation(0.0, 0.0, 0.0) settings.add_sensor(camera1) if self.config["encode_measurement"]: camera1 = Camera("CameraDepth", PostProcessing="Depth") camera1.set_image_size(self.config["render_x_res"], self.config["render_y_res"]) # camera1.set_position(30, 0, 170) camera1.set_position(0.5, 0.0, 1.6) camera1.set(FOV=120) settings.add_sensor(camera1) camera2 = Camera("CameraRGB") camera2.set_image_size(self.config["render_x_res"], self.config["render_y_res"]) camera2.set(FOV=120) camera2.set_position(0.5, 0.0, 1.6) settings.add_sensor(camera2) # Setup start and end positions scene = self.client.load_settings(settings) positions = scene.player_start_spots self.start_pos = positions[self.scenario["start_pos_id"]] self.end_pos = positions[self.scenario["end_pos_id"]] self.start_coord = [ self.start_pos.location.x, self.start_pos.location.y ] self.end_coord = [self.end_pos.location.x, self.end_pos.location.y] print("Start pos {} ({}), end {} ({})".format( self.scenario["start_pos_id"], [int(x) for x in self.start_coord], self.scenario["end_pos_id"], [int(x) for x in self.end_coord])) # Notify the server that we want to start the episode at the # player_start index. This function blocks until the server is ready # to start the episode. print("Starting new episode...") self.client.start_episode(self.scenario["start_pos_id"]) # Process observations: self._read_observation() returns image and py_measurements. image, py_measurements = self._read_observation() self.prev_measurement = py_measurements return self.encode_obs(self.preprocess_image(image), py_measurements) # rgb depth forward_speed next_comment 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["encode_measurement"]: feature_map = np.zeros([4, 4]) feature_map[1, :] = (py_measurements["forward_speed"] - 15) / 15 feature_map[2, :] = ( COMMAND_ORDINAL[py_measurements["next_command"]] - 2) / 2 feature_map[0, 0] = py_measurements["x_orient"] feature_map[0, 1] = py_measurements["y_orient"] feature_map[0, 2] = (py_measurements["distance_to_goal"] - 170) / 170 feature_map[0, 1] = (py_measurements["distance_to_goal_euclidean"] - 170) / 170 feature_map[3, 0] = (py_measurements["x"] - 50) / 150 feature_map[3, 1] = (py_measurements["y"] - 50) / 150 feature_map[3, 2] = (py_measurements["end_coord"][0] - 150) / 150 feature_map[3, 3] = (py_measurements["end_coord"][1] - 150) / 150 feature_map = np.tile(feature_map, (24, 24)) image = np.concatenate( [prev_image[:, :, 0:3], image, feature_map[:, :, np.newaxis]], axis=2) # print("image shape after encoding", image.shape) obs = (image, COMMAND_ORDINAL[py_measurements["next_command"]], [ py_measurements["forward_speed"], py_measurements["distance_to_goal"] ]) self.last_obs = obs # print('distance to goal', py_measurements["distance_to_goal"]) # print("speed", py_measurements["forward_speed"]) return obs # TODO:example of py_measurement # {'episode_id': '2019-02-22_11-26-36_990083', # 'step': 0, # 'x': 71.53003692626953, # 'y': 302.57000732421875, # 'x_orient': -1.0, # 'y_orient': -6.4373016357421875e-06, # 'forward_speed': -3.7578740032934155e-13, # 'distance_to_goal': 0.6572, # 'distance_to_goal_euclidean': 0.6200001239776611, # 'collision_vehicles': 0.0, # 'collision_pedestrians': 0.0, # 'collision_other': 0.0, # 'intersection_offroad': 0.0, # 'intersection_otherlane': 0.0, # 'weather': 0, # 'map': '/Game/Maps/Town02', # 'start_coord': [0.0, 3.0], # 'end_coord': [0.0, 3.0], # 'current_scenario': {'city': 'Town02', # 'num_vehicles': 0, # 'num_pedestrians': 20, # 'weather_distribution': [0], # 'start_pos_id': 36, # 'end_pos_id': 40, # 'max_steps': 600}, # 'x_res': 10, # 'y_res': 10, # 'num_vehicles': 0, # 'num_pedestrians': 20, # 'max_steps': 600, # 'next_command': 'GO_STRAIGHT', # 'action': (0, 1), # 'control': {'steer': 1.0, # 'throttle': 0.0, # 'brake': 0.0, # 'reverse': False, # 'hand_brake': False}, # 'reward': 0.0, # 'total_reward': 0.0, # 'done': False} def step(self, action): try: obs = self._step(action) return obs except Exception: print("Error during step, terminating episode early", traceback.format_exc()) self.clear_server_state() return (self.last_obs, 0.0, True, {}) def _step(self, action): if self.config["discrete_actions"]: action = DISCRETE_ACTIONS[int(action)] # Carla action is 2D. assert len(action) == 2, "Invalid action {}".format(action) if self.config["squash_action_logits"]: forward = 2 * float(sigmoid(action[0]) - 0.5) throttle = float(np.clip(forward, 0, 1)) brake = float(np.abs(np.clip(forward, -1, 0))) steer = 2 * float(sigmoid(action[1]) - 0.5) else: throttle = float(np.clip(action[0], 0, 1)) brake = float(np.abs(np.clip(action[0], -1, 0))) steer = float(np.clip(action[1], -1, 1)) # reverse and hand_brake are disabled. reverse = False hand_brake = False if self.config["verbose"]: print("steer", steer, "throttle", throttle, "brake", brake, "reverse", reverse) self.client.send_control(steer=steer, throttle=throttle, brake=brake, hand_brake=hand_brake, reverse=reverse) # Process observations: self._read_observation() returns image and py_measurements. image, py_measurements = self._read_observation() if self.config["verbose"]: print("Next command", py_measurements["next_command"]) if type(action) is np.ndarray: py_measurements["action"] = [float(a) for a in action] else: py_measurements["action"] = action py_measurements["control"] = { "steer": steer, "throttle": throttle, "brake": brake, "reverse": reverse, "hand_brake": hand_brake, } # compute reward reward = compute_reward(self, self.prev_measurement, py_measurements) self.total_reward += reward py_measurements["reward"] = reward py_measurements["total_reward"] = self.total_reward # done or not done = (self.num_steps > self.scenario["max_steps"] or py_measurements["next_command"] == "REACH_GOAL" or (self.config["early_terminate_on_collision"] and collided_done(py_measurements))) py_measurements["done"] = done self.prev_measurement = py_measurements # Write out measurements to file if self.config["verbose"] and CARLA_OUT_PATH: if not self.measurements_file: self.measurements_file = open( os.path.join( CARLA_OUT_PATH, "measurements_{}.json".format(self.episode_id)), "w") self.measurements_file.write(json.dumps(py_measurements)) self.measurements_file.write("\n") if done: self.measurements_file.close() self.measurements_file = None if self.config["convert_images_to_video"]: self.images_to_video() self.num_steps += 1 image = self.preprocess_image(image) return (self.encode_obs(image, py_measurements), reward, done, py_measurements) def images_to_video(self): videos_dir = os.path.join(CARLA_OUT_PATH, "Videos") if not os.path.exists(videos_dir): os.makedirs(videos_dir) ffmpeg_cmd = ( "ffmpeg -loglevel -8 -r 60 -f image2 -s {x_res}x{y_res} " "-start_number 0 -i " "{img}_%04d.jpg -vcodec libx264 {vid}.mp4 && rm -f {img}_*.jpg " ).format(x_res=self.config["render_x_res"], y_res=self.config["render_y_res"], vid=os.path.join(videos_dir, self.episode_id), img=os.path.join(CARLA_OUT_PATH, "CameraRGB", self.episode_id)) print("Executing ffmpeg command", ffmpeg_cmd) subprocess.call(ffmpeg_cmd, shell=True) def preprocess_image(self, image): if self.config["use_depth_camera"]: assert self.config["use_depth_camera"] data = (image.data - 0.5) * 2 data = data.reshape(self.config["render_y_res"], self.config["render_x_res"], 1) data = cv2.resize(data, (self.config["x_res"], self.config["y_res"]), interpolation=cv2.INTER_AREA) data = np.expand_dims(data, 2) elif self.config["encode_measurement"]: # data = (image - 0.5) * 2 data = image.reshape(self.config["render_y_res"], self.config["render_x_res"], -1) #data[:, :, 4] = (data[:, :, 0] - 0.5) * 2 #data[:, :, 0:3] = (data[:, :, 0:2].astype(np.float32) - 128) / 128 data = cv2.resize(data, (self.config["x_res"], self.config["y_res"]), interpolation=cv2.INTER_AREA) 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["encode_measurement"]: # print(sensor_data["CameraRGB"].data.shape, sensor_data["CameraDepth"].data.shape) observation = np.concatenate( ((sensor_data["CameraRGB"].data.astype(np.float32) - 128) / 128, (sensor_data["CameraDepth"].data[:, :, np.newaxis] - 0.5) * 0.5), axis=2) # print("observation_shape", observation.shape) else: 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 ]) # now the metrix should be meter carla8.0 # TODO run experience to verify the scale of distance in order to determine reward function 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 ])) 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
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): 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 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 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 CarlaEnv: 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 step(self, action): """ :param action: dict of control signals, such as {'steer':0, 'throttle':0.2} :return: next_state, reward, done, info """ if isinstance(action, dict): self.carla_client.send_control(**action) print(action) else: self.carla_client.send_control(action) measurements, self.cur_image = self.carla_client.read_data() self.cur_measurements = self.extract_measurements(measurements) # Todo: Checkup the reward function with the images reward, done = self.cal_reward() self.pre_measurements = self.cur_measurements self.pre_image = self.cur_image return self.cur_measurements, self.cur_image, reward, done, measurements def reset(self): """ :return: """ settings = CarlaSettings() settings.set(SynchronousMode=True, SendNonPlayerAgentsInfo=True, NumberOfVehicles=20, NumberOfPedestrians=40, WeatherId=random.choice([1, 3, 7, 8, 14]), QualityLevel='Epic') # CAMERA camera0 = Camera('CameraRGB', PostProcessing='SceneFinal') # Set image resolution in pixels. camera0.set_image_size(800, 600) # Set its position relative to the car in meters. camera0.set_position(0.30, 0, 1.30) settings.add_sensor(camera0) # Let's add another camera producing ground-truth depth. # camera1 = Camera('CameraDepth', PostProcessing='Depth') # camera1.set_image_size(800, 600) # camera1.set_position(0.30, 0, 1.30) # settings.add_sensor(camera1) # LIDAR # lidar = Lidar('Lidar32') # lidar.set_position(0, 0, 2.50) # lidar.set_rotation(0, 0, 0) # lidar.set( # Channels=32, # Range=50, # PointsPerSecond=100000, # RotationFrequency=10, # UpperFovLimit=10, # LowerFovLimit=-30) # settings.add_sensor(lidar) scene = self.carla_client.load_settings(settings) self.pre_image = None self.cur_image = None self.pre_measurements = None self.cur_measurements = None # define a random starting point of the agent for the appropriate trainning number_of_player_starts = len(scene.player_start_spots) player_start = random.randint(0, max(0, number_of_player_starts - 1)) # player_start = 140 self.carla_client.start_episode(player_start) print('Starting new episode at %r, %d...' % (scene.map_name, player_start)) # TODO: read and return status after reset return def extract_measurements(self, measurements): """ extract custom measurement data from carla measurement :param measurements: :return: custom measurement data dict """ 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 } message = 'Vehicle at %.1f, %.1f, ' % (pos_x, pos_y) message += '%.0f km/h, ' % (speed, ) message += 'Collision: vehicles=%.0f, pedestrians=%.0f, other=%.0f, ' % ( col_cars, col_ped, col_other, ) message += '%.0f%% other lane, %.0f%% off-road, ' % ( other_lane, offroad, ) message += '%d non-player agents in the scene.' % (agents_num, ) # print(message) return meas def cal_reward(self): """ :param :return: reward, done """ def delta(key): return self.cur_measurements[key] - self.pre_measurements[key] if self.pre_measurements is None: reward = 0.0 else: reward = 0.05 * delta('speed') - 0.002 * delta('col_damage') \ - 2 * delta('offroad') - 2 * delta('other_lane') # print('delta speed %.3f' % delta('speed') ) # print('[pre_offroad, current_offroad] =[%.2f, %.2f], reward: %.2f' % (self.pre_measurements['offroad'], self.cur_measurements['offroad'], reward)) # 1000 * delta('distance') + ignore the distance for auto-pilot # check distance to target done = self.cur_measurements[ 'distance'] < 1 # final state arrived or not return reward, done def action_discretize(self, control): """ discrete the control action :param action: :return: """ # print('Before processing, steer=%.5f,throttle=%.2f,brake=%.2f' % ( # action.steer, action.throttle, action.brake)) action = control steer = int( 100 * action.steer) + 100 #action.steer has 21 options from [-1, 1] throttle = int(action.throttle / 0.5) # action.throttle= 0, 0.5 or 1.0 brake = int(action.brake) # action.brake=0 or 1.0 if brake: gas = -brake # -1 else: gas = throttle # 0, 1, 2 gas += 1 action_no = steer << 2 | gas # map the action combination into the a numerical value #gas takes two digits, steer takes 5 digits action.steer, action.throttle, action.brake = ( steer - 100) / 100.0, throttle * 0.5, brake * 1.0 # print('After processing, steer=%.5f,throttle=%.2f,brake=%.2f' % ( # action.steer, action.throttle, action.brake)) return action_no, action def reverse_action(self, action_no): """ map the action number to the discretized action control :param action_no: :return: """ gas = action_no & 0b11 throttle = 0.0 brake = 0.0 if gas: throttle = (gas - 1) * 0.5 else: brake = abs(gas - 1) * 1.0 steer = (((action_no & 0b1111111100) >> 2) - 100) / 100.0 action = dict() action['steer'], action['throttle'], action[ 'brake'] = steer, throttle, brake return action def close(self): """ :return: """ self.carla_client.disconnect()
class CarlaEnvironment(EnvironmentInterface): def __init__(self, experiment_path=None, frame_skip=1, server_height=512, server_width=720, camera_height=88, camera_width=200, experiment_suite=None, quality="low", cameras=[CameraTypes.FRONT], weather_id=[1], episode_max_time=30000, max_speed=35.0, port=2000, map_name="Town01", verbose=True, seed=None, is_rendered=True, num_speedup_steps=30, separate_actions_for_throttle_and_brake=False, rendred_image_type='forward_camera'): self.frame_skip = frame_skip # the frame skip affects the fps of the server directly. fps = 30 / frameskip self.server_height = server_height self.server_width = server_width self.camera_height = camera_height self.camera_width = camera_width self.experiment_suite = experiment_suite # an optional CARLA experiment suite to use self.quality = quality self.cameras = cameras self.weather_id = weather_id self.episode_max_time = episode_max_time # miliseconds for each episode self.max_speed = max_speed # km/h self.port = port self.host = 'localhost' self.map_name = map_name self.map_path = map_path_mapper[self.map_name] self.experiment_path = experiment_path self.current_episode_steps_counter = 1 # client configuration self.verbose = verbose self.episode_idx = 0 self.num_speedup_steps = num_speedup_steps self.max_speed = max_speed self.is_rendered = is_rendered # setup server settings self.experiment_suite = experiment_suite self.separate_actions_for_throttle_and_brake = separate_actions_for_throttle_and_brake self.rendred_image_type = rendred_image_type self.left_poses = [] self.right_poses = [] self.follow_poses = [] self.Straight_poses = [] self.settings = CarlaSettings() self.settings.set(SynchronousMode=True, SendNonPlayerAgentsInfo=False, NumberOfVehicles=15, NumberOfPedestrians=30, WeatherId=self.weather_id, QualityLevel=self.quality, 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) print("Successfully opened the server") # open the client self.game = CarlaClient(self.host, self.port, timeout=99999999) print("Successfully opened the client") self.game.connect() print("Successfull Connection") 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 self.action_space = ActionSpace( shape=2, low=np.array([-1, -1]), high=np.array([1, 1]), descriptions=["steer", "gas_and_brake"]) # state space #define all measurments self.state_space = { "measurements": { "forward_speed": np.array([1]), "x": np.array([1]), "y": np.array([1]), "z": np.array([1]) } } #define all cameras for camera in self.scene.sensors: self.state_space[camera.name] = { "data": np.array([self.camera_height, self.camera_width, 3]) } print("Define ", camera.name, " Camera") # measurements self.autopilot = None self.planner = Planner(self.map_name) #rendering if self.is_rendered: pygame.init() pygame.font.init() self.display = pygame.display.set_mode( (self.camera_width, self.camera_height)) # env initialization self.reset(True) def reset(self, force_environment_reset=False): """ Reset the environment and all the variable of the wrapper :param force_environment_reset: forces environment reset even when the game did not end :return: A dictionary containing the observation, reward, done flag, action and measurements """ self.reset_ep = True self.restart_environment_episode(force_environment_reset) self.last_episode_time = time.time() if self.current_episode_steps_counter > 0: self.episode_idx += 1 self.done = False self.total_reward_in_current_episode = self.reward = 0.0 self.last_action = 0 self.current_episode_steps_counter = 0 self.last_episode_images = [] self.step([0, 1, 0]) self.last_env_response = { "reward": self.reward, "next_state": self.state, "goal": self.current_goal, "game_over": self.done } return self.last_env_response def add_cameras(self, settings, cameras, camera_width, camera_height): # add a front facing camera print("Available Cameras are ", cameras) 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) print("Successfully adding a SemanticSegmentation 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" ] print("CMD is : ", cmd) # 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 step(self, action): # get measurements and observations measurements = [] while type(measurements) == list: measurements, sensor_data = self.game.read_data() self.state = {} for camera in self.scene.sensors: if camera.name == 'segmentation': #labels_to_road_noroad taker Sensor.Image not numpy array self.state[camera.name] = labels_to_road_noroad( sensor_data[camera.name]) else: self.state[camera.name] = sensor_data[camera.name].data #self.state[camera.name] = sensor_data[camera.name].data self.location = [ measurements.player_measurements.transform.location.x, measurements.player_measurements.transform.location.y, measurements.player_measurements.transform.location.z ] self.distance_from_goal = np.linalg.norm( np.array(self.location[:2]) - [self.current_goal.location.x, self.current_goal.location.y]) is_collision = measurements.player_measurements.collision_vehicles != 0 \ or measurements.player_measurements.collision_pedestrians != 0 \ or measurements.player_measurements.collision_other != 0 speed_reward = measurements.player_measurements.forward_speed - 1 if speed_reward > 30.: speed_reward = 30. self.reward = speed_reward \ - (measurements.player_measurements.intersection_otherlane * 5) \ - (measurements.player_measurements.intersection_offroad * 5) \ - is_collision * 100 \ - np.abs(self.control.steer) * 10 # update measurements #self.measurements = [measurements.player_measurements.forward_speed] + self.location #TODO, Add control signals to measurements control_signals = [ np.clip(action[0], -1, 1), np.clip(action[1], 0, 1), np.clip(action[2], 0, 1) ] #steer, throttle, brake self.measurements = [measurements.player_measurements.forward_speed ] + self.location + control_signals 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) # if directions == 0: # if self.reset_ep: # self.follow_poses.append((self.current_start,self.current_goal)) # self.reset_ep = False # elif directions == 1: # self.left_poses.append((self.current_start,self.current_goal)) # elif directions == 2: # self.right_poses.append((self.current_start,self.current_goal)) # elif directions == 1: # if self.reset_ep: # self.left_poses.append((self.current_start,self.current_goal)) # self.reset_ep = False # elif directions == 2: # if self.reset_ep: # self.right_poses.append((self.current_start,self.current_goal)) # self.reset_ep = False # elif directions == 3: # if self.reset_ep: # self.Straight_poses.append((self.current_start,self.current_goal)) # self.reset_ep = False # else: # self.reset_ep = False self.state['high_level_command'] = directions if (measurements.game_timestamp >= self.episode_max_time) or is_collision: self.done = True self.state['measurements'] = np.array(self.measurements) #prepare rendered image and update display if self.is_rendered: #check if user wants to close rendering, else continue rendering for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.quit() self.is_rendered = False self.surface = pygame.surfarray.make_surface( self.state[self.rendred_image_type].swapaxes(0, 1)) self.display.blit(self.surface, (0, 0)) pygame.display.flip() self.take_action(action) def take_action(self, action): self.control = VehicleControl() # 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[2], 0, 1)) # 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): print("Loading the experiment") 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 print("restarting new Episode") 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_start = self.positions[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 self.current_start = self.positions[ self.current_start_position_idx] # 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 CarlaHuman(Driver): def __init__(self, driver_conf): Driver.__init__(self) # some behaviors self._autopilot = driver_conf.autopilot self._reset_period = driver_conf.reset_period # those reset period are in the actual system time, not in simulation time self._goal_reaching_threshold = 3 self.use_planner = driver_conf.use_planner # we need the planner to find a valid episode, so we initialize one no matter what self._world = None self._vehicle = None self._agent_autopilot = None self._camera_center = None self._spectator = None # (last) images store for several cameras self._data_buffers = dict() self.update_once = False self._collision_events = [] self.collision_sensor = None if __CARLA_VERSION__ == '0.8.X': self.planner = Planner(driver_conf.city_name) else: self.planner = None self.use_planner = False # resources self._host = driver_conf.host self._port = driver_conf.port # various config files self._driver_conf = driver_conf self._config_path = driver_conf.carla_config # some initializations self._straight_button = False self._left_button = False self._right_button = False self._rear = False self._recording = False self._skiped_frames = 20 self._stucked_counter = 0 self._prev_time = datetime.now() self._episode_t0 = datetime.now() self._vehicle_prev_location = namedtuple("vehicle", "x y z") self._vehicle_prev_location.x = 0.0 self._vehicle_prev_location.y = 0.0 self._vehicle_prev_location.z = 0.0 self._camera_left = None self._camera_right = None self._camera_center = None self._actor_list = [] self._sensor_list = [] self._weather_list = [ 'ClearNoon', 'CloudyNoon', 'WetNoon', 'WetCloudyNoon', 'MidRainyNoon', 'HardRainNoon', 'SoftRainNoon', 'ClearSunset', 'CloudySunset', 'WetSunset', 'WetCloudySunset', 'MidRainSunset', 'HardRainSunset', 'SoftRainSunset' ] self._current_weather = 4 self._current_command = 2.0 # steering wheel self._steering_wheel_flag = True if self._steering_wheel_flag: self._is_on_reverse = False self._control = VehicleControl() self._parser = SafeConfigParser() self._parser.read('wheel_config.ini') self._steer_idx = int( self._parser.get('G29 Racing Wheel', 'steering_wheel')) self._throttle_idx = int( self._parser.get('G29 Racing Wheel', 'throttle')) self._brake_idx = int(self._parser.get('G29 Racing Wheel', 'brake')) self._reverse_idx = int( self._parser.get('G29 Racing Wheel', 'reverse')) self._handbrake_idx = int( self._parser.get('G29 Racing Wheel', 'handbrake')) self.last_timestamp = lambda x: x self.last_timestamp.elapsed_seconds = 0.0 self.last_timestamp.delta_seconds = 0.2 self.initialize_map(driver_conf.city_name) 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 test_alive(self): if not hasattr(self, "wd"): return False wd = self.wd wd.tick() try: wd.wait_for_tick(5.0) except: return False return True def __del__(self): if hasattr(self, 'carla'): print("destructing the connection") if __CARLA_VERSION__ == '0.8.X': self.carla.disconnect() else: alive = self.test_alive() # destroy old actors print('destroying actors') if alive: if len(self._actor_list) > 0: for actor in self._actor_list: actor.destroy() self._actor_list = [] print('done.') if self._vehicle is not None: if alive: self._vehicle.destroy() self._vehicle = None if self._camera_center is not None: if alive: self._camera_center.destroy() self._camera_center = None if self._camera_left is not None: if alive: self._camera_left.destroy() self._camera_left = None if self._camera_right is not None: if alive: self._camera_right.destroy() self._camera_right = None if self.collision_sensor is not None: if alive: self.collision_sensor.sensor.destroy() self.collision_sensor = None # pygame.quit() # if self._camera is not None: # self._camera.destroy() # self._camera = None # if self._vehicle is not None: # self._vehicle.destroy() # self._vehicle = None def try_spawn_random_vehicle_at(self, blueprints, transform, auto_drive=True): blueprint = random.choice(blueprints) if blueprint.has_attribute('color'): color = random.choice( blueprint.get_attribute('color').recommended_values) blueprint.set_attribute('color', color) vehicle = self._world.try_spawn_actor(blueprint, transform) if vehicle is not None: self._actor_list.append(vehicle) if auto_drive: # TODO: this won't work in 0.9.5 with Exp_Town # however, we don't have traffic in that town right now, so we just ignore this vehicle.set_autopilot() #print('spawned %r at %s' % (vehicle.type_id, transform.location)) return True return False def get_parking_locations(self, filename, z_default=0.0, random_perturb=False): with open(filename, "r") as f: lines = f.readlines() ans = [] for line in lines: x, y, yaw = [float(v.strip()) for v in line.split(",")] if random_perturb: x += np.random.normal( 0, scale=self._driver_conf.extra_explore_location_std) y += np.random.normal( 0, scale=self._driver_conf.extra_explore_location_std) yaw += np.random.normal( 0, scale=self._driver_conf.extra_explore_yaw_std) ans.append( carla.Transform(location=carla.Location(x=x, y=y, z=z_default), rotation=carla.Rotation(roll=0, pitch=0, yaw=yaw))) return ans def print_transform(self, t): print(t.location.x, t.location.y, t.location.z) print(t.rotation.roll, t.rotation.pitch, t.rotation.yaw) def _reset(self): self._episode_t0 = datetime.now() if __CARLA_VERSION__ == '0.8.X': # create the carla config based on template and the params passed in config = ConfigParser() config.optionxform = str config.read(self._config_path) config.set('CARLA/LevelSettings', 'NumberOfVehicles', self._driver_conf.cars) config.set('CARLA/LevelSettings', 'NumberOfPedestrians', self._driver_conf.pedestrians) config.set('CARLA/LevelSettings', 'WeatherId', self._driver_conf.weather) output = io.StringIO() config.write(output) scene_descriptions = self.carla.load_settings(output.getvalue()) # based on the scene descriptions, find the start and end positions self.positions = scene_descriptions.player_start_spots # the episode_config saves [start_index, end_index] self.episode_config = find_valid_episode_position( self.positions, self.planner) self.carla.start_episode(self.episode_config[0]) print('RESET ON POSITION ', self.episode_config[0], ", the target location is: ", self.episode_config[1]) else: # destroy old actors print('destroying actors') for actor in self._actor_list: actor.destroy() self._actor_list = [] print('done.') # TODO: spawn pedestrains # TODO: spawn more vehicles if self._autopilot: self._current_weather = self._weather_list[ int(self._driver_conf.weather) - 1] else: self._current_weather = random.choice(self._weather_list) if not self._autopilot: # select one of the random starting points previously selected start_positions = np.loadtxt(self._driver_conf.positions_file, delimiter=',') if len(start_positions.shape) == 1: start_positions = start_positions.reshape( 1, len(start_positions)) # TODO: Assign random position from file WINDOW_WIDTH = 768 WINDOW_HEIGHT = 576 CAMERA_FOV = 103.0 CAMERA_CENTER_T = carla.Location(x=0.7, y=-0.0, z=1.60) CAMERA_LEFT_T = carla.Location(x=0.7, y=-0.4, z=1.60) CAMERA_RIGHT_T = carla.Location(x=0.7, y=0.4, z=1.60) CAMERA_CENTER_ROTATION = carla.Rotation(roll=0.0, pitch=0.0, yaw=0.0) CAMERA_LEFT_ROTATION = carla.Rotation(roll=0.0, pitch=0.0, yaw=-45.0) CAMERA_RIGHT_ROTATION = carla.Rotation(roll=0.0, pitch=0.0, yaw=45.0) CAMERA_CENTER_TRANSFORM = carla.Transform( location=CAMERA_CENTER_T, rotation=CAMERA_CENTER_ROTATION) CAMERA_LEFT_TRANSFORM = carla.Transform( location=CAMERA_LEFT_T, rotation=CAMERA_LEFT_ROTATION) CAMERA_RIGHT_TRANSFORM = carla.Transform( location=CAMERA_RIGHT_T, rotation=CAMERA_RIGHT_ROTATION) self._world = self.carla.get_world() settings = self._world.get_settings() settings.synchronous_mode = True self._world.apply_settings(settings) # add traffic blueprints_vehi = self._world.get_blueprint_library().filter( 'vehicle.*') blueprints_vehi = [ x for x in blueprints_vehi if int(x.get_attribute('number_of_wheels')) == 4 ] blueprints_vehi = [ x for x in blueprints_vehi if not x.id.endswith('isetta') ] # @todo Needs to be converted to list to be shuffled. spawn_points = list(self._world.get_map().get_spawn_points()) if len(spawn_points) == 0: if self.city_name_demo == "Exp_Town": spawn_points = [ carla.Transform( location=carla.Location(x=-11.5, y=-8.0, z=2.0)) ] random.shuffle(spawn_points) print('found %d spawn points.' % len(spawn_points)) # TODO: debug change 50 to 0 count = 0 if count > 0: for spawn_point in spawn_points: if self.try_spawn_random_vehicle_at( blueprints_vehi, spawn_point): count -= 1 if count <= 0: break while count > 0: time.sleep(0.5) if self.try_spawn_random_vehicle_at( blueprints_vehi, random.choice(spawn_points)): count -= 1 # end traffic addition! # begin parking addition if hasattr( self._driver_conf, "parking_position_file" ) and self._driver_conf.parking_position_file is not None: parking_points = self.get_parking_locations( self._driver_conf.parking_position_file) random.shuffle(parking_points) print('found %d parking points.' % len(parking_points)) count = 200 for spawn_point in parking_points: self.try_spawn_random_vehicle_at(blueprints_vehi, spawn_point, False) count -= 1 if count <= 0: break # end of parking addition blueprints = self._world.get_blueprint_library().filter('vehicle') vechile_blueprint = [ e for i, e in enumerate(blueprints) if e.id == 'vehicle.lincoln.mkz2017' ][0] if self._vehicle == None or self._autopilot: if self._autopilot and self._vehicle is not None: self._vehicle.destroy() self._vehicle = None while self._vehicle == None: if self._autopilot: # from designated points if hasattr(self._driver_conf, "extra_explore_prob") and random.random( ) < self._driver_conf.extra_explore_prob: extra_positions = self.get_parking_locations( self._driver_conf.extra_explore_position_file, z_default=3.0, random_perturb=True) print( "spawning hero vehicle from the extra exploration" ) START_POSITION = random.choice(extra_positions) else: START_POSITION = random.choice(spawn_points) else: random_position = start_positions[ np.random.randint(start_positions.shape[0]), :] START_POSITION = carla.Transform( carla.Location(x=random_position[0], y=random_position[1], z=random_position[2] + 1.0), carla.Rotation(pitch=random_position[3], roll=random_position[4], yaw=random_position[5])) self._vehicle = self._world.try_spawn_actor( vechile_blueprint, START_POSITION) else: if self._autopilot: # from designated points START_POSITION = random.choice(spawn_points) else: random_position = start_positions[ np.random.randint(start_positions.shape[0]), :] START_POSITION = carla.Transform( carla.Location(x=random_position[0], y=random_position[1], z=random_position[2] + 1.0), carla.Rotation(pitch=random_position[3], roll=random_position[4], yaw=random_position[5])) self._vehicle.set_transform(START_POSITION) print("after spawning the ego vehicle") print("warm up process to make the vehicle ego location correct") wd = self._world for i in range(25): wd.tick() if not wd.wait_for_tick(10.0): continue print("warmup finished") if self._autopilot: # Nope: self._vehicle.set_autopilot() print("before roaming agent") self._agent_autopilot = RoamingAgent(self._vehicle) print("after roaming agent") if self.collision_sensor is not None: print("before destroying the sensor") self.collision_sensor.sensor.destroy() print("after destroying the sensor") else: print("collision sensor is None") self.collision_sensor = CollisionSensor(self._vehicle, self) print("after spawning the collision sensor") # set weather weather = getattr(carla.WeatherParameters, self._current_weather) self._vehicle.get_world().set_weather(weather) self._spectator = self._world.get_spectator() cam_blueprint = self._world.get_blueprint_library().find( 'sensor.camera.rgb') cam_blueprint.set_attribute('image_size_x', str(WINDOW_WIDTH)) cam_blueprint.set_attribute('image_size_y', str(WINDOW_HEIGHT)) cam_blueprint.set_attribute('fov', str(CAMERA_FOV)) if self._camera_center is not None: self._camera_center.destroy() self._camera_left.destroy() self._camera_right.destroy() self._camera_center = None if self._camera_center == None: self._camera_center = self._world.spawn_actor( cam_blueprint, CAMERA_CENTER_TRANSFORM, attach_to=self._vehicle) self._camera_left = self._world.spawn_actor( cam_blueprint, CAMERA_LEFT_TRANSFORM, attach_to=self._vehicle) self._camera_right = self._world.spawn_actor( cam_blueprint, CAMERA_RIGHT_TRANSFORM, attach_to=self._vehicle) self._camera_center.listen(CallBack('CameraMiddle', self)) self._camera_left.listen(CallBack('CameraLeft', self)) self._camera_right.listen(CallBack('CameraRight', self)) # spectator server camera self._spectator = self._world.get_spectator() self._skiped_frames = 0 self._stucked_counter = 0 self._start_time = time.time() def get_recording(self): if self._autopilot: # debug: 0 for debugging if self._skiped_frames >= 20: return True else: self._skiped_frames += 1 return False else: ''' if (self.joystick.get_button(8)): self._recording = True if (self.joystick.get_button(9)): self._recording = False ''' if (self.joystick.get_button(6)): self._recording = True print("start recording!!!!!!!!!!!!!!!!!!!!!!!!1") if (self.joystick.get_button(7)): self._recording = False print("end recording!!!!!!!!!!!!!!!!!!!!!!!!1") return self._recording def initialize_map(self, city_name): self.city_name_demo = city_name if city_name == "RFS_MAP": path = get_current_folder() + "/maps_demo_area/rfs_demo_area.png" im = cv2.imread(path) im = im[:, :, :3] im = im[:, :, ::-1] self.demo_area_map = im else: print("do nothing since not a city with demo area") #raise ValueError("wrong city name: " + city_name) def loc_to_pix_rfs_sim(self, loc): u = 3.6090651558073654 * loc[1] + 2500.541076487252 v = -3.6103367739019054 * loc[0] + 2501.862578166202 return [int(v), int(u)] def in_valid_area(self, x, y): if self.city_name_demo == "RFS_MAP": pos = self.loc_to_pix_rfs_sim([x, y]) locality = 50 # 100 pixels local_area = self.demo_area_map[pos[0] - locality:pos[0] + locality, pos[1] - locality:pos[1] + locality, 0] > 0 valid = np.sum(local_area) > 0 if not valid: print( "detect the vehicle is not in the valid demonstrated area") return valid else: return True def get_reset(self): if self._autopilot: if __CARLA_VERSION__ == '0.8.X': # increase the stuck detector if conditions satisfy if self._latest_measurements.player_measurements.forward_speed < 0.1: self._stucked_counter += 1 else: self._stucked_counter = 0 # if within auto pilot, reset if long enough or has collisions if time.time() - self._start_time > self._reset_period \ or self._latest_measurements.player_measurements.collision_vehicles > 0.0 \ or self._latest_measurements.player_measurements.collision_pedestrians > 0.0 \ or self._latest_measurements.player_measurements.collision_other > 0.0 \ or (self._latest_measurements.player_measurements.intersection_otherlane > 0.0 and self._latest_measurements.player_measurements.autopilot_control.steer < -0.99) \ or self._stucked_counter > 250: if self._stucked_counter > 250: reset_because_stuck = True else: reset_because_stuck = False # TODO: commenting out this for debugging issue self._reset() if reset_because_stuck: print("resetting because getting stucked.....") return True else: # TODO: implement the collision detection algorithm, based on the new API if self.last_estimated_speed < 0.1: self._stucked_counter += 1 else: self._stucked_counter = 0 if time.time() - self._start_time > self._reset_period \ or self._last_collided \ or self._stucked_counter > 250 \ or not self.in_valid_area(self._latest_measurements.player_measurements.transform.location.x, self._latest_measurements.player_measurements.transform.location.y): #or np.abs(self._vehicle.get_vehicle_control().steer) > 0.95: #or np.abs(self._vehicle.get_vehicle_control().brake) > 1: # TODO intersection other lane is not available, so omit from the condition right now if self._stucked_counter > 250: reset_because_stuck = True else: reset_because_stuck = False if self._last_collided: print("reset becuase collision") self._reset() if reset_because_stuck: print("resetting because getting stucked.....") return True else: pass return False def get_waypoints(self): # TODO: waiting for German Ros to expose the waypoints wp1 = [1.0, 1.0] wp2 = [2.0, 2.0] return [wp1, wp2] def action_joystick(self): # joystick steering_axis = self.joystick.get_axis(0) acc_axis = self.joystick.get_axis(2) brake_axis = self.joystick.get_axis(5) # print("axis 0 %f, axis 2 %f, axis 3 %f" % (steering_axis, acc_axis, brake_axis)) if (self.joystick.get_button(3)): self._rear = True if (self.joystick.get_button(2)): self._rear = False control = VehicleControl() control.steer = steering_axis control.throttle = (acc_axis + 1) / 2.0 control.brake = (brake_axis + 1) / 2.0 if control.brake < 0.001: control.brake = 0.0 control.hand_brake = 0 control.reverse = self._rear control.steer -= 0.0822 #print("steer %f, throttle %f, brake %f" % (control.steer, control.throttle, control.brake)) pygame.event.pump() return control def action_steering_wheel(self, jsInputs, jsButtons): control = VehicleControl() # Custom function to map range of inputs [1, -1] to outputs [0, 1] i.e 1 from inputs means nothing is pressed # For the steering, it seems fine as it is K1 = 1.0 # 0.55 steerCmd = K1 * math.tan(1.1 * jsInputs[self._steer_idx]) K2 = 1.6 # 1.6 throttleCmd = K2 + ( 2.05 * math.log10(-0.7 * jsInputs[self._throttle_idx] + 1.4) - 1.2) / 0.92 if throttleCmd <= 0: throttleCmd = 0 elif throttleCmd > 1: throttleCmd = 1 brakeCmd = 1.6 + (2.05 * math.log10(-0.7 * jsInputs[self._brake_idx] + 1.4) - 1.2) / 0.92 if brakeCmd <= 0: brakeCmd = 0 elif brakeCmd > 1: brakeCmd = 1 #print("Steer Cmd, ", steerCmd, "Brake Cmd", brakeCmd, "ThrottleCmd", throttleCmd) control.steer = steerCmd control.brake = brakeCmd control.throttle = throttleCmd toggle = jsButtons[self._reverse_idx] if toggle == 1: self._is_on_reverse += 1 if self._is_on_reverse % 2 == 0: control.reverse = False if self._is_on_reverse > 1: self._is_on_reverse = True if self._is_on_reverse: control.reverse = True control.hand_brake = False # jsButtons[self.handbrake_idx] return control def compute_action(self, sensor, speed): if not self._autopilot: # get pygame input for event in pygame.event.get(): # Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN # JOYBUTTONUP JOYHATMOTION if event.type == pygame.JOYBUTTONDOWN: if event.__dict__['button'] == 0: self._current_command = 2.0 if event.__dict__['button'] == 1: self._current_command = 3.0 if event.__dict__['button'] == 2: self._current_command = 4.0 if event.__dict__['button'] == 3: self._current_command = 5.0 if event.__dict__['button'] == 23: self._current_command = 0.0 if event.__dict__['button'] == 4: self._reset() return VehicleControl() if event.type == pygame.JOYBUTTONUP: self._current_command = 2.0 #pygame.event.pump() numAxes = self.joystick.get_numaxes() jsInputs = [ float(self.joystick.get_axis(i)) for i in range(numAxes) ] # print (jsInputs) jsButtons = [ float(self.joystick.get_button(i)) for i in range(self.joystick.get_numbuttons()) ] if self._steering_wheel_flag: control = self.action_steering_wheel(jsInputs, jsButtons) else: control = self.action_joystick() else: if __CARLA_VERSION__ == '0.8.X': # This relies on the calling of get_sensor_data, otherwise self._latest_measurements are not filled control = self._latest_measurements.player_measurements.autopilot_control print('[Throttle = {}] [Steering = {}] [Brake = {}]'.format( control.throttle, control.steer, control.brake)) else: self._world.tick() if self._world.wait_for_tick(10.0): control, self._current_command = self._agent_autopilot.run_step( ) print('[Throttle = {}] [Steering = {}] [Brake = {}]'.format( control.throttle, control.steer, control.brake)) return control def estimate_speed(self): vel = self._vehicle.get_velocity() speed = m.sqrt(vel.x**2 + vel.y**2 + vel.z**2) # speed in m/s return speed def get_sensor_data(self, goal_pos=None, goal_ori=None): if __CARLA_VERSION__ == '0.8.X': # return the latest measurement and the next direction measurements, sensor_data = self.carla.read_data() self._latest_measurements = measurements if self.use_planner: player_data = measurements.player_measurements pos = [ player_data.transform.location.x, player_data.transform.location.y, 0.22 ] ori = [ player_data.transform.orientation.x, player_data.transform.orientation.y, player_data.transform.orientation.z ] if sldist([ player_data.transform.location.x, player_data.transform.location.y ], [ self.positions[self.episode_config[1]].location.x, self.positions[self.episode_config[1]].location.y ]) < self._goal_reaching_threshold: self._reset() direction = self.planner.get_next_command( pos, ori, [ self.positions[self.episode_config[1]].location.x, self.positions[self.episode_config[1]].location.y, 0.22 ], (1, 0, 0)) else: direction = 2.0 else: self.last_timestamp.elapsed_seconds += 0.2 #self.last_timestamp = self.carla.get_world().wait_for_tick(30.0) #print(timestamp.delta_seconds, "delta seconds") #while self.update_once == False: # time.sleep(0.01) self.last_estimated_speed = self.estimate_speed() data_buffer_lock.acquire() sensor_data = copy.deepcopy(self._data_buffers) data_buffer_lock.release() #self.update_once = False collision_lock.acquire() colllision_event = self._collision_events self._last_collided = len(colllision_event) > 0 self._collision_events = [] collision_lock.release() if len(colllision_event) > 0: print(colllision_event) # TODO: make sure those events are actually valid if 'Static' in colllision_event: collision_other = 1.0 else: collision_other = 0.0 if "Vehicles" in colllision_event: collision_vehicles = 1.0 else: collision_vehicles = 0.0 if "Pedestrians" in colllision_event: collision_pedestrians = 1.0 else: collision_pedestrians = 0.0 #current_ms_offset = int(math.ceil((datetime.now() - self._episode_t0).total_seconds() * 1000)) # TODO: get a gametime stamp, instead of os timestamp #current_ms_offset = int(self.carla.get_timestamp().elapsed_seconds * 1000) #print(current_ms_offset, "ms offset") current_ms_offset = self.last_timestamp.elapsed_seconds * 1000 second_level = namedtuple('second_level', [ 'forward_speed', 'transform', 'collision_other', 'collision_pedestrians', 'collision_vehicles' ]) transform = namedtuple('transform', ['location', 'orientation']) loc = namedtuple('loc', ['x', 'y']) ori = namedtuple('ori', ['x', 'y', 'z']) Meas = namedtuple('Meas', ['player_measurements', 'game_timestamp']) v_transform = self._vehicle.get_transform() measurements = Meas( second_level( self.last_estimated_speed, transform( loc(v_transform.location.x, v_transform.location.y), ori(v_transform.rotation.pitch, v_transform.rotation.roll, v_transform.rotation.yaw)), collision_other, collision_pedestrians, collision_vehicles), current_ms_offset) direction = self._current_command self._latest_measurements = measurements #print('[Speed = {} Km/h] [Direction = {}]'.format(measurements.player_measurements.forward_speed, direction)) #print(">>>>> planner output direction: ", direction) return measurements, sensor_data, direction def act(self, control): if __CARLA_VERSION__ == '0.8.X': self.carla.send_control(control) else: self._vehicle.apply_control(control)
class CarlaEnv(gym.Env): def __init__(self, config=ENV_CONFIG): self.config = config if config["discrete_actions"]: self.action_space = Discrete(10) else: self.action_space = Box(-1.0, 1.0, shape=(3, )) if config["use_depth_camera"]: self.observation_space = Box(-1.0, 1.0, shape=(config["x_res"], config["y_res"], 1)) else: self.observation_space = Box(0.0, 255.0, shape=(config["x_res"], config["y_res"], 3)) 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.prev_measurement = 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, "/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 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: os.killpg(os.getpgid(self.server_process.pid), signal.SIGKILL) 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.prev_measurement = 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() settings.set(SynchronousMode=True, SendNonPlayerAgentsInfo=True, NumberOfVehicles=self.config["num_vehicles"], NumberOfPedestrians=self.config["num_pedestrians"], WeatherId=random.choice(self.config["weather"])) settings.randomize_seeds() if self.config["use_depth_camera"]: camera = Camera("CameraDepth", PostProcessing="Depth") camera.set_image_size(self.config["x_res"], self.config["y_res"]) camera.set_position(30, 0, 130) settings.add_sensor(camera) else: camera = Camera("CameraRGB") camera.set_image_size(self.config["x_res"], self.config["y_res"]) camera.set_position(30, 0, 130) settings.add_sensor(camera) scene = self.client.load_settings(settings) # Choose one player start at random. number_of_player_starts = len(scene.player_start_spots) if self.config["random_starting_location"]: player_start = random.randint(0, max(0, number_of_player_starts - 1)) else: player_start = 0 # 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(player_start) image, measurements = self._read_observation() self.prev_measurement = measurements return self.preprocess_image(image) 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 np.zeros(self.observation_space.shape), 0.0, True, {} def _step(self, action): if self.config["discrete_actions"]: action = int(action) assert action in range(10) if action == 9: brake = 1.0 steer = 0.0 throttle = 0.0 reverse = False else: brake = 0.0 if action >= 6: steer = -1.0 elif action >= 3: steer = 1.0 else: steer = 0.0 action %= 3 if action == 0: throttle = 0.0 reverse = False elif action == 1: throttle = 1.0 reverse = False elif action == 2: throttle = 1.0 reverse = True else: assert len(action) == 3, "Invalid action {}".format(action) steer = action[0] throttle = min(1.0, abs(action[1])) brake = max(0.0, min(1.0, action[2])) reverse = action[1] < 0.0 print("steer", steer, "throttle", throttle, "brake", brake, "reverse", reverse) self.client.send_control(steer=steer, throttle=throttle, brake=brake, hand_brake=False, reverse=reverse) image, measurements = self._read_observation() reward, done = compute_reward(self.config, self.prev_measurement, measurements) self.prev_measurement = measurements if self.num_steps > self.config["max_steps"]: done = True self.num_steps += 1 info = {} image = self.preprocess_image(image) return image, reward, done, info def preprocess_image(self, image): if self.config["use_depth_camera"]: data = (image.data - 0.5) * 2 return data.reshape(self.config["x_res"], self.config["y_res"], 1) else: return image.data.reshape(self.config["x_res"], self.config["y_res"], 3) 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. 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 if IMAGE_OUT_PATH: for name, image in sensor_data.items(): scipy.misc.imsave( "{}/{}-{}.jpg".format(IMAGE_OUT_PATH, name, self.num_steps), image.data) assert observation is not None, sensor_data return observation, measurements
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) self.action_space = Discrete(len(DISCRETE_ACTIONS)) # RGB Camera self.frame_shape = (config["y_res"], config["x_res"]) image_space = Box( 0, 1, shape=( config["y_res"], config["x_res"], FRAME_DEPTH * config["framestack"]), dtype=np.float32) self.observation_space = image_space # 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.last_full_obs = None self.framestack = [None] * config["framestack"] self.framestack_index = 0 self.running_restarts = 0 self.on_step = None self.on_next = None self.photo_index = 1 self.episode_index = 0 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( [self.config["server_binary"], self.config["server_map"], "-windowed", "-ResX={}".format(self.config["render_x_res"]), "-ResY={}".format(self.config["render_y_res"]), "-fps={}".format(self.config["fps"]), "-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): if self.on_next is not None: self.on_next() error = None self.running_restarts += 1 for _ in range(RETRIES_ON_ERROR): try: # Force a full reset of Carla after some number of restarts if self.running_restarts > self.config["server_restart_interval"]: print("Shutting down carla server...") self.running_restarts = 0 self.clear_server_state() # If server down, initialize if not self.server_process: self.init_server() # Run episode reset return self._reset() except Exception as e: print("Error during reset: {}".format(traceback.format_exc())) self.clear_server_state() error = e time.sleep(5) raise error def _build_camera(self, name, post): camera = Camera(name, PostProcessing=post) camera.set_image_size( self.config["render_x_res"], self.config["render_y_res"]) camera.set_position(x=2.4, y=0, z=0.8) camera.set_rotation(pitch=-40, roll=0, yaw=0) # camera.FOV = 100 return camera def _reset(self): self.num_steps = 0 self.total_reward = 0 self.episode_index += 1 self.photo_index = 1 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=False, NumberOfVehicles=self.scenario["num_vehicles"], NumberOfPedestrians=self.scenario["num_pedestrians"], WeatherId=self.weather, QualityLevel=self.config["quality"]) settings.randomize_seeds() # Add the cameras if self.config["save_images_rgb"]: settings.add_sensor(self._build_camera(name="CameraRGB", post="SceneFinal")) settings.add_sensor(self._build_camera(name="CameraDepth", post="Depth")) settings.add_sensor(self._build_camera(name="CameraClass", post="SemanticSegmentation")) # 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() py_measurements["control"] = { "throttle_brake": 0, "steer": 0, "throttle": 0, "brake": 0, "reverse": False, "hand_brake": False, } self.prev_measurement = py_measurements return self.encode_obs(self.preprocess_image(image), py_measurements) def encode_obs(self, obs, py_measurements): new_obs = obs num_frames = int(self.config["framestack"]) if num_frames > 1: # Spread out to number of frames frame_array = [obs] * num_frames for frame_index in range(1, num_frames): index = (self.framestack_index - frame_index) % num_frames if self.framestack[index] is not None: frame_array[frame_index] = self.framestack[index] # Concatenate into a single np array new_obs = np.concatenate(frame_array, axis=2) # Store frame self.framestack[self.framestack_index] = obs self.framestack_index = (self.framestack_index + 1) % num_frames self.last_obs = obs # Return return new_obs def step(self, action): try: obs = self._step(action) self.last_full_obs = obs return obs except Exception: print( "Error during step, terminating episode early", traceback.format_exc()) self.clear_server_state() return (self.last_full_obs, 0.0, True, {}) def _step(self, action): 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"] = { "throttle_brake": action[0], "steer": steer, "throttle": throttle, "brake": brake, "reverse": reverse, "hand_brake": hand_brake, } # Done? finished = self.num_steps > self.scenario["max_steps"] # py_measurements["next_command"] == "REACH_GOAL" failed = TERM.compute_termination(self, py_measurements, self.prev_measurement) done = finished or failed py_measurements["finished"] = finished py_measurements["failed"] = failed py_measurements["done"] = done # Determine 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 self.prev_measurement = py_measurements # Callback if self.on_step is not None: self.on_step(py_measurements) # Write out measurements to file if self.config["carla_out_path"]: # Ensure measurements dir exists measurements_dir = os.path.join(self.config["carla_out_path"], self.config["measurements_subdir"]) if not os.path.exists(measurements_dir): os.mkdir(measurements_dir) if not self.measurements_file: self.measurements_file = open( os.path.join( measurements_dir, "m_{}.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(camera_name="RGB") self.images_to_video(camera_name="Depth") self.images_to_video(camera_name="Class") 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): return image def _fuse_observations(self, depth, clazz, speed): base_shape = (self.config["render_y_res"], self.config["render_x_res"]) new_shape = (self.config["y_res"], self.config["x_res"]) # Reduce class clazz = reduce_classifications(clazz) # Do we need to resize? if base_shape[0] is not new_shape[0] and base_shape[1] is not new_shape[1]: depth_reshape = depth.reshape(*depth.shape) depth = cv2.resize(depth_reshape, (new_shape[1], new_shape[0])) clazz = resize_classifications(clazz, new_shape) # Fuse with depth! obs = fuse_with_depth(clazz, depth, extra_layers=1) # Fuse with speed! obs[:, :, KEEP_CLASSIFICATIONS] = speed return obs, clazz def _read_observation(self): # Read the data produced by the server this frame. measurements, sensor_data = self.client.read_data() cur = measurements.player_measurements # Print some of the measurements. if self.config["verbose"]: print_measurements(measurements) # Fuse the observation data to create a single observation observation, clazzes = self._fuse_observations( sensor_data['CameraDepth'].data, sensor_data['CameraClass'].data, cur.forward_speed) 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, "applied_penalty": False, "total_reward": self.total_reward, } if self.config["carla_out_path"] \ and self.num_steps % self.config["save_image_frequency"] == 0\ and self.num_steps > 15: self.take_photo( sensor_data=sensor_data, class_data=clazzes, fused_data=observation ) assert observation is not None, sensor_data return observation, py_measurements def images_dir(self): dir = os.path.join(self.config["carla_out_path"], "images") if not os.path.exists(dir): os.mkdir(dir) return dir def episode_dir(self): episode_dir = os.path.join(self.images_dir(), "episode_" + str(self.episode_index)) if not os.path.exists(episode_dir): os.mkdir(episode_dir) return episode_dir def take_photo(self, sensor_data, class_data, fused_data): # Get the proper locations\ episode_dir = self.episode_dir() photo_index = self.photo_index name_prefix = "episode_{:>04}_step_{:>04}".format(self.episode_index, photo_index) self.photo_index += 1 # Save the image if self.config["save_images_rgb"]: rgb_image = sensor_data["CameraRGB"] image_path = os.path.join(episode_dir, name_prefix + "_rgb.png") scipy.misc.imsave(image_path, rgb_image.data) # Save the classes if self.config["save_images_class"]: classes_path = os.path.join(episode_dir, name_prefix + "_class.png") scipy.misc.imsave(classes_path, class_data.data) # Save the class images if self.config["save_images_fusion"]: fused_images = fused_to_rgb(fused_data) fused_path = os.path.join(episode_dir, name_prefix + "_fused.png") scipy.misc.imsave(fused_path, fused_images.data) def images_to_video(self, camera_name): # Video directory videos_dir = os.path.join(self.config["carla_out_path"], "Videos" + camera_name) if not os.path.exists(videos_dir): os.makedirs(videos_dir) # Build command video_fps = self.config["fps"] / self.config["saveimage_frequency"] ffmpeg_cmd = ( "ffmpeg -loglevel -8 -r 10 -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(self.config["carla_out_path"], "Camera" + camera_name, self.episode_id)) # Execute command print("Executing ffmpeg command: ", ffmpeg_cmd) try: subprocess.call(ffmpeg_cmd, shell=True, timeout=60) except Exception as ex: print("FFMPEG EXPIRED") print(ex)
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 CarlaEnvironmentWrapper(EnvironmentWrapper): def __init__(self, num_speedup_steps=10, require_explicit_reset=True, is_render_enabled=False, automatic_render=False, early_termination_enabled=False, run_offscreen=False, cameras=['SceneFinal'], save_screens=False, carla_settings=None, carla_server_settings=None, location_indices=(9, 16)): EnvironmentWrapper.__init__(self, is_render_enabled, save_screens) self.automatic_render = automatic_render self.episode_max_time = 100000 # miliseconds for each episode self.allow_braking = True self.log_path = "carla_logs.txt" self.verbose = True self.observation = None self.num_speedup_steps = num_speedup_steps self.counter = 0 self.rgb_camera_name = 'CameraRGB' self.rgb_camera = 'SceneFinal' in cameras self.is_game_ready_for_input = False self.run_offscreen = run_offscreen self.kill_when_connection_lost = True # server configuration self.carla_server_settings = carla_server_settings self.port = get_open_port() self.host = 'localhost' # Why town2: https://github.com/carla-simulator/carla/issues/10#issuecomment-342483829 self.level = 'town2' self.map = CarlaLevel().get(self.level) # client configuration if type(carla_settings) == str: # load settings from file with open(carla_settings, 'r') as fp: self.settings = fp.read() elif type(carla_settings) == CarlaSettings: self.settings = carla_settings carla_settings = None elif carla_settings == None: raise Exception( "Please load a CarlaSettings object or provide a path to a settings file.") self.car_speed = 0. self.max_speed = 10. #m/s # Will be true only when setup_client_and_server() is called, either explicitly, or by reset() self.is_game_setup = False # measurements self.autopilot = None self.kill_if_unmoved_for_n_steps = 50*4 self.unmoved_steps = 0.0 self.early_termination_enabled = early_termination_enabled if self.early_termination_enabled: self.max_neg_steps = 100*4 self.cur_neg_steps = 0 self.early_termination_punishment = 20.0 # env initialization if not require_explicit_reset: self.reset(True) # camera-view renders if self.automatic_render: self.init_renderer() if self.save_screens: create_dir(self.images_path) self.rgb_img_path = self.images_path+"/rgb/" # locations indices self.start_loc_idx = location_indices[0] self.end_loc_idx = location_indices[1] self.end_loc = None self.last_distance = 0 self.curr_distance = 0 self.end_loc_measurement = None 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 close_client_and_server(self): self._close_server() print("\t 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'} pass with open(self.log_path, "wb") as out: cmd = [path.join(environ.get('CARLA_ROOT'), 'CarlaUE4.sh'), self.map, "-benchmark", "-carla-server", "-fps=10", "-world-port={}".format( self.port), "-windowed -ResX={} -ResY={}".format( carla_config.server_width, carla_config.server_height), "-carla-no-hud"] if self.carla_server_settings: cmd.append("-carla-settings={}".format(self.carla_server_settings)) 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.75 or \ if immediate_reward < 0 : # or \ # (self.control.throttle == 0.0 and player_measurements.forward_speed < 0.1 and self.control.brake != 0.0): self.cur_neg_steps += 1 # print("updated neg steps:{}".format(self.cur_neg_steps)) early_done = (self.cur_neg_steps > self.max_neg_steps) if early_done: print("\t Early terminate. neg steps:{}, unmoved:{}".format(self.cur_neg_steps, self.unmoved_steps)) return early_done, self.early_termination_punishment else: self.cur_neg_steps = self.cur_neg_steps - 1 if self.cur_neg_steps > 0 else 0 # decay return False, 0.0 def _update_state(self): # get measurements and observations try: measurements, sensor_data = self.game.read_data() except: # Connection between cli and server lost; reconnect if self.kill_when_connection_lost: raise print("\t 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] self.last_distance = self.curr_distance self.curr_distance = self.cal_distance( measurements.player_measurements.transform.location, self.end_loc) 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.25 and self.car_speed < 0.7 and self.is_game_ready_for_input: self.unmoved_steps += 1 # print("updated unmoved(carla bug) steps:{}".format(self.unmoved_steps), self.control.throttle, self.car_speed, self.is_game_ready_for_input) if self.unmoved_steps > self.kill_if_unmoved_for_n_steps: is_collision = True print("\t Car stuck somewhere. neg steps:{}, unmoved:{}".format(self.cur_neg_steps, self.unmoved_steps)) # elif self.unmoved_steps > 0: # # decay slowly, since it may be stuck and not accelerate few times # self.unmoved_steps -= 0.50 if is_collision: print("\t Collision occured.") # Reward Shaping: speed_reward = self.car_speed if speed_reward > 30.: speed_reward = 30. self.reward = speed_reward * 5 \ - (measurements.player_measurements.intersection_otherlane * 1.5) \ - (measurements.player_measurements.intersection_offroad * 1.5) \ - is_collision * 10 \ - np.abs(self.control.steer) * 1.5 \ + (self.last_distance - self.curr_distance)*10 # Scale down the reward by a factor # self.reward /= 10 # print("reward: {} = {} - ({} * 1.5) - ({} * 1.5) - {} * 10 - np.abs({}) * 10 + ({} - {})*10".format(self.reward, # speed_reward, # measurements.player_measurements.intersection_otherlane, # measurements.player_measurements.intersection_offroad, # is_collision, # self.control.steer, # self.last_distance, # self.curr_distance)) 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 = np.hstack(self.location + self.end_loc_measurement + [measurements.player_measurements.acceleration.x, measurements.player_measurements.acceleration.y, measurements.player_measurements.acceleration.z, measurements.player_measurements.forward_speed]) if self.rgb_camera: img_data = sensor_data[self.rgb_camera_name].data/255. self.observation = [self.observation, img_data] # self.counter +=1 # self.observation = np.array( # [np.array([self.counter - 1]), np.array([self.counter, self.counter + 1])]) # print(self.observation.shape) self.autopilot = measurements.player_measurements.autopilot_control if (measurements.game_timestamp >= self.episode_max_time) or is_collision: # print('EPISODE IS DONE. GameTime: {}, Collision: {}'.format(str(measurements.game_timestamp), # str(is_collision))) self.done = True def _take_action(self, action): if not self.is_game_setup: print("\t Reset the environment by reset() before calling step()") sys.exit(1) # assert len(actions) == 2, "Send actions in the format [steer, accelaration]" self.control = VehicleControl() 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)) self.control.hand_brake = False self.control.reverse = False # prevent braking if not self.allow_braking or self.control.brake < 0.1 or self.control.throttle > self.control.brake: self.control.brake = 0 if self.car_speed > self.max_speed and self.control.brake == 0: self.control.throttle = 0.0 controls_sent = False while not controls_sent: try: self.game.send_control(self.control) controls_sent = True # print("controls sent!") except Exception: traceback.print_exc() if self.kill_when_connection_lost: print("\t 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 def _restart_environment_episode(self, force_environment_reset=True, settings=None): 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: # get settings. Only needed if we want random rollouts if type(settings) == str: # load settings from file with open(settings, 'r') as fp: self.settings = fp.read() self.scene = self.game.load_settings(self.settings) elif type(settings) == CarlaSettings: self.settings = settings self.scene = self.game.load_settings(self.settings) elif settings == None: pass # already set during initialization # self.iterator_start_positions += 1 # if self.iterator_start_positions >= self.num_pos: # self.iterator_start_positions = 0 try: self.game.start_episode(self.start_loc_idx) except: self.game.connect() self.game.start_episode(self.start_loc_idx) self.unmoved_steps = 0.0 self.cur_neg_steps = 0 self.car_speed = 0 # start the game with some initial speed observation = None for i in range(self.num_speedup_steps): observation, reward, done, _ = self.step([0, 0.25]) self.observation = observation self.is_game_ready_for_input = True return observation def init_renderer(self): self.num_cameras = 0 if self.rgb_camera: self.num_cameras += 1 self.renderer.create_screen( carla_config.render_width, carla_config.render_height*self.num_cameras) def get_rendered_image(self): temp = [] if self.rgb_camera: temp.append(self.observation['rgb_image']) 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']) def cal_distance(self, start, end): return ((end.x - start.x)**2 + (end.y - start.y)**2 + (end.z - start.z)**2)**0.5
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 gym_carla_car_following: def __init__(self, host="127.0.0.1", port=2000, timeout=15): # Steer, Throttle/Brake self.action_space = Box(low=np.array([-1.0, -1.0]), high=np.array([1.0, 1.0])) self.speed_scale = 50 # km/h self.relative_x_scale = 4.0 # m self.relative_y_scale = 40.0 # m self.relative_angle_scale = 180 # degree self._history_path_num = 80 low = np.zeros([self._history_path_num * 2 + 5]) high = np.ones([self._history_path_num * 2 + 5]) # Only shape of low/high array matters self.observation_space = Box(low=low, high=high) self._host = host self._port = port self._timeout = timeout 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 step(self, action): steering = action[0] # filter small value if action[1] >= 0.01: acceleration = action[1] brake = 0 elif action[1] <= -0.01: acceleration = 0 brake = -action[1] else: acceleration = 0 brake = 0 # one command for every 2 frame for i in range(2): self._client.send_control(steer=steering, throttle=acceleration, brake=brake, hand_brake=False, reverse=False) observation = self._observe() info = {} done = self._calculate_done(observation[:5]) reward = self._calculate_reward(observation[:5], done) self._his_steer = self._cur_steer self._cur_steer = steering return observation, reward, done, info def stop(self): self._client.disconnect() # Wait for server to be connectable again import time time.sleep(8) def print_state(self, step, state, action, reward, epsilon=0): content = "Step %d : epsilong=%f \n" \ "\tState : self_speed=%f(km/h), npc_speed=%f(km/h), rel_angle=%f(degree)\n," \ "\t\trel_x=%f(m), rel_y=%f(m), last_rel_x=%f(m), last_rel_y=%f(m)\n" \ "\tAction : steer=%f, throttle/brake=%f, reward=%f\n" \ % (step, epsilon, state[0] * self.speed_scale, state[1] * self.speed_scale, state[2] * self.relative_angle_scale, state[3] * self.relative_x_scale, state[4] * self.relative_y_scale, state[-2] * self.relative_x_scale, state[-1] * self.relative_y_scale, action[0], action[1], reward) print_over_same_line(content) def _observe(self): #################################### current state ####################################### measurements, _ = self._client.read_data() self_car = measurements.player_measurements self_speed = self_car.forward_speed npc_car = measurements.non_player_agents[-1].vehicle npc_speed = npc_car.forward_speed # normalization self_speed /= self.speed_scale if self_speed < 0.0: self_speed = 0.0 npc_speed /= self.speed_scale if npc_speed < 0.0: npc_speed = 0.0 # Coordination in this simulator is like this : # ^ + # I # I # <-------0-------- # + I - # I - # cm -> m npc_pos_x = -npc_car.transform.location.x / 100.0 npc_pos_y = npc_car.transform.location.y / 100.0 npc_orientation_x = -npc_car.transform.orientation.x npc_orientation_y = npc_car.transform.orientation.y pos_x = -self_car.transform.location.x / 100.0 pos_y = self_car.transform.location.y / 100.0 orientation_x = -self_car.transform.orientation.x orientation_y = self_car.transform.orientation.y [npc_relative_x, npc_relative_y, npc_relative_angle] = \ self._transform_coordination(pos_x, pos_y, orientation_x, orientation_y, npc_pos_x, npc_pos_y, npc_orientation_x, npc_orientation_y) # normalization npc_relative_x /= self.relative_x_scale npc_relative_y /= self.relative_y_scale npc_relative_angle /= self.relative_angle_scale npc_relative_angle = np.clip(npc_relative_angle, -1.0, 1.0) observation = np.zeros([self._history_path_num * 2 + 5]) observation[0] = self_speed observation[1] = npc_speed observation[2] = npc_relative_angle observation[3] = npc_relative_x observation[4] = npc_relative_y #################################### history state ####################################### state = np.hstack( (npc_pos_x, npc_pos_y, npc_orientation_x, npc_orientation_y)) if self._reset == True: self._history_path[:] = state self._reset = False for i in range(self._history_path_num): observation[5 + (self._history_path_num - 1 - i) * 2] = npc_relative_x observation[5 + (self._history_path_num - 1 - i) * 2 + 1] = npc_relative_y else: last_his_num = 0 for i in range(self._history_path_num): npc_pos_x = self._history_path[i, 0] npc_pos_y = self._history_path[i, 1] npc_orientation_x = self._history_path[i, 2] npc_orientation_y = self._history_path[i, 3] [npc_relative_x, npc_relative_y, npc_relative_angle] = \ self._transform_coordination(pos_x, pos_y, orientation_x, orientation_y, npc_pos_x, npc_pos_y, npc_orientation_x, npc_orientation_y) # normalization npc_relative_x /= self.relative_x_scale npc_relative_y /= self.relative_y_scale npc_relative_angle /= self.relative_angle_scale npc_relative_angle = np.clip(npc_relative_angle, -1.0, 1.0) if npc_relative_y > 0.025: observation[5 + i * 2] = npc_relative_x observation[5 + i * 2 + 1] = npc_relative_y else: last_his_num = i # print("********************************************************") # print(observation[5 + i * 2 - 2], observation[5 + i * 2 + 1 - 2]) # print("********************************************************") break if last_his_num > 0: # print("**********************************************") # print(last_his_num) # print("*************************************************") for i in range(last_his_num, self._history_path_num): observation[5 + i * 2] = observation[5 + (i - 1) * 2] observation[5 + i * 2 + 1] = observation[5 + (i - 1) * 2 + 1] self._history_path = np.roll(self._history_path, 1, axis=0) self._history_path[0] = state return observation def _calculate_reward(self, observation, done): speed = observation[0] npc_speed = observation[1] rel_angle = observation[2] rel_x = observation[3] rel_y = observation[4] # reward = 0 if done == True: reward = -1000.0 else: # How to calculate reward ? reward = ((rel_y - 0.5)**2) * 20.0 \ - abs(rel_x) * 20.0 + \ - abs(rel_angle - 1.0 / 2) * 20 \ - abs(speed - npc_speed) * 20.0 \ - abs(self._cur_steer - self._his_steer) * 20 return reward def _calculate_done(self, observation): speed = observation[0] npc_speed = observation[1] rel_angle = observation[2] rel_x = observation[3] rel_y = observation[4] done = False # if (abs(rel_x) >= 2.5) or \ # (rel_y >= 2.5) or (rel_y <= 0.15) or \ # (rel_angle < 1.0 / 8) or (rel_angle > 7.0 / 8): # done = True distance = math.sqrt((rel_x * self.relative_x_scale)**2 + \ (rel_y * self.relative_y_scale)**2) if (distance > 80) or \ (distance < 8) or \ ((rel_angle < -0.1) and (rel_angle > -0.9)): done = True elif self._calculate_stuck() == True: # judge if stuck done = True return done def _calculate_stuck(self): return False def _normalize_angle(self, angle): while angle > 180: angle -= 360 while angle < -180: angle += 360 return angle def _transform_coordination(self, pos_x, pos_y, orientation_x, orientation_y, npc_pos_x, npc_pos_y, npc_orientation_x, npc_orientation_y): absolute_angle = math.atan2(orientation_y, orientation_x) * 180 / 3.1415926 npc_absolute_angle = math.atan2(npc_orientation_y, npc_orientation_x) * 180 / 3.1415926 # print("self %f %f with %f %f %f" % (pos_x, pos_y, orientation_x, orientation_y, absolute_angle)) # print("npc %f %f with %f %f %f" % (npc_pos_x, npc_pos_y, npc_orientation_x, npc_orientation_y, npc_absolute_angle)) delta_pos_x = npc_pos_x - pos_x delta_pos_y = npc_pos_y - pos_y npc_absolute_pos_angle = math.atan2(delta_pos_y, delta_pos_x) * 180 / 3.1415926 # print("delta_x = %f, delta_y = %f " % (delta_pos_x, delta_pos_y)) npc_relative_angle = (90 - absolute_angle) + npc_absolute_angle npc_relative_pos_angle = (90 - absolute_angle) + npc_absolute_pos_angle distance = math.sqrt(delta_pos_x**2 + delta_pos_y**2) npc_relative_pos_x = distance * math.cos( npc_relative_pos_angle * 3.1415926 / 180) npc_relative_pos_y = distance * math.sin( npc_relative_pos_angle * 3.1415926 / 180) npc_relative_angle = self._normalize_angle(npc_relative_angle) # print("relative %f %f %f %f" % (npc_relative_pos_x, npc_relative_pos_y, npc_relative_angle, distance)) return [npc_relative_pos_x, npc_relative_pos_y, npc_relative_angle]
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 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, config: str, episode_max_time: int, allow_braking: bool, quality: CarlaEnvironmentParameters.Quality, cameras: List[CameraTypes], weather_id: List[int], experiment_path: str, **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 = self.env_id 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.camera_width = camera_width self.camera_height = camera_height # state space self.state_space = StateSpace({ "measurements": VectorObservationSpace( 4, measurements_names=["forward_speed", "x", "y", "z"]) }) for camera in self.cameras: self.state_space[camera.value] = ImageObservationSpace( shape=np.array([self.camera_height, self.camera_width, 3]), high=255) # setup server settings 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() 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 # action space self.action_space = BoxActionSpace(shape=2, low=np.array([-1, -1]), high=np.array([1, 1])) # 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 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 = 30 # measurements self.autopilot = None # 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_image_size(camera_width, camera_height) camera.set_position(0.2, 0, 1.3) camera.set_rotation(8, 0, 0) settings.add_sensor(camera) # add a left facing camera if CameraTypes.LEFT in cameras: camera = Camera(CameraTypes.LEFT.value) camera.set_image_size(camera_width, camera_height) camera.set_position(0.2, 0, 1.3) camera.set_rotation(8, -30, 0) settings.add_sensor(camera) # add a right facing camera if CameraTypes.RIGHT in cameras: camera = Camera(CameraTypes.RIGHT.value) camera.set_image_size(camera_width, camera_height) camera.set_position(0.2, 0, 1.3) camera.set_rotation(8, 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 _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, "-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.cameras: self.state[camera.value] = sensor_data[camera.value].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 # 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 self.state['measurements'] = self.measurements def _take_action(self, action): self.control = VehicleControl() 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 self.game.send_control(self.control) def _restart_environment_episode(self, force_environment_reset=False): 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) # start the game with some initial speed for i in range(self.num_speedup_steps): self._take_action([1.0, 0]) 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.value] for camera in self.cameras] 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
class CarlaEnvironmentWrapper(EnvironmentWrapper): def __init__(self, tuning_parameters): EnvironmentWrapper.__init__(self, tuning_parameters) self.tp = tuning_parameters # server configuration self.server_height = self.tp.env.server_height self.server_width = self.tp.env.server_width self.port = get_open_port() self.host = 'localhost' self.map = CarlaLevel().get(self.tp.env.level) # client configuration self.verbose = self.tp.env.verbose self.depth = self.tp.env.depth self.stereo = self.tp.env.stereo self.semantic_segmentation = self.tp.env.semantic_segmentation self.height = self.server_height * (1 + int(self.depth) + int(self.semantic_segmentation)) self.width = self.server_width * (1 + int(self.stereo)) self.size = (self.width, self.height) self.config = self.tp.env.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=1) self.settings.randomize_seeds() # add cameras camera = Camera('CameraRGB') camera.set_image_size(self.width, self.height) camera.set_position(200, 0, 140) camera.set_rotation(0, 0, 0) self.settings.add_sensor(camera) # 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() 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 # action space self.discrete_controls = False 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.5 self.gas_strength = 1.0 self.brake_strength = 0.5 self.actions = { 0: [0., 0.], 1: [0., -self.steering_strength], 2: [0., self.steering_strength], 3: [self.gas_strength, 0.], 4: [-self.brake_strength, 0], 5: [self.gas_strength, -self.steering_strength], 6: [self.gas_strength, 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 self.num_speedup_steps = 30 # measurements self.measurements_size = (1, ) self.autopilot = None # env initialization self.reset(True) # render if self.is_rendered: image = self.get_rendered_image() self.renderer.create_screen(image.shape[1], image.shape[0]) def _open_server(self): log_path = path.join(logger.experiments_path, "CARLA_LOG_{}.txt".format(self.port)) with open(log_path, "wb") as out: cmd = [ path.join(environ.get('CARLA_ROOT'), 'CarlaUE4.sh'), self.map, "-benchmark", "-carla-server", "-fps=10", "-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.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.state = { 'observation': sensor_data['CameraRGB'].data, 'measurements': [measurements.player_measurements.forward_speed], } 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.tp.env.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 type(action_idx) == int: action = self.actions[action_idx] else: action = action_idx self.last_action_idx = action self.control = VehicleControl() 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.tp.env.allow_braking: self.control.brake = 0 self.control.hand_brake = False self.control.reverse = False self.game.send_control(self.control) def _restart_environment_episode(self, force_environment_reset=False): 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) # start the game with some initial speed state = None for i in range(self.num_speedup_steps): state = self.step([1.0, 0])['state'] self.state = state return state
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 CarlaEnv(gym.Env): def __init__(self, config=ENV_CONFIG): self.config = config if config["discrete_actions"]: self.action_space = Discrete(10) else: self.action_space = Box(-1.0, 1.0, shape=(3, )) if config["use_depth_camera"]: self.observation_space = Box(-1.0, 1.0, shape=(config["y_res"], config["x_res"], 1)) else: self.observation_space = Box(0.0, 255.0, shape=(config["y_res"], config["x_res"], 3)) 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.episode_id = None self.measurements_file = None self.weather = None self.player_start = 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["map"], "-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 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: os.killpg(os.getpgid(self.server_process.pid), signal.SIGKILL) 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() # reset twice since the first time a server is initialized, # the starting location is different self._reset() 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.prev_measurement = 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.weather = random.choice(self.config["weather"]) settings.set(SynchronousMode=True, SendNonPlayerAgentsInfo=True, NumberOfVehicles=self.config["num_vehicles"], NumberOfPedestrians=self.config["num_pedestrians"], WeatherId=self.weather) settings.randomize_seeds() 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) scene = self.client.load_settings(settings) # Choose one player start at random. number_of_player_starts = len(scene.player_start_spots) if self.config["random_starting_location"]: self.player_start = random.randint( 0, max(0, number_of_player_starts - 1)) else: self.player_start = 0 # 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.player_start) image, py_measurements = self._read_observation() self.prev_measurement = py_measurements return self.preprocess_image(image) 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 np.zeros(self.observation_space.shape), 0.0, True, {} def _step(self, action): if self.config["discrete_actions"]: action = int(action) assert action in range(10) if action == 9: brake = 1.0 steer = 0.0 throttle = 0.0 reverse = False else: brake = 0.0 if action >= 6: steer = -1.0 elif action >= 3: steer = 1.0 else: steer = 0.0 action %= 3 if action == 0: throttle = 0.0 reverse = False elif action == 1: throttle = 1.0 reverse = False elif action == 2: throttle = 1.0 reverse = True else: assert len(action) == 3, "Invalid action {}".format(action) steer = action[0] throttle = min(1.0, abs(action[1])) brake = max(0.0, min(1.0, action[2])) reverse = action[1] < 0.0 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() reward, done = compute_reward(self.config, self.prev_measurement, py_measurements) if self.num_steps > self.config["max_steps"]: done = True self.total_reward += reward py_measurements["reward"] = reward py_measurements["total_reward"] = self.total_reward py_measurements["done"] = done py_measurements["action"] = action py_measurements["control"] = { "steer": steer, "throttle": throttle, "brake": brake, "reverse": reverse, "hand_brake": hand_brake, } 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 self.num_steps += 1 image = self.preprocess_image(image) return image, reward, done, py_measurements def preprocess_image(self, image): if 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) 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 py_measurements = { "episode_id": self.episode_id, "step": self.num_steps, "x": cur.transform.location.x, "y": cur.transform.location.y, "forward_speed": cur.forward_speed, "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["map"], "target_x": self.config["target_x"], "target_y": self.config["target_y"], "x_res": self.config["x_res"], "y_res": self.config["y_res"], "num_vehicles": self.config["num_vehicles"], "num_pedestrians": self.config["num_pedestrians"], "max_steps": self.config["max_steps"], } if CARLA_OUT_PATH: 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