ray/examples/carla/env.py

from __future__ import absolute_import
from __future__ import division
from __future__ import print_function

from datetime import datetime
import cv2
import os
import json
import random
import signal
import subprocess
import time
import traceback

import numpy as np
try:
    import scipy.misc
except Exception:
    pass

from carla.client import CarlaClient
from carla.sensor import Camera
from carla.settings import CarlaSettings

import gym
from gym.spaces import Box, Discrete


# Set this where you want to save image outputs (or empty string to disable)
CARLA_OUT_PATH = os.environ.get("CARLA_OUT", os.path.expanduser("~/carla_out"))
if CARLA_OUT_PATH and not os.path.exists(CARLA_OUT_PATH):
    os.makedirs(CARLA_OUT_PATH)

# Set this to the path of your Carla binary
SERVER_BINARY = os.environ.get(
    "CARLA_SERVER", "/home/ubuntu/carla-0.7/CarlaUE4.sh")

# Number of retries if the server doesn't respond
RETRIES_ON_ERROR = 5

# Default environment configuration
ENV_CONFIG = {
    "verbose": True,
    "render_x_res": 400,
    "render_y_res": 300,
    "x_res": 80,
    "y_res": 80,
    "map": "/Game/Maps/Town02",
    "random_starting_location": False,
    "use_depth_camera": False,
    "discrete_actions": False,
    "max_steps": 50,
    "num_vehicles": 20,
    "num_pedestrians": 40,
    "weather": [1],  # [1, 3, 7, 8, 14]

    # Defaults to driving down the road /Game/Maps/Town02, start pos 0
    "target_x": -7.5,
    "target_y": 120,
}


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


def distance(x1, y1, x2, y2):
    return ((x1 - x2)**2 + (y1 - y2)**2)**0.5


def compute_reward(config, prev, current):
    prev_x = prev["x"] / 100  # cm -> m
    prev_y = prev["y"] / 100
    cur_x = current["x"] / 100  # cm -> m
    cur_y = current["y"] / 100

    reward = 0.0
    done = False

    # Distance travelled toward the goal in m
    reward += (
        distance(prev_x, prev_y, config["target_x"], config["target_y"]) -
        distance(cur_x, cur_y, config["target_x"], config["target_y"]))

    # Change in speed (km/h)
    reward += 0.05 * (current["forward_speed"] - prev["forward_speed"])

    # New collision damage
    reward -= .00002 * (
        current["collision_vehicles"] + current["collision_pedestrians"] +
        current["collision_other"] - prev["collision_vehicles"] -
        prev["collision_pedestrians"] - prev["collision_other"])

    # New sidewalk intersection
    reward -= 2 * (
        current["intersection_offroad"] - prev["intersection_offroad"])

    # New opposite lane intersection
    reward -= 2 * (
        current["intersection_otherlane"] - prev["intersection_otherlane"])

    if distance(cur_x, cur_y, config["target_x"], config["target_y"]) < 10:
        done = True

    return reward, done


def print_measurements(measurements):
    number_of_agents = len(measurements.non_player_agents)
    player_measurements = measurements.player_measurements
    message = 'Vehicle at ({pos_x:.1f}, {pos_y:.1f}), '
    message += '{speed:.2f} km/h, '
    message += 'Collision: {{vehicles={col_cars:.0f}, '
    message += 'pedestrians={col_ped:.0f}, other={col_other:.0f}}}, '
    message += '{other_lane:.0f}% other lane, {offroad:.0f}% off-road, '
    message += '({agents_num:d} non-player agents in the scene)'
    message = message.format(
        pos_x=player_measurements.transform.location.x / 100,  # cm -> m
        pos_y=player_measurements.transform.location.y / 100,
        speed=player_measurements.forward_speed,
        col_cars=player_measurements.collision_vehicles,
        col_ped=player_measurements.collision_pedestrians,
        col_other=player_measurements.collision_other,
        other_lane=100 * player_measurements.intersection_otherlane,
        offroad=100 * player_measurements.intersection_offroad,
        agents_num=number_of_agents)
    print(message)


if __name__ == '__main__':
    env = CarlaEnv()
    obs = env.reset()
    print("reset", obs)
    start = time.time()
    done = False
    i = 0
    total_reward = 0.0
    while not done:
        i += 1
        if ENV_CONFIG["discrete_actions"]:
            obs, reward, done, info = env.step(1)
        else:
            obs, reward, done, info = env.step([0, 1, 0])
        total_reward += reward
        print(
            i, "obs", obs.shape, "rew", reward, "total", total_reward,
            "done", done)
    print("{} fps".format(100 / (time.time() - start)))