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