def _setup_carla_client(self): carla_client = CarlaClient(self._params['host'], self._params['port'], timeout=None) carla_client.connect() ### create initial settings carla_settings = CarlaSettings() carla_settings.set( SynchronousMode=True, SendNonPlayerAgentsInfo=False, NumberOfVehicles=self._params['number_of_vehicles'], NumberOfPedestrians=self._params['number_of_pedestrians'], WeatherId=self._params['weather']) carla_settings.randomize_seeds() ### add cameras for camera_name in self._params['cameras']: camera_params = self._params[camera_name] camera_postprocessing = camera_params['postprocessing'] camera = Camera(camera_name, PostProcessing=camera_postprocessing) camera.set_image_size(CarlaCollSpeedEnv.CAMERA_HEIGHT * CarlaCollSpeedEnv.WIDTH_TO_HEIGHT_RATIO, CarlaCollSpeedEnv.CAMERA_HEIGHT) camera.set_position(*camera_params['position']) camera.set(**{'FOV': camera_params['fov']}) carla_settings.add_sensor(camera) ### load settings carla_scene = carla_client.load_settings(carla_settings) self._carla_client = carla_client self._carla_settings = carla_settings self._carla_scene = carla_scene
def reset(self): print('reset!') print('=.' * 40) a = 0 log_level = logging.INFO logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level) if os.path.exists('port.txt'): with open('port.txt', 'r') as f: a = int(f.readlines()[0].strip()) print(a) print('=.' * 40) with open('port.txt', 'w') as f: f.write(str(a + 1) + '\n') logging.info('listening to server %s:%s', 'localhost', 7899) while True: try: client = CarlaClient('localhost', 7899, timeout=1000) client.connect() self.carla_game = CarlaGame(client) self.carla_game.reset() obs = self.carla_game.get_obs() self.carla_game.display() return obs[0] except TCPConnectionError as error: logging.error(error) time.sleep(1)
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) 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 CarlaLegacyOperator(Op): """ CarlaLegacyOperator initializes and controls the simulator. This operator connects to the simulator, spawns actors, gets and publishes ground info, and sends vehicle commands. The operator works with Carla 0.8.4. """ def __init__(self, name, flags, auto_pilot, camera_setups=[], lidar_setups=[], log_file_name=None, csv_file_name=None): super(CarlaLegacyOperator, self).__init__(name) self._flags = flags self._logger = setup_logging(self.name, log_file_name) self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name) self._auto_pilot = auto_pilot if self._flags.carla_high_quality: quality = 'Epic' else: quality = 'Low' self._settings = CarlaSettings() self._settings.set( SynchronousMode=self._flags.carla_synchronous_mode, SendNonPlayerAgentsInfo=True, NumberOfVehicles=self._flags.carla_num_vehicles, NumberOfPedestrians=self._flags.carla_num_pedestrians, WeatherId=self._flags.carla_weather, QualityLevel=quality) self._settings.randomize_seeds() self._transforms = {} # Add cameras to the simulation. for cs in camera_setups: self.__add_camera(cs) self._transforms[cs.name] = cs.get_transform() # Add lidars to the simulation. for ls in lidar_setups: self.__add_lidar(ls) self._transforms[ls.name] = ls.get_transform() self.agent_id_map = {} self.pedestrian_count = 0 # Initialize the control state. self.control = { 'steer': 0.0, 'throttle': 0.0, 'brake': 0.0, 'hand_brake': False, 'reverse': False } # Register custom serializers for Messages and WatermarkMessages ray.register_custom_serializer(Message, use_pickle=True) ray.register_custom_serializer(WatermarkMessage, use_pickle=True) @staticmethod def setup_streams(input_streams, camera_setups, lidar_setups): input_streams.add_callback(CarlaLegacyOperator.on_control_msg) camera_streams = [ pylot.utils.create_camera_stream(cs) for cs in camera_setups ] lidar_streams = [ pylot.utils.create_lidar_stream(ls) for ls in lidar_setups ] return [ pylot.utils.create_can_bus_stream(), pylot.utils.create_ground_traffic_lights_stream(), pylot.utils.create_ground_vehicles_stream(), pylot.utils.create_ground_pedestrians_stream(), pylot.utils.create_ground_speed_limit_signs_stream(), pylot.utils.create_ground_stop_signs_stream() ] + camera_streams + lidar_streams def __add_camera(self, camera_setup): """Adds a camera and a corresponding output stream. Args: camera_setup: A camera setup object. """ # Transform from Carla 0.9.x postprocessing strings to Carla 0.8.4. if camera_setup.camera_type == 'sensor.camera.rgb': postprocessing = 'SceneFinal' elif camera_setup.camera_type == 'sensor.camera.depth': postprocessing = 'Depth' elif camera_setup.camera_type == 'sensor.camera.semantic_segmentation': postprocessing = 'SemanticSegmentation' transform = camera_setup.get_transform() camera = Camera(name=camera_setup.name, PostProcessing=postprocessing, FOV=camera_setup.fov, ImageSizeX=camera_setup.width, ImageSizeY=camera_setup.height, PositionX=transform.location.x, PositionY=transform.location.y, PositionZ=transform.location.z, RotationPitch=transform.rotation.pitch, RotationRoll=transform.rotation.roll, RotationYaw=transform.rotation.yaw) self._settings.add_sensor(camera) def __add_lidar(self, lidar_setup): """Adds a LIDAR sensor and a corresponding output stream. Args: lidar_setup: A LidarSetup object.. """ transform = lidar_setup.get_transform() lidar = Lidar(lidar_setup.name, Channels=lidar_setup.channels, Range=lidar_setup.range, PointsPerSecond=lidar_setup.points_per_second, RotationFrequency=lidar_setup.rotation_frequency, UpperFovLimit=lidar_setup.upper_fov, LowerFovLimit=lidar_setup.lower_fov, PositionX=transform.location.x, PositionY=transform.location.y, PositionZ=transform.location.z, RotationPitch=transform.rotation.pitch, RotationYaw=transform.rotation.yaw, RotationRoll=transform.rotation.roll) self._settings.add_sensor(lidar) def publish_world_data(self): read_start_time = time.time() measurements, sensor_data = self.client.read_data() measure_time = time.time() self._logger.info( 'Got readings for game time {} and platform time {}'.format( measurements.game_timestamp, measurements.platform_timestamp)) timestamp = Timestamp(coordinates=[measurements.game_timestamp]) watermark = WatermarkMessage(timestamp) # Send player data on data streams. self.__send_player_data(measurements.player_measurements, timestamp, watermark) # Extract agent data from measurements. agents = self.__get_ground_agents(measurements) # Send agent data on data streams. self.__send_ground_agent_data(agents, timestamp, watermark) # Send sensor data on data stream. self.__send_sensor_data(sensor_data, timestamp, watermark) # Get Autopilot control data. if self._auto_pilot: self.control = measurements.player_measurements.autopilot_control end_time = time.time() measurement_runtime = (measure_time - read_start_time) * 1000 total_runtime = (end_time - read_start_time) * 1000 self._logger.info('Carla measurement time {}; total time {}'.format( measurement_runtime, total_runtime)) self._csv_logger.info('{},{},{},{}'.format(time_epoch_ms(), self.name, measurement_runtime, total_runtime)) def __send_player_data(self, player_measurements, timestamp, watermark): """ Sends hero vehicle information. It populates a CanBus tuple. """ location = pylot.simulation.utils.Location( carla_loc=player_measurements.transform.location) rotation = pylot.simulation.utils.Rotation( player_measurements.transform.rotation.pitch, player_measurements.transform.rotation.yaw, player_measurements.transform.rotation.roll) orientation = pylot.simulation.utils.Orientation( player_measurements.transform.orientation.x, player_measurements.transform.orientation.y, player_measurements.transform.orientation.z) vehicle_transform = pylot.simulation.utils.Transform( location, rotation, orientation=orientation) forward_speed = player_measurements.forward_speed * 3.6 can_bus = pylot.simulation.utils.CanBus(vehicle_transform, forward_speed) self.get_output_stream('can_bus').send(Message(can_bus, timestamp)) self.get_output_stream('can_bus').send(watermark) def __get_ground_agents(self, measurements): vehicles = [] pedestrians = [] traffic_lights = [] speed_limit_signs = [] for agent in measurements.non_player_agents: if agent.HasField('vehicle'): transform = pylot.simulation.utils.to_pylot_transform( agent.vehicle.transform) bb = pylot.simulation.utils.BoundingBox( agent.vehicle.bounding_box) forward_speed = agent.vehicle.forward_speed vehicle = pylot.simulation.utils.Vehicle( transform, bb, forward_speed) vehicles.append(vehicle) elif agent.HasField('pedestrian'): if not self.agent_id_map.get(agent.id): self.pedestrian_count += 1 self.agent_id_map[agent.id] = self.pedestrian_count pedestrian_index = self.agent_id_map[agent.id] transform = pylot.simulation.utils.to_pylot_transform( agent.pedestrian.transform) bb = pylot.simulation.utils.BoundingBox( agent.pedestrian.bounding_box) forward_speed = agent.pedestrian.forward_speed pedestrian = pylot.simulation.utils.Pedestrian( pedestrian_index, transform, bb, forward_speed) pedestrians.append(pedestrian) elif agent.HasField('traffic_light'): transform = pylot.simulation.utils.to_pylot_transform( agent.traffic_light.transform) if agent.traffic_light.state == 2: erdos_tl_state = TrafficLightColor.RED elif agent.traffic_light.state == 1: erdos_tl_state = TrafficLightColor.YELLOW elif agent.traffic_light.state == 0: erdos_tl_state = TrafficLightColor.GREEN else: erdos_tl_state = TrafficLightColor.OFF traffic_light = pylot.simulation.utils.TrafficLight( agent.id, transform, erdos_tl_state, None) traffic_lights.append(traffic_light) elif agent.HasField('speed_limit_sign'): transform = pylot.simulation.utils.to_pylot_transform( agent.speed_limit_sign.transform) speed_sign = pylot.simulation.utils.SpeedLimitSign( transform, agent.speed_limit_sign.speed_limit) speed_limit_signs.append(speed_sign) return vehicles, pedestrians, traffic_lights, speed_limit_signs def __send_ground_agent_data(self, agents, timestamp, watermark): vehicles, pedestrians, traffic_lights, speed_limit_signs = agents vehicles_msg = pylot.simulation.messages.GroundVehiclesMessage( vehicles, timestamp) self.get_output_stream('vehicles').send(vehicles_msg) self.get_output_stream('vehicles').send(watermark) pedestrians_msg = pylot.simulation.messages.GroundPedestriansMessage( pedestrians, timestamp) self.get_output_stream('pedestrians').send(pedestrians_msg) self.get_output_stream('pedestrians').send(watermark) traffic_lights_msg = pylot.simulation.messages.GroundTrafficLightsMessage( traffic_lights, timestamp) self.get_output_stream('traffic_lights').send(traffic_lights_msg) self.get_output_stream('traffic_lights').send(watermark) speed_limits_msg = pylot.simulation.messages.GroundSpeedSignsMessage( speed_limit_signs, timestamp) self.get_output_stream('speed_limit_signs').send(speed_limits_msg) self.get_output_stream('speed_limit_signs').send(watermark) # We do not have any stop signs. stop_signs_msg = pylot.simulation.messages.GroundStopSignsMessage( [], timestamp) self.get_output_stream('stop_signs').send(stop_signs_msg) self.get_output_stream('stop_signs').send(watermark) def __send_sensor_data(self, sensor_data, timestamp, watermark): for name, measurement in sensor_data.items(): data_stream = self.get_output_stream(name) if data_stream.get_label('camera_type') == 'sensor.camera.rgb': # Transform the Carla RGB images to BGR. data_stream.send( pylot.simulation.messages.FrameMessage( pylot.utils.bgra_to_bgr(to_bgra_array(measurement)), timestamp)) elif data_stream.get_label( 'camera_type') == 'sensor.camera.semantic_segmentation': frame = labels_to_array(measurement) data_stream.send(SegmentedFrameMessage(frame, 0, timestamp)) elif data_stream.get_label('camera_type') == 'sensor.camera.depth': # NOTE: depth_to_array flips the image. data_stream.send( pylot.simulation.messages.DepthFrameMessage( depth_to_array(measurement), self._transforms[name], measurement.fov, timestamp)) elif data_stream.get_label( 'sensor_type') == 'sensor.lidar.ray_cast': pc_msg = pylot.simulation.messages.PointCloudMessage( measurement.data, self._transforms[name], timestamp) data_stream.send(pc_msg) else: data_stream.send(Message(measurement, timestamp)) data_stream.send(watermark) def _tick_simulator(self): if (not self._flags.carla_synchronous_mode or self._flags.carla_step_frequency == -1): # Run as fast as possible. self.publish_world_data() return time_until_tick = self._tick_at - time.time() if time_until_tick > 0: time.sleep(time_until_tick) else: self._logger.error('Cannot tick Carla at frequency {}'.format( self._flags.carla_step_frequency)) self._tick_at += 1.0 / self._flags.carla_step_frequency self.publish_world_data() def on_control_msg(self, msg): """ Invoked when a ControlMessage is received. Args: msg: A control.messages.ControlMessage message. """ if not self._auto_pilot: # Update the control dict state. self.control['steer'] = msg.steer self.control['throttle'] = msg.throttle self.control['brake'] = msg.brake self.control['hand_brake'] = msg.hand_brake self.control['reverse'] = msg.reverse self.client.send_control(**self.control) else: self.client.send_control(self.control) self._tick_simulator() def execute(self): # Connect to the simulator. self.client = CarlaClient(self._flags.carla_host, self._flags.carla_port, timeout=self._flags.carla_timeout) self.client.connect() scene = self.client.load_settings(self._settings) # Choose one player start at random. number_of_player_starts = len(scene.player_start_spots) player_start = self._flags.carla_start_player_num if self._flags.carla_random_player_start: player_start = np.random.randint( 0, max(0, number_of_player_starts - 1)) self.client.start_episode(player_start) self._tick_at = time.time() self._tick_simulator() self.spin()
class 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 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(object): def __init__( self, host='eceftl1.ces.clemson.edu', port=2000, image_filename_format='_images/episode_{:0>3d}/{:s}/image_{:0>5d}.png', save_images_to_disk=False): self.image_filename_format = image_filename_format self.save_images_to_disk = save_images_to_disk self.host = host self.port = port self.timeout = 60 self._Observation = collections.namedtuple('Observation', ['image', 'label']) self.frame = 0 self.episode_index = -1 self.client = CarlaClient(self.host, self.port, self.timeout) def connect(self): self.client.connect() def __exit__(self, exc_type, exc_val, exc_tb): self.client.disconnect() def start_new_episode(self, player_index): settings = CarlaSettings() settings.set(SynchronousMode=True, SendNonPlayerAgentsInfo=True, NumberOfVehicles=20, NumberOfPedestrians=40, WeatherId=random.choice([1, 3, 7, 8, 14])) settings.randomize_seeds() camera0 = Camera('CameraRGB') camera0.set(CameraFOV=100) camera0.set_image_size(800, 400) camera0.set_position(120, 0, 130) settings.add_sensor(camera0) camera2 = Camera('CameraSeg', PostProcessing='SemanticSegmentation') camera2.set(CameraFOV=100) camera2.set_image_size(800, 400) camera2.set_position(120, 0, 130) settings.add_sensor(camera2) self.client.load_settings(settings) self.episode_index += 1 self.frame = 0 print('\nStarting episode {0}.'.format(self.episode_index)) self.client.start_episode(player_index) def _get_observation(self, sensor_data): camera_rgb_data = sensor_data['CameraRGB'] camera_rgb_img = PImage.frombytes(mode='RGBA', size=(camera_rgb_data.width, camera_rgb_data.height), data=camera_rgb_data.raw_data, decoder_name='raw') b, g, r, a = camera_rgb_img.split() camera_rgb_img = PImage.merge("RGB", (r, g, b)) camera_seg_data = sensor_data['CameraSeg'] camera_seg_gray = image_converter.labels_to_grayscale2( camera_seg_data).astype('uint8') camera_seg_img = PImage.fromarray(camera_seg_gray, 'L') return self._Observation(camera_rgb_img, camera_seg_img) def _gen_images(self, observation): camera_rgb_img_filename = self.image_filename_format.format( self.episode_index, 'CameraRGB', self.frame) camera_seg_img_filename = self.image_filename_format.format( self.episode_index, 'CameraSeg', self.frame) rgb_folder = os.path.dirname(camera_rgb_img_filename) if not os.path.isdir(rgb_folder): os.makedirs(rgb_folder) seg_folder = os.path.dirname(camera_seg_img_filename) if not os.path.isdir(seg_folder): os.makedirs(seg_folder) observation.image.save(camera_rgb_img_filename) observation.label.save(camera_seg_img_filename) def auto_drive(self): measurements, sensor_data = self.client.read_data() print_measurements(measurements) observation = self._get_observation(sensor_data) if self.save_images_to_disk: self._gen_images(self._get_observation(sensor_data)) control = measurements.player_measurements.autopilot_control control.steer += random.uniform(-0.1, 0.1) self.client.send_control(control) self.frame += 1 def step(self, *args, **kwargs): measurements, sensor_data = self.client.read_data() print_measurements(measurements) observation = self._get_observation(sensor_data) if self.save_images_to_disk: self._gen_images(observation) pm = measurements.player_measurements rewards = pm.collision_vehicles + pm.collision_pedestrians + pm.collision_other self.client.send_control(*args, **kwargs) self.frame += 1 return observation, rewards
class CarlaEnv: 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 Env(object): def __init__(self, log_dir, data_dir, image_agent, city="/Game/Maps/Town01"): self.log_dir = log_dir self.data_dir = data_dir self.carla_server_settings = None self.server = None self.server_pid = -99999 self.game = None #carla client self.map = city self.host = 'localhost' self.port = 2366 self.client = None self.is_connected = False self.render = None #TODO render with pygame self.Image_agent = image_agent self.timestep = 0 self.collision = 0 self.collision_vehicles = 0 self.collision_other = 0 self.stuck_cnt = 0 self.collision_cnt = 0 self.offroad_cnt = 0 self.ignite = False #steer,throttle,brake #self.action_space = action_space(3, (1.0, 1.0,1.0), (-1.0,0,0), SEED) #featured image,speed,steer,other lane ,offroad, #collision with pedestrians,vehicles,other #self.observation_space = observation_space(512 + 7) self.max_episode = 1000000 self.time_out_step = 10000 self.max_speed = 35 self.speed_up_steps = 20 self.current_episode = 0 self.weather = -1 self.current_step = 0 self.current_position = None self.total_reward = 0 self.planner = None self.carla_setting = None self.number_of_vehicles = None self.control = None self.nospeed_times = 0 self.reward = 0 self.observation = None self.done = False self.testing = False self.load_config() self.setup_client_and_server() def load_config(self): self.vehicle_pair = carla_config.NumberOfVehicles self.pedestrian_pair = carla_config.NumberOfPedestrians self.weather_set = carla_config.set_of_weathers #[straight,one_curve,navigation,navigation] if self.map == "/Game/Maps/Town01": self.poses = carla_config.poses_town01() elif self.map == "/Game/Maps/Town02": self.poses = carla_config.poses_town02() else: print("Unsupported Map Name") def reset(self, vehicle_num): #if not self.is_process_alive(self.server_pid): #self.setup_client_and_server() self.nospeed_times = 0 pose_type = random.choice(self.poses) #pose_type = self.poses[0] self.current_position = random.choice( pose_type) #start and end index #self.current_position = (53,67) # self.number_of_vehicles = random.randint( self.vehicle_pair[0],self.vehicle_pair[1]) # self.number_of_pedestrians = random.randint( self.vehicle_pair[0],self.vehicle_pair[1]) self.number_of_vehicles = vehicle_num self.number_of_pedestrians = 0 self.weather = random.choice(self.weather_set) self.timestep = 0 self.collision = 0 self.collision_vehicles = 0 self.collision_other = 0 self.stuck_cnt = 0 self.collision_cnt = 0 self.offroad_cnt = 0 self.ignite = False settings = carla_config.make_carla_settings() settings.set(NumberOfVehicles=self.number_of_vehicles, NumberOfPedestrians=self.number_of_pedestrians, WeatherId=self.weather) self.carla_setting = settings self.scene = self.game.load_settings(settings) self.game.start_episode( self.current_position[0]) #set the start position #print(self.current_position) self.target_transform = self.scene.player_start_spots[ self.current_position[1]] self.planner = Planner(self.scene.map_name) #skip the car fall to sence frame for i in range(self.speed_up_steps): self.control = VehicleControl() self.control.steer = 0 self.control.throttle = 0.025 * i self.control.brake = 0 self.control.hand_brake = False self.control.reverse = False time.sleep(0.05) send_success = self.send_control(self.control) if not send_success: return None self.game.send_control(self.control) #measurements, sensor_data = self.game.read_data() #measurements,sensor #direction =self.get_directions(measurements,self.target_transform,self.planner) #self.get_state(measurements,sensor_data,direction) measurements, sensor_data = self.game.read_data() #measurements,sensor directions = self.get_directions(measurements, self.target_transform, self.planner) if directions is None or measurements is None: return None state, _, _ = self.get_state(measurements, sensor_data, directions) return state def get_data(self): measurements = None sensor_data = None try: measurements, sensor_data = self.game.read_data() except Exception: return None, None return measurements, sensor_data def send_control(self, control): send_success = False try: self.game.send_control(control) send_success = True except Exception: print("Send Control error") return send_success def step(self, action): #take action ,update state #return: observation, reward,done 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[2], 0, 1)) self.control.hand_brake = False self.control.reverse = False send_success = self.send_control(self.control) if not send_success: return None, None, None #recive new data measurements, sensor_data = self.game.read_data() #measurements,sensor directions = self.get_directions(measurements, self.target_transform, self.planner) if measurements is None or directions is None: return None, None, None state, reward, done = self.get_state(measurements, sensor_data, directions) self.current_step += 1 return state, reward, done #comute new state,reward,and is done def get_state(self, measurements, sensor_data, directions): self.reward = 0 done = False img_feature = self.Image_agent.compute_feature( sensor_data) #shape = (512,) speed = measurements.player_measurements.forward_speed # m/s intersection_offroad = measurements.player_measurements.intersection_offroad intersection_otherlane = measurements.player_measurements.intersection_otherlane collision_vehicles = measurements.player_measurements.collision_vehicles collision_pedestrians = measurements.player_measurements.collision_pedestrians collision_other = measurements.player_measurements.collision_other # reward for steer if directions == 5: #go straight if abs(self.control.steer) > 0.2: self.reward -= 20 self.reward += min(35, speed * 3.6) elif directions == 2: #follow lane self.reward += min(25, speed * 3.6) elif directions == 3: #turn left ,steer should be 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 directions == 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 # reward for offroad and collision if intersection_offroad > 0: self.reward -= 100 if intersection_otherlane > 0: self.reward -= 100 elif collision_vehicles > 0: self.reward -= 100 elif collision_pedestrians > 0: self.reward -= 100 elif collision_other > 0: self.reward -= 50 # teminal state if collision_pedestrians > 0 or collision_vehicles > 0 or collision_other > 0: done = True #print("Collision~~~~~") if intersection_offroad > 0.2 or intersection_otherlane > 0.2: done = True #print("Offroad~~~~~") if speed * 3.6 <= 1.0: self.nospeed_times += 1 if self.nospeed_times > 100: done = True self.reward -= 1 else: self.nospeed_times = 0 speed = min(1, speed / 20.0) info = self.convert_info(measurements) self.reward = self.reward_from_info(info) done = self.done_from_info(info) return np.concatenate( (img_feature, (speed, directions))), self.reward, done def done_from_info(self, info): if info['collision'] > 0 or (self.collision_cnt > 0 and info['speed'] < 0.5): self.collision_cnt += 1 else: self.collision_cnt = 0 self.ignite = self.ignite or info['speed'] > 1 stuck = int(info['speed'] < 1) self.stuck_cnt = (self.stuck_cnt + stuck) * stuck * int( bool(self.ignite) or self.testing) if info['offroad'] > 0.5: self.offroad_cnt += 1 if self.stuck_cnt > 30: print("finish becase of stuck") return True elif self.offroad_cnt > 30: print("finish because of offroad") return True elif self.collision_cnt > 20: print("finish because of collision") return True else: return False def reward_from_info(self, info): reward = dict() reward['without_pos'] = info['speed'] / 15 - info[ 'offroad'] - info['collision'] * 2.0 reward['with_pos'] = reward['without_pos'] - info['offlane'] / 5 return reward['with_pos'] def convert_info(self, measurements): info = dict() info['speed'] = measurements.player_measurements.forward_speed info['collision'] = int( measurements.player_measurements.collision_other + measurements.player_measurements.collision_pedestrians + measurements.player_measurements.collision_vehicles > self.collision or (self.collision_cnt > 0 and info['speed'] < 0.5)) info['collision_other'] = int(measurements.player_measurements. collision_other > self.collision_other) info['collision_vehicles'] = int( measurements.player_measurements.collision_vehicles > self.collision_vehicles) info['coll_veh_num'] = int( measurements.player_measurements.collision_vehicles - self.collision_vehicles) self.collision_vehicles = measurements.player_measurements.collision_vehicles self.collision_other = measurements.player_measurements.collision_other self.collision = measurements.player_measurements.collision_other + measurements.player_measurements.collision_pedestrians + measurements.player_measurements.collision_vehicles info['offlane'] = int( measurements.player_measurements.intersection_otherlane > 0.01) info['offroad'] = int( measurements.player_measurements.intersection_offroad > 0.001) info[ 'expert_control'] = measurements.player_measurements.autopilot_control return info def _open_server(self): with open(self.log_dir, "wb") as out: cmd = [ os.path.join(os.environ.get('CARLA_ROOT'), 'CarlaUE4.sh'), self.map, "-carla-server", "-fps=10", "-world-port={}".format(self.port), "-windowed -ResX={} -ResY={}".format( carla_config.WINDOW_WIDTH, carla_config.WINDOW_HEIGHT) ] if self.carla_server_settings: cmd.append("-carla-settings={}".format( self.carla_server_settings)) p = subprocess.Popen(cmd, stdout=out, stderr=out) time.sleep(20) return p # This is not work def _close_server(self): no_of_attempts = 0 try: while self.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 except Exception as e: print(e) def close_server(self): sudopass = "******" command = "echo {0} |sudo -S kill -9 {1}".format( sudopass, self.server_pid) os.system(command) def is_process_alive(self, 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 def setup_client_and_server(self): # self.server = self._open_server() # self.server_pid = self.server.pid self.game = CarlaClient(self.host, self.port, timeout=99999999) #carla client self.game.connect(connection_attempts=100) def get_directions(self, measurements, target_transform, planner): """ Function to get the high level commands and the waypoints. The waypoints correspond to the local planning, the near path the car has to follow. """ # Get the current position from the measurements current_point = measurements.player_measurements.transform try: directions = 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), (target_transform.location.x, target_transform.location.y, 0.22), (target_transform.orientation.x, target_transform.orientation.y, target_transform.orientation.z)) except Exception: print("Route plan error ") directions = None return directions
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 CarlaOperator(Op): """Provides an ERDOS interface to the CARLA simulator. Args: synchronous_mode (bool): whether the simulator will wait for control input from the client. """ def __init__(self, name, flags, camera_setups=[], lidar_stream_names=[], log_file_name=None, csv_file_name=None): super(CarlaOperator, self).__init__(name) self._flags = flags self._logger = setup_logging(self.name, log_file_name) self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name) self.message_num = 0 if self._flags.carla_high_quality: quality = 'Epic' else: quality = 'Low' self.settings = CarlaSettings() self.settings.set( SynchronousMode=self._flags.carla_synchronous_mode, SendNonPlayerAgentsInfo=True, NumberOfVehicles=self._flags.carla_num_vehicles, NumberOfPedestrians=self._flags.carla_num_pedestrians, WeatherId=self._flags.carla_weather, QualityLevel=quality) self.settings.randomize_seeds() self.lidar_streams = [] for (camera_stream_name, camera_type, image_size, pos) in camera_setups: self.__add_camera(name=camera_stream_name, postprocessing=camera_type, image_size=image_size, position=pos) for lidar_stream_name in lidar_stream_names: self.__add_lidar(name=lidar_stream_name) self.agent_id_map = {} self.pedestrian_count = 0 @staticmethod def setup_streams(input_streams, camera_setups, lidar_stream_names): input_streams.add_callback(CarlaOperator.update_control) camera_streams = [ DataStream(name=camera, labels={ 'sensor_type': 'camera', 'camera_type': camera_type }) for (camera, camera_type, _, _) in camera_setups ] lidar_streams = [ DataStream(name=lidar, labels={'sensor_type': 'lidar'}) for lidar in lidar_stream_names ] return [ DataStream(name='world_transform'), DataStream(name='vehicle_pos'), DataStream(name='acceleration'), DataStream(data_type=Float64, name='forward_speed'), DataStream(data_type=Float64, name='vehicle_collisions'), DataStream(data_type=Float64, name='pedestrian_collisions'), DataStream(data_type=Float64, name='other_collisions'), DataStream(data_type=Float64, name='other_lane'), DataStream(data_type=Float64, name='offroad'), DataStream(name='traffic_lights'), DataStream(name='pedestrians'), DataStream(name='vehicles'), DataStream(name='traffic_signs'), ] + camera_streams + lidar_streams def __add_camera(self, name, postprocessing, image_size=(800, 600), field_of_view=90.0, position=(0.3, 0, 1.3), rotation_pitch=0, rotation_roll=0, rotation_yaw=0): """Adds a camera and a corresponding output stream. Args: name: A string naming the camera. postprocessing: "SceneFinal", "Depth", "SemanticSegmentation". """ camera = Camera(name, PostProcessing=postprocessing, FOV=field_of_view, ImageSizeX=image_size[0], ImageSizeY=image_size[1], PositionX=position[0], PositionY=position[1], PositionZ=position[2], RotationPitch=rotation_pitch, RotationRoll=rotation_roll, RotationYaw=rotation_yaw) self.settings.add_sensor(camera) def __add_lidar(self, name, channels=32, max_range=50, points_per_second=100000, rotation_frequency=10, upper_fov_limit=10, lower_fov_limit=-30, position=(0, 0, 1.4), rotation_pitch=0, rotation_yaw=0, rotation_roll=0): """Adds a LIDAR sensor and a corresponding output stream. Args: name: A string naming the camera. """ lidar = Lidar(name, Channels=channels, Range=max_range, PointsPerSecond=points_per_second, RotationFrequency=rotation_frequency, UpperFovLimit=upper_fov_limit, LowerFovLimit=lower_fov_limit, PositionX=position[0], PositionY=position[1], PositionZ=position[2], RotationPitch=rotation_pitch, RotationYaw=rotation_yaw, RotationRoll=rotation_roll) self.settings.add_sensor(lidar) output_stream = DataStream(name=name, labels={"sensor_type": "lidar"}) self.lidar_streams.append(output_stream) def read_carla_data(self): read_start_time = time.time() measurements, sensor_data = self.client.read_data() measure_time = time.time() self._logger.info( 'Got readings for game time {} and platform time {}'.format( measurements.game_timestamp, measurements.platform_timestamp)) # Send measurements player_measurements = measurements.player_measurements vehicle_pos = ((player_measurements.transform.location.x, player_measurements.transform.location.y, player_measurements.transform.location.z), (player_measurements.transform.orientation.x, player_measurements.transform.orientation.y, player_measurements.transform.orientation.z)) world_transform = Transform(player_measurements.transform) timestamp = Timestamp( coordinates=[measurements.game_timestamp, self.message_num]) self.message_num += 1 ray.register_custom_serializer(Message, use_pickle=True) ray.register_custom_serializer(WatermarkMessage, use_pickle=True) watermark = WatermarkMessage(timestamp) self.get_output_stream('world_transform').send( Message(world_transform, timestamp)) self.get_output_stream('world_transform').send(watermark) self.get_output_stream('vehicle_pos').send( Message(vehicle_pos, timestamp)) self.get_output_stream('vehicle_pos').send(watermark) acceleration = (player_measurements.acceleration.x, player_measurements.acceleration.y, player_measurements.acceleration.z) self.get_output_stream('acceleration').send( Message(acceleration, timestamp)) self.get_output_stream('acceleration').send(watermark) self.get_output_stream('forward_speed').send( Message(player_measurements.forward_speed, timestamp)) self.get_output_stream('forward_speed').send(watermark) self.get_output_stream('vehicle_collisions').send( Message(player_measurements.collision_vehicles, timestamp)) self.get_output_stream('vehicle_collisions').send(watermark) self.get_output_stream('pedestrian_collisions').send( Message(player_measurements.collision_pedestrians, timestamp)) self.get_output_stream('pedestrian_collisions').send(watermark) self.get_output_stream('other_collisions').send( Message(player_measurements.collision_other, timestamp)) self.get_output_stream('other_collisions').send(watermark) self.get_output_stream('other_lane').send( Message(player_measurements.intersection_otherlane, timestamp)) self.get_output_stream('other_lane').send(watermark) self.get_output_stream('offroad').send( Message(player_measurements.intersection_offroad, timestamp)) self.get_output_stream('offroad').send(watermark) vehicles = [] pedestrians = [] traffic_lights = [] speed_limit_signs = [] for agent in measurements.non_player_agents: if agent.HasField('vehicle'): pos = messages.Transform(agent.vehicle.transform) bb = messages.BoundingBox(agent.vehicle.bounding_box) forward_speed = agent.vehicle.forward_speed vehicle = messages.Vehicle(pos, bb, forward_speed) vehicles.append(vehicle) elif agent.HasField('pedestrian'): if not self.agent_id_map.get(agent.id): self.pedestrian_count += 1 self.agent_id_map[agent.id] = self.pedestrian_count pedestrian_index = self.agent_id_map[agent.id] pos = messages.Transform(agent.pedestrian.transform) bb = messages.BoundingBox(agent.pedestrian.bounding_box) forward_speed = agent.pedestrian.forward_speed pedestrian = messages.Pedestrian(pedestrian_index, pos, bb, forward_speed) pedestrians.append(pedestrian) elif agent.HasField('traffic_light'): transform = messages.Transform(agent.traffic_light.transform) traffic_light = messages.TrafficLight( transform, agent.traffic_light.state) traffic_lights.append(traffic_light) elif agent.HasField('speed_limit_sign'): transform = messages.Transform( agent.speed_limit_sign.transform) speed_sign = messages.SpeedLimitSign( transform, agent.speed_limit_sign.speed_limit) speed_limit_signs.append(speed_sign) vehicles_msg = Message(vehicles, timestamp) self.get_output_stream('vehicles').send(vehicles_msg) self.get_output_stream('vehicles').send(watermark) pedestrians_msg = Message(pedestrians, timestamp) self.get_output_stream('pedestrians').send(pedestrians_msg) self.get_output_stream('pedestrians').send(watermark) traffic_lights_msg = Message(traffic_lights, timestamp) self.get_output_stream('traffic_lights').send(traffic_lights_msg) self.get_output_stream('traffic_lights').send(watermark) traffic_sings_msg = Message(speed_limit_signs, timestamp) self.get_output_stream('traffic_signs').send(traffic_sings_msg) self.get_output_stream('traffic_signs').send(watermark) # Send sensor data for name, measurement in sensor_data.items(): data_stream = self.get_output_stream(name) if data_stream.get_label('camera_type') == 'SceneFinal': # Transform the Carla RGB images to BGR. data_stream.send( Message( pylot_utils.bgra_to_bgr(to_bgra_array(measurement)), timestamp)) else: data_stream.send(Message(measurement, timestamp)) data_stream.send(watermark) self.client.send_control(**self.control) end_time = time.time() measurement_runtime = (measure_time - read_start_time) * 1000 total_runtime = (end_time - read_start_time) * 1000 self._logger.info('Carla measurement time {}; total time {}'.format( measurement_runtime, total_runtime)) self._csv_logger.info('{},{},{},{}'.format(time_epoch_ms(), self.name, measurement_runtime, total_runtime)) def read_data_at_frequency(self): period = 1.0 / self._flags.carla_step_frequency trigger_at = time.time() + period while True: time_until_trigger = trigger_at - time.time() if time_until_trigger > 0: time.sleep(time_until_trigger) else: self._logger.error( 'Cannot read Carla data at frequency {}'.format( self._flags.carla_step_frequency)) self.read_carla_data() trigger_at += period def update_control(self, msg): """Updates the control dict""" self.control.update(msg.data) def execute(self): # Subscribe to control streams self.control = { 'steer': 0.0, 'throttle': 0.0, 'break': 0.0, 'hand_break': False, 'reverse': False } self.client = CarlaClient(self._flags.carla_host, self._flags.carla_port, timeout=10) self.client.connect() scene = self.client.load_settings(self.settings) # Choose one player start at random. number_of_player_starts = len(scene.player_start_spots) player_start = self._flags.carla_start_player_num if self._flags.carla_random_player_start: player_start = np.random.randint( 0, max(0, number_of_player_starts - 1)) self.client.start_episode(player_start) self.read_data_at_frequency() self.spin()
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 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 CarlaClientConsole(cmd.Cmd): def __init__(self, args): cmd.Cmd.__init__(self) self.args = args self.prompt = '\x1b[1;34m%s\x1b[0m ' % '(carla)' self.client = CarlaClient(args.host, args.port) self.settings = get_default_carla_settings(args) self.print_measurements = False self.control = _Control() self.thread = threading.Thread(target=self._agent_thread_worker) self.done = False self.thread.start() def cleanup(self): self.do_disconnect() self.done = True if self.thread.is_alive(): self.thread.join() def default(self, line): logging.error('unknown command \'%s\'!', line) def emptyline(self): pass def precmd(self, line): return line.strip().lower() def can_exit(self): return True def do_exit(self, line=None): """Exit the console.""" return True def do_eof(self, line=None): """Exit the console.""" print('exit') return self.do_exit(line) def help_help(self): print('usage: help [topic]') def do_disconnect(self, line=None): """Disconnect from the server.""" self.client.disconnect() def do_new_episode(self, line=None): """Request a new episode. Connect to the server if not connected.""" try: self.control = _Control() if not self.client.connected(): self.client.connect() self.client.load_settings(self.settings) self.client.start_episode(0) logging.info('new episode started') except Exception as exception: logging.error(exception) def do_control(self, line=None): """Set control message:\nusage: control [reset|<param> <value>]\n(e.g., control throttle 0.5)""" try: if line == 'reset': self.control = _Control() else: self.control.set(line) logging.debug('control: %s', self.control.kwargs()) except Exception as exception: logging.error(exception) def complete_control(self, text, *args, **kwargs): options = self.control.action_list() options.append('reset') return [x + ' ' for x in options if x.startswith(text)] def do_settings(self, line=None): """Open a text editor to edit CARLA settings.""" result = edit_text(self.settings) if result is not None: self.settings = result def do_print_measurements(self, line): """Print received measurements to console.\nusage: print_measurements [t/f]""" self.print_measurements = True if not line else _Control._parse( bool, line) def _agent_thread_worker(self): filename = '_images/console/camera_{:0>3d}/image_{:0>8d}.png' while not self.done: try: measurements, sensor_data = self.client.read_data() if self.print_measurements: print(measurements) if self.args.images_to_disk: images = [ x for x in sensor_data.values() if isinstance(x, Image) ] for n, image in enumerate(images): path = filename.format(n, measurements.game_timestamp) image.save_to_disk(path) self.client.send_control(**self.control.kwargs()) except Exception as exception: # logging.exception(exception) time.sleep(1)
class CarlaEnv(gym.Env): def __init__(self, host="localhost", num_vehicles=1, vehicles_seed=lambda: 200, player_starts=2, goals=0): self.num_vehicles = num_vehicles self.vehicles_seed = vehicles_seed self.starts = list_from_scalar(player_starts) self.goals = list_from_scalar(goals) self.port = get_open_port() self.host = host self.metadata = { 'render.modes': ['rgb_array'], #'video.frames_per_second': int(np.round(1.0 / self.dt)) } self.server = self.open_server() print(f"Connecting to CARLA Client on port {self.port}") self.client = CarlaClient(self.host, self.port, timeout=99999999) time.sleep(3) self.client.connect(connection_attempts=1000) print(f"Connected on port: {self.port}") self.action_space = gym.spaces.Box( -2, 2, (len(self._map_controls([1, 2, 3, 4, 5]).items()), ), dtype=np.float32) obs_size = len(self.reset()) self.observation_space = gym.spaces.Box(-float("inf"), float("inf"), (obs_size, ), dtype=np.float32) def dist_from_goal(self, measurements): x = array_from_loc(measurements.player_measurements.transform.location) y = array_from_loc(self.scene.player_start_spots[self.goal].location) return np.sqrt(np.sum((x - y)**2)) def open_server(self): #os.devnull log_file = f"./logs/carla_logs/carla_{self.port}.txt" os.makedirs(os.path.dirname(log_file), exist_ok=True) with open(log_file, "wb+") as out: """ true_script_name = os.path.join(CARLA_PATH, 'CarlaUE4.sh') if os.path.islink(true_script_name): true_script_name = os.readlink(execute_path) project_root = os.path.dirname(true_script_name) execute_path = f"{project_root}/CarlaUE4/Binaries/Linux/CarlaUE4" # chmod +x st = os.stat(execute_path) os.chmod(execute_path, st.st_mode | stat.S_IEXEC) """ cmd = [ os.path.join(CARLA_PATH, 'CarlaUE4.sh'), #execute_path, "CarlaUE4", "-carla-server", "-fps=60", f"-world-port={self.port}", f"-windowed -ResX={500} -ResY={500}", "-carla-no-hud" ] # - benchmark p = subprocess.Popen(cmd, stdout=out, stderr=out, stdin=subprocess.PIPE, preexec_fn=os.setpgrp) return p def close_server(self): #self.server.terminate() pid = self.server.pid no_of_attempts = 0 def is_process_alive(pid): ## Source: https://stackoverflow.com/questions/568271/how-to-check-if-there-exists-a-process-with-a-given-pid-in-python try: os.kill(pid, 0) except OSError: return False return True while is_process_alive(pid): pgroup = os.getpgid(self.server.pid) self.server.terminate() os.killpg(pgroup, signal.SIGTERM) _, _ = os.waitpid(pid, os.WNOHANG) time.sleep(5) def _add_settings(self): self.settings.set(SynchronousMode=True, SendNonPlayerAgentsInfo=True, NumberOfVehicles=self.num_vehicles, NumberOfPedestrians=0, WeatherId=1, QualityLevel="Low", SeedVehicles=self.vehicles_seed()) #settings.randomize_seeds() def _add_sensors(self): camera0 = Camera('RenderCamera0') # Set image resolution in pixels. camera0.set_image_size(800, 600) # Set its position relative to the car in meters. camera0.set_position(-4.30, 0, 2.60) camera0.set_rotation(pitch=-25, yaw=0, roll=0) self.settings.add_sensor(camera0) def _map_controls(self, a): return dict(steer=a[0], throttle=a[1]) def _process_observation(self, measurements, sensor_data): return array_from_measurements(measurements) def _get_reward_and_termination(self): measurements, sensor_data = self.current_state collision_penalty = ( measurements.player_measurements.collision_vehicles + measurements.player_measurements.collision_other) is_collided = collision_penalty > 2 offroad_penalty = measurements.player_measurements.intersection_offroad is_offroad = offroad_penalty > 0.5 dist_from_goal = self.dist_from_goal(measurements) is_at_target = dist_from_goal < 5 is_done = is_collided or is_at_target or is_offroad distance_reward = 300 - dist_from_goal collision_cost = collision_penalty / 300 offroad_cost = (offroad_penalty * 10) reward = distance_reward - collision_cost - offroad_cost reward = reward / 20 return reward, is_done def get_new_start_goal(self): start = np.random.choice(self.starts) goal = np.random.choice(self.goals) while goal == start: start = np.random.choice(self.starts) goal = np.random.choice(self.goals) return start, goal def reset(self): self.settings = CarlaSettings() self._add_settings() self._add_sensors() self.scene = self.client.load_settings(self.settings) start, goal = self.get_new_start_goal() self.goal = goal for i in range(100): try: self.client.start_episode(start) self.current_state = self.client.read_data() if i > 0: print("Reconnected.") break except carla.tcp.TCPConnectionError: if i % 10 == 0: print(f"There was a TCP Error (Attempt {i}). Retrying. ") time.sleep(3) measurements, sensor_data = self.current_state return self._process_observation(measurements, sensor_data) def step(self, a): control = self._map_controls(a) for i in range(100): try: self.client.send_control(**control) break except carla.tcp.TCPConnectionError: time.sleep(0.5) self.current_state = self.client.read_data() measurements, sensor_data = self.current_state reward, is_done = self._get_reward_and_termination() obs = self._process_observation(measurements, sensor_data) return obs, reward, is_done, {} def render(self, mode='human', **kwargs): if mode == 'rgb_array': return np.concatenate([ sensor.data for name, sensor in self.current_state[1].items() if "Render" in name ], axis=1) super().render(mode=mode, **kwargs) def _close(self): self.close() def close(self): print( f"Disconnecting from CARLA Client (port: {self.port}, pid: {self.server.pid})" ) self.close_server() time.sleep(3) while self.client.connected(): self.client.disconnect() print( f"Disconnected from CARLA Client (port: {self.port}, pid: {self.server.pid})." )
class 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 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 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 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(object): ''' An OpenAI Gym Environment for CARLA. ''' def __init__(self, obs_converter, action_converter, env_id, random_seed=0, exp_suite_name='TrainingSuite', reward_class_name='RewardCarla', host='127.0.0.1', port=2000, city_name='Town01', subset=None, video_every=100, video_dir='./video/', distance_for_success=2.0, benchmark=False): self.logger = get_carla_logger() self.logger.info('Environment {} running in port {}'.format( env_id, port)) self.host, self.port = host, port self.id = env_id self._obs_converter = obs_converter self.observation_space = obs_converter.get_observation_space() self._action_converter = action_converter self.action_space = self._action_converter.get_action_space() if benchmark: self._experiment_suite = getattr(experiment_suites_benchmark, exp_suite_name)(city_name) else: self._experiment_suite = getattr(experiment_suites, exp_suite_name)(city_name, subset) self._reward = getattr(rewards, reward_class_name)() self._experiments = self._experiment_suite.get_experiments() self.subset = subset self._make_carla_client(host, port) self._distance_for_success = distance_for_success self._planner = Planner(city_name) self.done = False self.last_obs = None self.last_distance_to_goal = None self.last_direction = None self.last_measurements = None np.random.seed(random_seed) self.video_every = video_every self.video_dir = video_dir self.video_writer = None self._success = False self._failure_timeout = False self._failure_collision = False self.benchmark = benchmark self.benchmark_index = [0, 0, 0] try: if not os.path.isdir(self.video_dir): os.makedirs(self.video_dir) except OSError: pass self.steps = 0 self.num_episodes = 0 def step(self, action): if self.done: raise ValueError( 'self.done should always be False when calling step') while True: try: # Send control control = self._action_converter.action_to_control( action, self.last_measurements) self._client.send_control(control) # Gather the observations (including measurements, sensor and directions) measurements, sensor_data = self._client.read_data() self.last_measurements = measurements current_timestamp = measurements.game_timestamp distance_to_goal = self._get_distance_to_goal( measurements, self._target) self.last_distance_to_goal = distance_to_goal directions = self._get_directions( measurements.player_measurements.transform, self._target) self.last_direction = directions obs = self._obs_converter.convert(measurements, sensor_data, directions, self._target, self.id) if self.video_writer is not None and self.steps % 2 == 0: self._raster_frame(sensor_data, measurements, directions, obs) self.last_obs = obs except CameraException: self.logger.debug('Camera Exception in step()') obs = self.last_obs distance_to_goal = self.last_distance_to_goal current_timestamp = self.last_measurements.game_timestamp except TCPConnectionError as e: self.logger.debug( 'TCPConnectionError inside step(): {}'.format(e)) self.done = True return self.last_obs, 0.0, True, {'carla-reward': 0.0} break # Check if terminal state timeout = (current_timestamp - self._initial_timestamp) > (self._time_out * 1000) collision, _ = self._is_collision(measurements) success = distance_to_goal < self._distance_for_success if timeout: self.logger.debug('Timeout') self._failure_timeout = True if collision: self.logger.debug('Collision') self._failure_collision = True if success: self.logger.debug('Success') self.done = timeout or collision or success # Get the reward env_state = { 'timeout': timeout, 'collision': collision, 'success': success } reward = self._reward.get_reward(measurements, self._target, self.last_direction, control, env_state) # Additional information info = {'carla-reward': reward} self.steps += 1 return obs, reward, self.done, info def reset(self): # Loop forever due to TCPConnectionErrors while True: try: self._reward.reset_reward() self.done = False if self.video_writer is not None: try: self.video_writer.close() except Exception as e: self.logger.debug( 'Error when closing video writer in reset') self.logger.error(e) self.video_writer = None if self.benchmark: end_indicator = self._new_episode_benchmark() if end_indicator is False: return False else: self._new_episode() # Hack: Try sleeping so that the server is ready. Reduces the number of TCPErrors time.sleep(4) # measurements, sensor_data = self._client.read_data() self._client.send_control(VehicleControl()) measurements, sensor_data = self._client.read_data() self._initial_timestamp = measurements.game_timestamp self.last_measurements = measurements self.last_distance_to_goal = self._get_distance_to_goal( measurements, self._target) directions = self._get_directions( measurements.player_measurements.transform, self._target) self.last_direction = directions obs = self._obs_converter.convert(measurements, sensor_data, directions, self._target, self.id) self.last_obs = obs self.done = False self._success = False self._failure_timeout = False self._failure_collision = False return obs except CameraException: self.logger.debug('Camera Exception in reset()') continue except TCPConnectionError as e: self.logger.debug('TCPConnectionError in reset()') self.logger.error(e) # Disconnect and reconnect self.disconnect() time.sleep(5) self._make_carla_client(self.host, self.port) def disconnect(self): if self.video_writer is not None: try: self.video_writer.close() except Exception as e: self.logger.debug( 'Error when closing video writer in disconnect') self.logger.error(e) self.video_writer = None self._client.disconnect() def _raster_frame(self, sensor_data, measurements, directions, obs): frame = sensor_data['CameraRGB'].data.copy() cv2.putText(frame, text='Episode number: {:,}'.format(self.num_episodes - 1), org=(50, 50), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1.0, color=[0, 0, 0], thickness=2) cv2.putText(frame, text='Environment steps: {:,}'.format(self.steps), org=(50, 80), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1.0, color=[0, 0, 0], thickness=2) REACH_GOAL = 0.0 GO_STRAIGHT = 5.0 TURN_RIGHT = 4.0 TURN_LEFT = 3.0 LANE_FOLLOW = 2.0 if np.isclose(directions, REACH_GOAL): dir_str = 'REACH GOAL' elif np.isclose(directions, GO_STRAIGHT): dir_str = 'GO STRAIGHT' elif np.isclose(directions, TURN_RIGHT): dir_str = 'TURN RIGHT' elif np.isclose(directions, TURN_LEFT): dir_str = 'TURN LEFT' elif np.isclose(directions, LANE_FOLLOW): dir_str = 'LANE FOLLOW' else: raise ValueError(directions) cv2.putText(frame, text='Direction: {}'.format(dir_str), org=(50, 110), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1.0, color=[0, 0, 0], thickness=2) cv2.putText(frame, text='Speed: {:.02f}'.format( measurements.player_measurements.forward_speed * 3.6), org=(50, 140), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1.0, color=[0, 0, 0], thickness=2) cv2.putText(frame, text='rel_x: {:.02f}, rel_y: {:.02f}'.format( obs['v'][-2].item(), obs['v'][-1].item()), org=(50, 170), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=1.0, color=[0, 0, 0], thickness=2) self.video_writer.writeFrame(frame) def _get_distance_to_goal(self, measurements, target): current_x = measurements.player_measurements.transform.location.x current_y = measurements.player_measurements.transform.location.y distance_to_goal = np.linalg.norm(np.array([current_x, current_y]) - \ np.array([target.location.x, target.location.y])) return distance_to_goal def _new_episode(self): experiment_idx = np.random.randint(0, len(self._experiments)) experiment = self._experiments[experiment_idx] exp_settings = experiment.conditions exp_settings.set(QualityLevel='Low') positions = self._client.load_settings(exp_settings).player_start_spots idx_pose = np.random.randint(0, len(experiment.poses)) pose = experiment.poses[idx_pose] self.logger.info('Env {} gets experiment {} with pose {}'.format( self.id, experiment_idx, idx_pose)) start_index = pose[0] end_index = pose[1] self._client.start_episode(start_index) self._time_out = self._experiment_suite.calculate_time_out( self._get_shortest_path(positions[start_index], positions[end_index])) self._target = positions[end_index] self._episode_name = str(experiment.Conditions.WeatherId) + '_' \ + str(experiment.task) + '_' + str(start_index) \ + '_' + str(end_index) if ((self.num_episodes % self.video_every) == 0) and (self.id == 0): video_path = os.path.join( self.video_dir, '{:08d}_'.format(self.num_episodes) + self._episode_name + '.mp4') self.logger.info('Writing video at {}'.format(video_path)) self.video_writer = skvideo.io.FFmpegWriter( video_path, inputdict={'-r': '30'}, outputdict={'-r': '30'}) else: self.video_writer = None self.num_episodes += 1 def _new_episode_benchmark(self): experiment_idx_past = self.benchmark_index[0] pose_idx_past = self.benchmark_index[1] repetition_idx_past = self.benchmark_index[2] experiment_past = self._experiments[experiment_idx_past] poses_past = experiment_past.poses[0:] repetition_past = experiment_past.repetitions if repetition_idx_past == repetition_past: if pose_idx_past == len(poses_past) - 1: if experiment_idx_past == len(self._experiments) - 1: return False else: experiment = self._experiments[experiment_idx_past + 1] pose = experiment.poses[0:][0] self.benchmark_index = [experiment_idx_past + 1, 0, 1] else: experiment = experiment_past pose = poses_past[pose_idx_past + 1] self.benchmark_index = [ experiment_idx_past, pose_idx_past + 1, 1 ] else: experiment = experiment_past pose = poses_past[pose_idx_past] self.benchmark_index = [ experiment_idx_past, pose_idx_past, repetition_idx_past + 1 ] exp_settings = experiment.Conditions exp_settings.set(QualityLevel='Low') positions = self._client.load_settings(exp_settings).player_start_spots start_index = pose[0] end_index = pose[1] self._client.start_episode(start_index) self._time_out = self._experiment_suite.calculate_time_out( self._get_shortest_path(positions[start_index], positions[end_index])) self._target = positions[end_index] self._episode_name = str(experiment.Conditions.WeatherId) + '_' \ + str(experiment.task) + '_' + str(start_index) \ + '_' + str(end_index) if ((self.num_episodes % self.video_every) == 0) and (self.id == 0): video_path = os.path.join( self.video_dir, '{:08d}_'.format(self.num_episodes) + self._episode_name + '.mp4') self.logger.info('Writing video at {}'.format(video_path)) self.video_writer = skvideo.io.FFmpegWriter( video_path, inputdict={'-r': '30'}, outputdict={'-r': '30'}) else: self.video_writer = None self.num_episodes += 1 def _get_directions(self, current_point, end_point): directions = self._planner.get_next_command( (current_point.location.x, current_point.location.y, 0.22), (current_point.orientation.x, current_point.orientation.y, current_point.orientation.z), (end_point.location.x, end_point.location.y, 0.22), (end_point.orientation.x, end_point.orientation.y, end_point.orientation.z)) return directions def _get_shortest_path(self, start_point, end_point): return self._planner.get_shortest_path_distance( [start_point.location.x, start_point.location.y, 0.22], [start_point.orientation.x, start_point.orientation.y, 0.22], [end_point.location.x, end_point.location.y, end_point.location.z], [ end_point.orientation.x, end_point.orientation.y, end_point.orientation.z ]) @staticmethod def _is_collision(measurements): c = 0 c += measurements.player_measurements.collision_vehicles c += measurements.player_measurements.collision_pedestrians c += measurements.player_measurements.collision_other sidewalk_intersection = measurements.player_measurements.intersection_offroad otherlane_intersection = measurements.player_measurements.intersection_otherlane return (c > 1e-9) or (sidewalk_intersection > 0.01) or (otherlane_intersection > 0.9), c def _make_carla_client(self, host, port): while True: try: self.logger.info( "Trying to make client on port {}".format(port)) self._client = CarlaClient(host, port, timeout=100) self._client.connect() self._client.load_settings(CarlaSettings(QualityLevel='Low')) self._client.start_episode(0) self.logger.info( "Successfully made client on port {}".format(port)) break except TCPConnectionError as error: self.logger.debug('Got TCPConnectionError..sleeping for 1') self.logger.error(error) time.sleep(1)
class CarlaEnv(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): """ 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