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=90.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=10", "-world-port={}".format(self.port),
"-windowed -ResX={} -ResY={}".format(
carla_config.server_width,
carla_config.server_height), "-carla-no-hud"
]
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 duh by reset() 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 CarlaEnvironmentWrapper(EnvironmentWrapper):
def __init__(self,
num_speedup_steps=10,
require_explicit_reset=True,
is_render_enabled=False,
automatic_render=False,
early_termination_enabled=False,
run_offscreen=False,
cameras=['SceneFinal'],
save_screens=False,
carla_settings=None,
carla_server_settings=None,
location_indices=(9, 16)):
EnvironmentWrapper.__init__(self, is_render_enabled, save_screens)
self.automatic_render = automatic_render
self.episode_max_time = 100000 # miliseconds for each episode
self.allow_braking = True
self.log_path = "carla_logs.txt"
self.verbose = True
self.observation = None
self.num_speedup_steps = num_speedup_steps
self.counter = 0
self.rgb_camera_name = 'CameraRGB'
self.rgb_camera = 'SceneFinal' in cameras
self.is_game_ready_for_input = False
self.run_offscreen = run_offscreen
self.kill_when_connection_lost = True
# server configuration
self.carla_server_settings = carla_server_settings
self.port = get_open_port()
self.host = 'localhost'
# Why town2: https://github.com/carla-simulator/carla/issues/10#issuecomment-342483829
self.level = 'town2'
self.map = CarlaLevel().get(self.level)
# client configuration
if type(carla_settings) == str:
# load settings from file
with open(carla_settings, 'r') as fp:
self.settings = fp.read()
elif type(carla_settings) == CarlaSettings:
self.settings = carla_settings
carla_settings = None
elif carla_settings == None:
raise Exception(
"Please load a CarlaSettings object or provide a path to a settings file.")
self.car_speed = 0.
self.max_speed = 10. #m/s
# Will be true only when setup_client_and_server() is called, either explicitly, or by reset()
self.is_game_setup = False
# measurements
self.autopilot = None
self.kill_if_unmoved_for_n_steps = 50*4
self.unmoved_steps = 0.0
self.early_termination_enabled = early_termination_enabled
if self.early_termination_enabled:
self.max_neg_steps = 100*4
self.cur_neg_steps = 0
self.early_termination_punishment = 20.0
# env initialization
if not require_explicit_reset:
self.reset(True)
# camera-view renders
if self.automatic_render:
self.init_renderer()
if self.save_screens:
create_dir(self.images_path)
self.rgb_img_path = self.images_path+"/rgb/"
# locations indices
self.start_loc_idx = location_indices[0]
self.end_loc_idx = location_indices[1]
self.end_loc = None
self.last_distance = 0
self.curr_distance = 0
self.end_loc_measurement = None
def setup_client_and_server(self, reconnect_client_only=False, settings=None):
# open the server
if not reconnect_client_only:
self.server = self._open_server()
self.server_pid = self.server.pid # To kill incase child process gets lost
# open the client
self.game = CarlaClient(self.host, self.port, timeout=99999999)
# It's taking a very long time for the server process to spawn, so the client needs to wait or try sufficient no. of times lol
self.game.connect(connection_attempts=100)
self.scene = self.game.load_settings(self.settings)
# get available start positions
positions = self.scene.player_start_spots
start_loc = positions[self.start_loc_idx].location
end_loc = positions[self.end_loc_idx].location
self.end_loc = end_loc
self.last_distance = self.cal_distance(start_loc, end_loc)
self.curr_distance = self.last_distance
self.end_loc_measurement = [end_loc.x, end_loc.y, end_loc.z]
# self.num_pos = len(positions)
# self.iterator_start_positions = 0
self.is_game_setup = self.server and self.game
return
def close_client_and_server(self):
self._close_server()
print("\t Disconnecting the client")
self.game.disconnect()
self.game = None
self.server = None
self.is_game_setup = False
return
def _open_server(self):
# Note: There is no way to disable rendering in CARLA as of now
# https://github.com/carla-simulator/carla/issues/286
# decrease the window resolution if you want to see if performance increases
# Command: $CARLA_ROOT/CarlaUE4.sh /Game/Maps/Town02 -benchmark -carla-server -fps=15 -world-port=9876 -windowed -ResX=480 -ResY=360 -carla-no-hud
# To run off_screen: SDL_VIDEODRIVER=offscreen SDL_HINT_CUDA_DEVICE=0 <command> #https://github.com/carla-simulator/carla/issues/225
my_env = None
if self.run_offscreen:
# my_env = {**os.environ, 'SDL_VIDEODRIVER': 'offscreen', 'SDL_HINT_CUDA_DEVICE':'0'}
pass
with open(self.log_path, "wb") as out:
cmd = [path.join(environ.get('CARLA_ROOT'), 'CarlaUE4.sh'),
self.map,
"-benchmark", "-carla-server", "-fps=10", "-world-port={}".format(
self.port),
"-windowed -ResX={} -ResY={}".format(
carla_config.server_width, carla_config.server_height),
"-carla-no-hud"]
if self.carla_server_settings:
cmd.append("-carla-settings={}".format(self.carla_server_settings))
p = subprocess.Popen(cmd, stdout=out, stderr=out, env=my_env)
return p
def _close_server(self):
if self.kill_when_connection_lost:
os.killpg(os.getpgid(self.server.pid), signal.SIGKILL)
return
no_of_attempts = 0
while is_process_alive(self.server_pid):
print("Trying to close Carla server with pid %d" % self.server_pid)
if no_of_attempts < 5:
self.server.terminate()
elif no_of_attempts < 10:
self.server.kill()
elif no_of_attempts < 15:
os.kill(self.server_pid, signal.SIGTERM)
else:
os.kill(self.server_pid, signal.SIGKILL)
time.sleep(10)
no_of_attempts += 1
def check_early_stop(self, player_measurements, immediate_reward):
# if player_measurements.intersection_offroad > 0.75 or \
if immediate_reward < 0 :
# or \
# (self.control.throttle == 0.0 and player_measurements.forward_speed < 0.1 and self.control.brake != 0.0):
self.cur_neg_steps += 1
# print("updated neg steps:{}".format(self.cur_neg_steps))
early_done = (self.cur_neg_steps > self.max_neg_steps)
if early_done:
print("\t Early terminate. neg steps:{}, unmoved:{}".format(self.cur_neg_steps, self.unmoved_steps))
return early_done, self.early_termination_punishment
else:
self.cur_neg_steps = self.cur_neg_steps - 1 if self.cur_neg_steps > 0 else 0 # decay
return False, 0.0
def _update_state(self):
# get measurements and observations
try:
measurements, sensor_data = self.game.read_data()
except:
# Connection between cli and server lost; reconnect
if self.kill_when_connection_lost:
raise
print("\t Connection to server lost while reading state. Reconnecting...")
self.close_client_and_server()
self.setup_client_and_server(reconnect_client_only=False)
self.done = True
self.location = [measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z]
self.last_distance = self.curr_distance
self.curr_distance = self.cal_distance(
measurements.player_measurements.transform.location, self.end_loc)
is_collision = measurements.player_measurements.collision_vehicles != 0 \
or measurements.player_measurements.collision_pedestrians != 0 \
or measurements.player_measurements.collision_other != 0
# CARLA doesn't recognize if collision occured and colliding speed is less than 5 km/h (Around 0.7 m/s)
# Ref: https://github.com/carla-simulator/carla/issues/13
# Recognize that as a collision
self.car_speed = measurements.player_measurements.forward_speed
if self.control.throttle > 0.25 and self.car_speed < 0.7 and self.is_game_ready_for_input:
self.unmoved_steps += 1
# print("updated unmoved(carla bug) steps:{}".format(self.unmoved_steps), self.control.throttle, self.car_speed, self.is_game_ready_for_input)
if self.unmoved_steps > self.kill_if_unmoved_for_n_steps:
is_collision = True
print("\t Car stuck somewhere. neg steps:{}, unmoved:{}".format(self.cur_neg_steps, self.unmoved_steps))
# elif self.unmoved_steps > 0:
# # decay slowly, since it may be stuck and not accelerate few times
# self.unmoved_steps -= 0.50
if is_collision:
print("\t Collision occured.")
# Reward Shaping:
speed_reward = self.car_speed
if speed_reward > 30.:
speed_reward = 30.
self.reward = speed_reward * 5 \
- (measurements.player_measurements.intersection_otherlane * 1.5) \
- (measurements.player_measurements.intersection_offroad * 1.5) \
- is_collision * 10 \
- np.abs(self.control.steer) * 1.5 \
+ (self.last_distance - self.curr_distance)*10
# Scale down the reward by a factor
# self.reward /= 10
# print("reward: {} = {} - ({} * 1.5) - ({} * 1.5) - {} * 10 - np.abs({}) * 10 + ({} - {})*10".format(self.reward,
# speed_reward,
# measurements.player_measurements.intersection_otherlane,
# measurements.player_measurements.intersection_offroad,
# is_collision,
# self.control.steer,
# self.last_distance,
# self.curr_distance))
if self.early_termination_enabled:
early_done, punishment = self.check_early_stop(
measurements.player_measurements, self.reward)
if early_done:
self.done = True
self.reward -= punishment
# update measurements
self.observation = np.hstack(self.location + self.end_loc_measurement + [measurements.player_measurements.acceleration.x,
measurements.player_measurements.acceleration.y,
measurements.player_measurements.acceleration.z,
measurements.player_measurements.forward_speed])
if self.rgb_camera:
img_data = sensor_data[self.rgb_camera_name].data/255.
self.observation = [self.observation, img_data]
# self.counter +=1
# self.observation = np.array(
# [np.array([self.counter - 1]), np.array([self.counter, self.counter + 1])])
# print(self.observation.shape)
self.autopilot = measurements.player_measurements.autopilot_control
if (measurements.game_timestamp >= self.episode_max_time) or is_collision:
# print('EPISODE IS DONE. GameTime: {}, Collision: {}'.format(str(measurements.game_timestamp),
# str(is_collision)))
self.done = True
def _take_action(self, action):
if not self.is_game_setup:
print("\t Reset the environment by reset() before calling step()")
sys.exit(1)
# assert len(actions) == 2, "Send actions in the format [steer, accelaration]"
self.control = VehicleControl()
self.control.steer = np.clip(action[0], -1, 1)
self.control.throttle = np.clip(action[1], 0, 1)
self.control.brake = np.abs(np.clip(action[1], -1, 0))
self.control.hand_brake = False
self.control.reverse = False
# prevent braking
if not self.allow_braking or self.control.brake < 0.1 or self.control.throttle > self.control.brake:
self.control.brake = 0
if self.car_speed > self.max_speed and self.control.brake == 0:
self.control.throttle = 0.0
controls_sent = False
while not controls_sent:
try:
self.game.send_control(self.control)
controls_sent = True
# print("controls sent!")
except Exception:
traceback.print_exc()
if self.kill_when_connection_lost:
print("\t Connection to server lost while sending controls. Reconnecting...")
self.close_client_and_server()
self.setup_client_and_server(reconnect_client_only=False)
self.done = True
def _restart_environment_episode(self, force_environment_reset=True, settings=None):
if not force_environment_reset and not self.done and self.is_game_setup:
print("Can't reset dude, episode ain't over yet")
return None # User should handle this
self.is_game_ready_for_input = False
if not self.is_game_setup:
self.setup_client_and_server()
# if self.is_render_enabled:
# self.init_renderer()
else:
# get settings. Only needed if we want random rollouts
if type(settings) == str:
# load settings from file
with open(settings, 'r') as fp:
self.settings = fp.read()
self.scene = self.game.load_settings(self.settings)
elif type(settings) == CarlaSettings:
self.settings = settings
self.scene = self.game.load_settings(self.settings)
elif settings == None:
pass # already set during initialization
# self.iterator_start_positions += 1
# if self.iterator_start_positions >= self.num_pos:
# self.iterator_start_positions = 0
try:
self.game.start_episode(self.start_loc_idx)
except:
self.game.connect()
self.game.start_episode(self.start_loc_idx)
self.unmoved_steps = 0.0
self.cur_neg_steps = 0
self.car_speed = 0
# start the game with some initial speed
observation = None
for i in range(self.num_speedup_steps):
observation, reward, done, _ = self.step([0, 0.25])
self.observation = observation
self.is_game_ready_for_input = True
return observation
def init_renderer(self):
self.num_cameras = 0
if self.rgb_camera:
self.num_cameras += 1
self.renderer.create_screen(
carla_config.render_width, carla_config.render_height*self.num_cameras)
def get_rendered_image(self):
temp = []
if self.rgb_camera:
temp.append(self.observation['rgb_image'])
return np.vstack((img for img in temp))
def save_screenshots(self):
if not self.save_screens:
print("save_screens is set False")
return
filename = str(int(time.time()*100))
if self.rgb_camera:
save_image(self.rgb_img_path+filename+".png",
self.observation['rgb_image'])
def cal_distance(self, start, end):
return ((end.x - start.x)**2 + (end.y - start.y)**2 + (end.z - start.z)**2)**0.5
class CarlaClientConsole(cmd.Cmd):
def __init__(self, args):
cmd.Cmd.__init__(self)
self.args = args
self.prompt = '\x1b[1;34m%s\x1b[0m ' % '(carla)'
self.client = CarlaClient(args.host, args.port)
self.settings = get_default_carla_settings(args)
self.print_measurements = False
self.control = _Control()
self.thread = threading.Thread(target=self._agent_thread_worker)
self.done = False
self.thread.start()
def cleanup(self):
self.do_disconnect()
self.done = True
if self.thread.is_alive():
self.thread.join()
def default(self, line):
logging.error('unknown command \'%s\'!', line)
def emptyline(self):
pass
def precmd(self, line):
return line.strip().lower()
def can_exit(self):
return True
def do_exit(self, line=None):
"""Exit the console."""
return True
def do_eof(self, line=None):
"""Exit the console."""
print('exit')
return self.do_exit(line)
def help_help(self):
print('usage: help [topic]')
def do_disconnect(self, line=None):
"""Disconnect from the server."""
self.client.disconnect()
def do_new_episode(self, line=None):
"""Request a new episode. Connect to the server if not connected."""
try:
self.control = _Control()
if not self.client.connected():
self.client.connect()
self.client.load_settings(self.settings)
self.client.start_episode(0)
logging.info('new episode started')
except Exception as exception:
logging.error(exception)
def do_control(self, line=None):
"""Set control message:\nusage: control [reset|<param> <value>]\n(e.g., control throttle 0.5)"""
try:
if line == 'reset':
self.control = _Control()
else:
self.control.set(line)
logging.debug('control: %s', self.control.kwargs())
except Exception as exception:
logging.error(exception)
def complete_control(self, text, *args, **kwargs):
options = self.control.action_list()
options.append('reset')
return [x + ' ' for x in options if x.startswith(text)]
def do_settings(self, line=None):
"""Open a text editor to edit CARLA settings."""
result = edit_text(self.settings)
if result is not None:
self.settings = result
def do_print_measurements(self, line):
"""Print received measurements to console.\nusage: print_measurements [t/f]"""
self.print_measurements = True if not line else _Control._parse(bool, line)
def _agent_thread_worker(self):
filename = '_images/console/camera_{:0>3d}/image_{:0>8d}.png'
while not self.done:
try:
measurements, sensor_data = self.client.read_data()
if self.print_measurements:
print(measurements)
if self.args.images_to_disk:
images = [x for x in sensor_data.values() if isinstance(x, Image)]
for n, image in enumerate(images):
path = filename.format(n, measurements.game_timestamp)
image.save_to_disk(path)
self.client.send_control(**self.control.kwargs())
except Exception as exception:
# logging.exception(exception)
time.sleep(1)
class 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", "-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(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)
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)) or
(self.config["early_terminate_on_bounds"]
and bounds_done(self.config["early_terminate_bounds_limit"],
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)
try:
subprocess.call(ffmpeg_cmd, shell=True, timeout=20)
except Exception:
print("FFMPEG EXPIRED")
def preprocess_image(self, image):
if self.config["use_depth_camera"]:
assert self.config["use_depth_camera"]
data = (image.data - 0.5) * 2
data = data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 1)
data = cv2.resize(data,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = np.expand_dims(data, 2)
else:
data = image.data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 3)
data = cv2.resize(data,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = (data.astype(np.float32) - 128) / 128
return data
def _read_observation(self):
# Read the data produced by the server this frame.
measurements, sensor_data = self.client.read_data()
# Print some of the measurements.
if self.config["verbose"]:
print_measurements(measurements)
observation = None
if self.config["use_depth_camera"]:
camera_name = "CameraDepth"
else:
camera_name = "CameraRGB"
for name, image in sensor_data.items():
if name == camera_name:
observation = image
cur = measurements.player_measurements
if self.config["enable_planner"]:
next_command = COMMANDS_ENUM[self.planner.get_next_command(
[cur.transform.location.x, cur.transform.location.y, GROUND_Z],
[
cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z
],
[self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [
self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z
])]
else:
next_command = "LANE_FOLLOW"
if next_command == "REACH_GOAL":
distance_to_goal = 0.0 # avoids crash in planner
elif self.config["enable_planner"]:
distance_to_goal = self.planner.get_shortest_path_distance([
cur.transform.location.x, cur.transform.location.y, GROUND_Z
], [
cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z
], [self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [
self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z
]) / 100
else:
distance_to_goal = -1
distance_to_goal_euclidean = float(
np.linalg.norm([
cur.transform.location.x - self.end_pos.location.x,
cur.transform.location.y - self.end_pos.location.y
]) / 100)
py_measurements = {
"episode_id": self.episode_id,
"step": self.num_steps,
"x": cur.transform.location.x,
"y": cur.transform.location.y,
"x_orient": cur.transform.orientation.x,
"y_orient": cur.transform.orientation.y,
"forward_speed": cur.forward_speed,
"distance_to_goal": distance_to_goal,
"distance_to_goal_euclidean": distance_to_goal_euclidean,
"collision_vehicles": cur.collision_vehicles,
"collision_pedestrians": cur.collision_pedestrians,
"collision_other": cur.collision_other,
"intersection_offroad": cur.intersection_offroad,
"intersection_otherlane": cur.intersection_otherlane,
"weather": self.weather,
"map": self.config["server_map"],
"start_coord": self.start_coord,
"end_coord": self.end_coord,
"current_scenario": self.scenario,
"x_res": self.config["x_res"],
"y_res": self.config["y_res"],
"num_vehicles": self.scenario["num_vehicles"],
"num_pedestrians": self.scenario["num_pedestrians"],
"max_steps": self.scenario["max_steps"],
"next_command": next_command,
}
if CARLA_OUT_PATH and self.config["log_images"]:
for name, image in sensor_data.items():
out_dir = os.path.join(CARLA_OUT_PATH, name)
if not os.path.exists(out_dir):
os.makedirs(out_dir)
out_file = os.path.join(
out_dir, "{}_{:>04}.jpg".format(self.episode_id,
self.num_steps))
scipy.misc.imsave(out_file, image.data)
assert observation is not None, sensor_data
return observation, py_measurements
class CarlaEnv(gym.Env):
def __init__(self,
host="localhost",
num_vehicles=1,
vehicles_seed=lambda: 200,
player_starts=2,
goals=0):
self.num_vehicles = num_vehicles
self.vehicles_seed = vehicles_seed
self.starts = list_from_scalar(player_starts)
self.goals = list_from_scalar(goals)
self.port = get_open_port()
self.host = host
self.metadata = {
'render.modes': ['rgb_array'],
#'video.frames_per_second': int(np.round(1.0 / self.dt))
}
self.server = self.open_server()
print(f"Connecting to CARLA Client on port {self.port}")
self.client = CarlaClient(self.host, self.port, timeout=99999999)
time.sleep(3)
self.client.connect(connection_attempts=1000)
print(f"Connected on port: {self.port}")
self.action_space = gym.spaces.Box(
-2,
2, (len(self._map_controls([1, 2, 3, 4, 5]).items()), ),
dtype=np.float32)
obs_size = len(self.reset())
self.observation_space = gym.spaces.Box(-float("inf"),
float("inf"), (obs_size, ),
dtype=np.float32)
def dist_from_goal(self, measurements):
x = array_from_loc(measurements.player_measurements.transform.location)
y = array_from_loc(self.scene.player_start_spots[self.goal].location)
return np.sqrt(np.sum((x - y)**2))
def open_server(self):
#os.devnull
log_file = f"./logs/carla_logs/carla_{self.port}.txt"
os.makedirs(os.path.dirname(log_file), exist_ok=True)
with open(log_file, "wb+") as out:
"""
true_script_name = os.path.join(CARLA_PATH, 'CarlaUE4.sh')
if os.path.islink(true_script_name):
true_script_name = os.readlink(execute_path)
project_root = os.path.dirname(true_script_name)
execute_path = f"{project_root}/CarlaUE4/Binaries/Linux/CarlaUE4"
# chmod +x
st = os.stat(execute_path)
os.chmod(execute_path, st.st_mode | stat.S_IEXEC)
"""
cmd = [
os.path.join(CARLA_PATH,
'CarlaUE4.sh'), #execute_path, "CarlaUE4",
"-carla-server",
"-fps=60",
f"-world-port={self.port}",
f"-windowed -ResX={500} -ResY={500}",
"-carla-no-hud"
]
# - benchmark
p = subprocess.Popen(cmd,
stdout=out,
stderr=out,
stdin=subprocess.PIPE,
preexec_fn=os.setpgrp)
return p
def close_server(self):
#self.server.terminate()
pid = self.server.pid
no_of_attempts = 0
def is_process_alive(pid):
## Source: https://stackoverflow.com/questions/568271/how-to-check-if-there-exists-a-process-with-a-given-pid-in-python
try:
os.kill(pid, 0)
except OSError:
return False
return True
while is_process_alive(pid):
pgroup = os.getpgid(self.server.pid)
self.server.terminate()
os.killpg(pgroup, signal.SIGTERM)
_, _ = os.waitpid(pid, os.WNOHANG)
time.sleep(5)
def _add_settings(self):
self.settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self.num_vehicles,
NumberOfPedestrians=0,
WeatherId=1,
QualityLevel="Low",
SeedVehicles=self.vehicles_seed())
#settings.randomize_seeds()
def _add_sensors(self):
camera0 = Camera('RenderCamera0')
# Set image resolution in pixels.
camera0.set_image_size(800, 600)
# Set its position relative to the car in meters.
camera0.set_position(-4.30, 0, 2.60)
camera0.set_rotation(pitch=-25, yaw=0, roll=0)
self.settings.add_sensor(camera0)
def _map_controls(self, a):
return dict(steer=a[0], throttle=a[1])
def _process_observation(self, measurements, sensor_data):
return array_from_measurements(measurements)
def _get_reward_and_termination(self):
measurements, sensor_data = self.current_state
collision_penalty = (
measurements.player_measurements.collision_vehicles +
measurements.player_measurements.collision_other)
is_collided = collision_penalty > 2
offroad_penalty = measurements.player_measurements.intersection_offroad
is_offroad = offroad_penalty > 0.5
dist_from_goal = self.dist_from_goal(measurements)
is_at_target = dist_from_goal < 5
is_done = is_collided or is_at_target or is_offroad
distance_reward = 300 - dist_from_goal
collision_cost = collision_penalty / 300
offroad_cost = (offroad_penalty * 10)
reward = distance_reward - collision_cost - offroad_cost
reward = reward / 20
return reward, is_done
def get_new_start_goal(self):
start = np.random.choice(self.starts)
goal = np.random.choice(self.goals)
while goal == start:
start = np.random.choice(self.starts)
goal = np.random.choice(self.goals)
return start, goal
def reset(self):
self.settings = CarlaSettings()
self._add_settings()
self._add_sensors()
self.scene = self.client.load_settings(self.settings)
start, goal = self.get_new_start_goal()
self.goal = goal
for i in range(100):
try:
self.client.start_episode(start)
self.current_state = self.client.read_data()
if i > 0:
print("Reconnected.")
break
except carla.tcp.TCPConnectionError:
if i % 10 == 0:
print(f"There was a TCP Error (Attempt {i}). Retrying. ")
time.sleep(3)
measurements, sensor_data = self.current_state
return self._process_observation(measurements, sensor_data)
def step(self, a):
control = self._map_controls(a)
for i in range(100):
try:
self.client.send_control(**control)
break
except carla.tcp.TCPConnectionError:
time.sleep(0.5)
self.current_state = self.client.read_data()
measurements, sensor_data = self.current_state
reward, is_done = self._get_reward_and_termination()
obs = self._process_observation(measurements, sensor_data)
return obs, reward, is_done, {}
def render(self, mode='human', **kwargs):
if mode == 'rgb_array':
return np.concatenate([
sensor.data for name, sensor in self.current_state[1].items()
if "Render" in name
],
axis=1)
super().render(mode=mode, **kwargs)
def _close(self):
self.close()
def close(self):
print(
f"Disconnecting from CARLA Client (port: {self.port}, pid: {self.server.pid})"
)
self.close_server()
time.sleep(3)
while self.client.connected():
self.client.disconnect()
print(
f"Disconnected from CARLA Client (port: {self.port}, pid: {self.server.pid})."
)
class CarlaClientConsole(cmd.Cmd):
def __init__(self, args):
cmd.Cmd.__init__(self)
self.args = args
self.prompt = '\x1b[1;34m%s\x1b[0m ' % '(carla)'
self.client = CarlaClient(args.host, args.port)
self.settings = get_default_carla_settings(args)
self.print_measurements = False
self.control = _Control()
self.thread = threading.Thread(target=self._agent_thread_worker)
self.done = False
self.thread.start()
def cleanup(self):
self.do_disconnect()
self.done = True
if self.thread.is_alive():
self.thread.join()
def default(self, line):
logging.error('unknown command \'%s\'!', line)
def emptyline(self):
pass
def precmd(self, line):
return line.strip().lower()
def can_exit(self):
return True
def do_exit(self, line=None):
"""Exit the console."""
return True
def do_eof(self, line=None):
"""Exit the console."""
print('exit')
return self.do_exit(line)
def help_help(self):
print('usage: help [topic]')
def do_disconnect(self, line=None):
"""Disconnect from the server."""
self.client.disconnect()
def do_new_episode(self, line=None):
"""Request a new episode. Connect to the server if not connected."""
try:
self.control = _Control()
if not self.client.connected():
self.client.connect()
self.client.load_settings(self.settings)
self.client.start_episode(0)
logging.info('new episode started')
except Exception as exception:
logging.error(exception)
def do_control(self, line=None):
"""Set control message:\nusage: control [reset|<param> <value>]\n(e.g., control throttle 0.5)"""
try:
if line == 'reset':
self.control = _Control()
else:
self.control.set(line)
logging.debug('control: %s', self.control.kwargs())
except Exception as exception:
logging.error(exception)
def complete_control(self, text, *args, **kwargs):
options = self.control.action_list()
options.append('reset')
return [x + ' ' for x in options if x.startswith(text)]
def do_settings(self, line=None):
"""Open a text editor to edit CARLA settings."""
result = edit_text(self.settings)
if result is not None:
self.settings = result
def do_print_measurements(self, line):
"""Print received measurements to console.\nusage: print_measurements [t/f]"""
self.print_measurements = True if not line else _Control._parse(bool, line)
def _agent_thread_worker(self):
filename = '_images/console/camera_{:0>3d}/image_{:0>8d}.png'
while not self.done:
try:
measurements, sensor_data = self.client.read_data()
if self.print_measurements:
print(measurements)
if self.args.images_to_disk:
images = [x for x in sensor_data.values() if isinstance(x, Image)]
for n, image in enumerate(images):
path = filename.format(n, measurements.game_timestamp)
image.save_to_disk(path)
self.client.send_control(**self.control.kwargs())
except Exception as exception:
# logging.exception(exception)
time.sleep(1)
class CarlaEnv(gym.Env):
metadata = {'render.modes': ['human', 'rgb_array']}
reward_range = (-np.inf, np.inf)
spec = None
action_space = None
observation_space = None
def __init__(self, target=(158.08, 27.18)):
# action space range: steer, throttle, brake
min_steer, max_steer = -1, 1
min_throttle, max_throttle = 0, 1
min_brake, max_brake = 0, 1
self.action_space = spaces.Box(low=np.array([min_steer, -max_brake]),
high=np.array([max_steer,
max_throttle]),
dtype=np.float32)
# observation, 3 channel image
self.observation_space = spaces.Box(
low=0,
high=1.0,
shape=(3, carla_config.CARLA_IMG_HEIGHT,
carla_config.CARLA_IMG_WIDTH),
dtype=np.float32)
self.viewer = ImageViewer()
self.rng = np.random.RandomState() # used for random number generators
self.target = np.array(target)
self.cur_image = None # image with (H, W, C)
self.cur_measurements = None
self.carla_client = CarlaClient(carla_config.CARLA_HOST_ADDRESS,
carla_config.CARLA_HOST_PORT,
timeout=100)
self.carla_client.connect()
if self.carla_client.connected():
print("Successfully connected to", end=" ")
else:
print("Failed to connect", end=" ")
print(
f"Carla on {carla_config.CARLA_HOST_ADDRESS}:{carla_config.CARLA_HOST_PORT}"
)
def reset(self):
"""
Resets the state of the environment and returns an initial observation.
:return: observation (object): the initial observation of the space.
"""
# load Carla settings
settings = CarlaSettings()
settings = self._load_settings(settings)
scene = self.carla_client.load_settings(settings)
# define a random starting point of the agent for the appropriate training
number_of_player_starts = len(scene.player_start_spots)
player_start = self.rng.randint(0, max(0, number_of_player_starts - 1))
self.carla_client.start_episode(player_start)
print(f'Starting new episode at {scene.map_name}, {player_start}...')
# read and return status after reset
self.cur_measurements, self.cur_image = self._read_data()
state = self._state(
self.cur_image, self.cur_image
) #possibly revise the state to be some operation of several images.
meas = self.cur_measurements
return state, meas
def render(self, mode='human'):
"""Renders the environment.
:param mode: - human: render to the current display or terminal and
return nothing. Usually for human consumption.
- rgb_array: Return an numpy.ndarray with shape (x, y, 3),
representing RGB values for an x-by-y pixel image, suitable
for turning into a video.
:return:
"""
if mode == 'rgb_array':
return self.cur_image
elif mode == 'human':
self.viewer.imshow(arr=self.cur_image)
return self.viewer.isopen
else:
super(CarlaEnv, self).render(mode=mode) # just raise an exception
def step(self, action):
"""
Run one time step of the environment's dynamics. When end of
episode is reached, you are responsible for calling `reset()`
to reset this environment's state.
Accepts an action and returns a tuple (observation, reward, done, info).
:param action: an action provided by the environment
:return: observation (object): agent's observation of the current environment
reward (float) : amount of reward returned after previous action
done (boolean): whether the episode has ended, in which case further step() calls will return undefined results
info (dict): contains auxiliary diagnostic information (helpful for debugging, and sometimes learning)
"""
if isinstance(action, dict):
self.carla_client.send_control(**action)
elif isinstance(action, np.ndarray): # parse control from array
throttle = max(0, action[1])
brake = max(0, -action[1])
self.carla_client.send_control(steer=action[0],
throttle=throttle,
brake=brake)
else:
self.carla_client.send_control(action)
# read measurements and image from Carla
measurements, image = self._read_data()
# calculate reward
reward = self._reward(self.cur_measurements, measurements)
done = self._done(measurements)
state = self._state(self.cur_image, image)
# save current measurements for next use
self.cur_measurements = measurements
self.cur_image = image
return state, reward, done, measurements
def close(self):
"""
Close connection to Carla
:return:
"""
self.carla_client.disconnect()
self.viewer.close()
def seed(self, seed=None):
"""Set seed for random number generators used in the code
Set seed so that results are consistent in multi runs
:param seed: seed number, defaults to None
"""
self.rng = np.random.RandomState(seed)
def _state(self, pre_image, cur_image):
def image_proc(img):
img = img.transpose(2, 0, 1)
img = np.ascontiguousarray(img, dtype=np.float32) / 255
return img
# pre_image = image_proc(pre_image)
cur_image = image_proc(cur_image)
# CHW, use current image as state
return cur_image
def _load_settings(self, settings):
"""Load Carla settings
Override to customize settings
:param settings: default settings
:return: new settings
"""
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=20,
NumberOfPedestrians=40,
WeatherId=self.rng.choice([1, 3, 7, 8, 14]),
QualityLevel='Low')
# CAMERA
camera0 = Camera('CameraRGB', PostProcessing='SceneFinal')
# Set image resolution in pixels.
camera0.set_image_size(carla_config.CARLA_IMG_HEIGHT,
carla_config.CARLA_IMG_WIDTH)
# Set its position relative to the car in meters.
camera0.set_position(0.30, 0, 1.30)
settings.add_sensor(camera0)
return settings
def _get_sensor_data(self, sensor_data):
"""Extract sensor data from multi sensor dict
Override to customize which sensor (Camera/LiDar) to use
:param sensor_data: multi sensor dict
:type sensor_data: extracted sensor data
"""
return sensor_data['CameraRGB'].data
def _read_data(self, ):
"""
read data from carla
:return: custom measurement data dict, camera image
"""
measurements, sensor_data = self.carla_client.read_data()
p_meas = measurements.player_measurements
pos_x = p_meas.transform.location.x
pos_y = p_meas.transform.location.y
speed = p_meas.forward_speed * 3.6 # m/s -> km/h
col_cars = p_meas.collision_vehicles
col_ped = p_meas.collision_pedestrians
col_other = p_meas.collision_other
other_lane = 100 * p_meas.intersection_otherlane
if other_lane:
print('Intersection into other lane %.2f' % other_lane)
offroad = 100 * p_meas.intersection_offroad
if offroad:
print('offroad %.2f' % offroad)
agents_num = len(measurements.non_player_agents)
distance = np.linalg.norm(np.array([pos_x, pos_y]) - self.target)
meas = {
'pos_x': pos_x,
'pos_y': pos_y,
'speed': speed,
'col_damage': col_cars + col_ped + col_other,
'other_lane': other_lane,
'offroad': offroad,
'agents_num': agents_num,
'distance': distance,
'autopilot_control': p_meas.autopilot_control
}
return meas, self._get_sensor_data(sensor_data)
def _reward(self, pre_measurements, cur_measurements):
"""
Calculate reward
:param pre_measurements: previous measurement
:param cur_measurements: latest measurement
:return: reward
"""
def delta(key):
return cur_measurements[key] - pre_measurements[key]
if pre_measurements is None:
rwd = 0.0
else:
rwd = 0.15 * delta('speed') - 0.002 * delta('col_damage') \
- 2 * delta('offroad') - 2 * delta('other_lane')
return rwd
def _done(self, cur_measurements):
"""
Check done or not
:param cur_measurements: latest measurement
:return:
"""
# check distance to target
d = cur_measurements['distance'] < 1 # final state arrived or not
return d
class CarlaHuman(Driver):
def __init__(self, driver_conf):
Driver.__init__(self)
# some behaviors
self._autopilot = driver_conf.autopilot
self._reset_period = driver_conf.reset_period # those reset period are in the actual system time, not in simulation time
self._goal_reaching_threshold = 3
self.use_planner = driver_conf.use_planner
# we need the planner to find a valid episode, so we initialize one no matter what
self._world = None
self._vehicle = None
self._agent_autopilot = None
self._camera_center = None
self._spectator = None
# (last) images store for several cameras
self._data_buffers = dict()
self.update_once = False
self._collision_events = []
self.collision_sensor = None
if __CARLA_VERSION__ == '0.8.X':
self.planner = Planner(driver_conf.city_name)
else:
self.planner = None
self.use_planner = False
# resources
self._host = driver_conf.host
self._port = driver_conf.port
# various config files
self._driver_conf = driver_conf
self._config_path = driver_conf.carla_config
# some initializations
self._straight_button = False
self._left_button = False
self._right_button = False
self._rear = False
self._recording = False
self._skiped_frames = 20
self._stucked_counter = 0
self._prev_time = datetime.now()
self._episode_t0 = datetime.now()
self._vehicle_prev_location = namedtuple("vehicle", "x y z")
self._vehicle_prev_location.x = 0.0
self._vehicle_prev_location.y = 0.0
self._vehicle_prev_location.z = 0.0
self._camera_left = None
self._camera_right = None
self._camera_center = None
self._actor_list = []
self._sensor_list = []
self._weather_list = [
'ClearNoon', 'CloudyNoon', 'WetNoon', 'WetCloudyNoon',
'MidRainyNoon', 'HardRainNoon', 'SoftRainNoon', 'ClearSunset',
'CloudySunset', 'WetSunset', 'WetCloudySunset', 'MidRainSunset',
'HardRainSunset', 'SoftRainSunset'
]
self._current_weather = 4
self._current_command = 2.0
# steering wheel
self._steering_wheel_flag = True
if self._steering_wheel_flag:
self._is_on_reverse = False
self._control = VehicleControl()
self._parser = SafeConfigParser()
self._parser.read('wheel_config.ini')
self._steer_idx = int(
self._parser.get('G29 Racing Wheel', 'steering_wheel'))
self._throttle_idx = int(
self._parser.get('G29 Racing Wheel', 'throttle'))
self._brake_idx = int(self._parser.get('G29 Racing Wheel',
'brake'))
self._reverse_idx = int(
self._parser.get('G29 Racing Wheel', 'reverse'))
self._handbrake_idx = int(
self._parser.get('G29 Racing Wheel', 'handbrake'))
self.last_timestamp = lambda x: x
self.last_timestamp.elapsed_seconds = 0.0
self.last_timestamp.delta_seconds = 0.2
self.initialize_map(driver_conf.city_name)
def start(self):
if __CARLA_VERSION__ == '0.8.X':
self.carla = CarlaClient(self._host, int(self._port), timeout=120)
self.carla.connect()
else:
self.carla = CarlaClient(self._host, int(self._port))
self.carla.set_timeout(5000)
wd = self.carla.get_world()
self.wd = wd
settings = wd.get_settings()
settings.synchronous_mode = True
wd.apply_settings(settings)
self._reset()
if not self._autopilot:
pygame.joystick.init()
joystick_count = pygame.joystick.get_count()
if joystick_count > 1:
print("Please Connect Just One Joystick")
raise ValueError()
self.joystick = pygame.joystick.Joystick(0)
self.joystick.init()
def test_alive(self):
if not hasattr(self, "wd"):
return False
wd = self.wd
wd.tick()
try:
wd.wait_for_tick(5.0)
except:
return False
return True
def __del__(self):
if hasattr(self, 'carla'):
print("destructing the connection")
if __CARLA_VERSION__ == '0.8.X':
self.carla.disconnect()
else:
alive = self.test_alive()
# destroy old actors
print('destroying actors')
if alive:
if len(self._actor_list) > 0:
for actor in self._actor_list:
actor.destroy()
self._actor_list = []
print('done.')
if self._vehicle is not None:
if alive:
self._vehicle.destroy()
self._vehicle = None
if self._camera_center is not None:
if alive:
self._camera_center.destroy()
self._camera_center = None
if self._camera_left is not None:
if alive:
self._camera_left.destroy()
self._camera_left = None
if self._camera_right is not None:
if alive:
self._camera_right.destroy()
self._camera_right = None
if self.collision_sensor is not None:
if alive:
self.collision_sensor.sensor.destroy()
self.collision_sensor = None
# pygame.quit()
# if self._camera is not None:
# self._camera.destroy()
# self._camera = None
# if self._vehicle is not None:
# self._vehicle.destroy()
# self._vehicle = None
def try_spawn_random_vehicle_at(self,
blueprints,
transform,
auto_drive=True):
blueprint = random.choice(blueprints)
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
vehicle = self._world.try_spawn_actor(blueprint, transform)
if vehicle is not None:
self._actor_list.append(vehicle)
if auto_drive:
# TODO: this won't work in 0.9.5 with Exp_Town
# however, we don't have traffic in that town right now, so we just ignore this
vehicle.set_autopilot()
#print('spawned %r at %s' % (vehicle.type_id, transform.location))
return True
return False
def get_parking_locations(self,
filename,
z_default=0.0,
random_perturb=False):
with open(filename, "r") as f:
lines = f.readlines()
ans = []
for line in lines:
x, y, yaw = [float(v.strip()) for v in line.split(",")]
if random_perturb:
x += np.random.normal(
0, scale=self._driver_conf.extra_explore_location_std)
y += np.random.normal(
0, scale=self._driver_conf.extra_explore_location_std)
yaw += np.random.normal(
0, scale=self._driver_conf.extra_explore_yaw_std)
ans.append(
carla.Transform(location=carla.Location(x=x,
y=y,
z=z_default),
rotation=carla.Rotation(roll=0,
pitch=0,
yaw=yaw)))
return ans
def print_transform(self, t):
print(t.location.x, t.location.y, t.location.z)
print(t.rotation.roll, t.rotation.pitch, t.rotation.yaw)
def _reset(self):
self._episode_t0 = datetime.now()
if __CARLA_VERSION__ == '0.8.X':
# create the carla config based on template and the params passed in
config = ConfigParser()
config.optionxform = str
config.read(self._config_path)
config.set('CARLA/LevelSettings', 'NumberOfVehicles',
self._driver_conf.cars)
config.set('CARLA/LevelSettings', 'NumberOfPedestrians',
self._driver_conf.pedestrians)
config.set('CARLA/LevelSettings', 'WeatherId',
self._driver_conf.weather)
output = io.StringIO()
config.write(output)
scene_descriptions = self.carla.load_settings(output.getvalue())
# based on the scene descriptions, find the start and end positions
self.positions = scene_descriptions.player_start_spots
# the episode_config saves [start_index, end_index]
self.episode_config = find_valid_episode_position(
self.positions, self.planner)
self.carla.start_episode(self.episode_config[0])
print('RESET ON POSITION ', self.episode_config[0],
", the target location is: ", self.episode_config[1])
else:
# destroy old actors
print('destroying actors')
for actor in self._actor_list:
actor.destroy()
self._actor_list = []
print('done.')
# TODO: spawn pedestrains
# TODO: spawn more vehicles
if self._autopilot:
self._current_weather = self._weather_list[
int(self._driver_conf.weather) - 1]
else:
self._current_weather = random.choice(self._weather_list)
if not self._autopilot:
# select one of the random starting points previously selected
start_positions = np.loadtxt(self._driver_conf.positions_file,
delimiter=',')
if len(start_positions.shape) == 1:
start_positions = start_positions.reshape(
1, len(start_positions))
# TODO: Assign random position from file
WINDOW_WIDTH = 768
WINDOW_HEIGHT = 576
CAMERA_FOV = 103.0
CAMERA_CENTER_T = carla.Location(x=0.7, y=-0.0, z=1.60)
CAMERA_LEFT_T = carla.Location(x=0.7, y=-0.4, z=1.60)
CAMERA_RIGHT_T = carla.Location(x=0.7, y=0.4, z=1.60)
CAMERA_CENTER_ROTATION = carla.Rotation(roll=0.0,
pitch=0.0,
yaw=0.0)
CAMERA_LEFT_ROTATION = carla.Rotation(roll=0.0,
pitch=0.0,
yaw=-45.0)
CAMERA_RIGHT_ROTATION = carla.Rotation(roll=0.0,
pitch=0.0,
yaw=45.0)
CAMERA_CENTER_TRANSFORM = carla.Transform(
location=CAMERA_CENTER_T, rotation=CAMERA_CENTER_ROTATION)
CAMERA_LEFT_TRANSFORM = carla.Transform(
location=CAMERA_LEFT_T, rotation=CAMERA_LEFT_ROTATION)
CAMERA_RIGHT_TRANSFORM = carla.Transform(
location=CAMERA_RIGHT_T, rotation=CAMERA_RIGHT_ROTATION)
self._world = self.carla.get_world()
settings = self._world.get_settings()
settings.synchronous_mode = True
self._world.apply_settings(settings)
# add traffic
blueprints_vehi = self._world.get_blueprint_library().filter(
'vehicle.*')
blueprints_vehi = [
x for x in blueprints_vehi
if int(x.get_attribute('number_of_wheels')) == 4
]
blueprints_vehi = [
x for x in blueprints_vehi if not x.id.endswith('isetta')
]
# @todo Needs to be converted to list to be shuffled.
spawn_points = list(self._world.get_map().get_spawn_points())
if len(spawn_points) == 0:
if self.city_name_demo == "Exp_Town":
spawn_points = [
carla.Transform(
location=carla.Location(x=-11.5, y=-8.0, z=2.0))
]
random.shuffle(spawn_points)
print('found %d spawn points.' % len(spawn_points))
# TODO: debug change 50 to 0
count = 0
if count > 0:
for spawn_point in spawn_points:
if self.try_spawn_random_vehicle_at(
blueprints_vehi, spawn_point):
count -= 1
if count <= 0:
break
while count > 0:
time.sleep(0.5)
if self.try_spawn_random_vehicle_at(
blueprints_vehi, random.choice(spawn_points)):
count -= 1
# end traffic addition!
# begin parking addition
if hasattr(
self._driver_conf, "parking_position_file"
) and self._driver_conf.parking_position_file is not None:
parking_points = self.get_parking_locations(
self._driver_conf.parking_position_file)
random.shuffle(parking_points)
print('found %d parking points.' % len(parking_points))
count = 200
for spawn_point in parking_points:
self.try_spawn_random_vehicle_at(blueprints_vehi,
spawn_point, False)
count -= 1
if count <= 0:
break
# end of parking addition
blueprints = self._world.get_blueprint_library().filter('vehicle')
vechile_blueprint = [
e for i, e in enumerate(blueprints)
if e.id == 'vehicle.lincoln.mkz2017'
][0]
if self._vehicle == None or self._autopilot:
if self._autopilot and self._vehicle is not None:
self._vehicle.destroy()
self._vehicle = None
while self._vehicle == None:
if self._autopilot:
# from designated points
if hasattr(self._driver_conf,
"extra_explore_prob") and random.random(
) < self._driver_conf.extra_explore_prob:
extra_positions = self.get_parking_locations(
self._driver_conf.extra_explore_position_file,
z_default=3.0,
random_perturb=True)
print(
"spawning hero vehicle from the extra exploration"
)
START_POSITION = random.choice(extra_positions)
else:
START_POSITION = random.choice(spawn_points)
else:
random_position = start_positions[
np.random.randint(start_positions.shape[0]), :]
START_POSITION = carla.Transform(
carla.Location(x=random_position[0],
y=random_position[1],
z=random_position[2] + 1.0),
carla.Rotation(pitch=random_position[3],
roll=random_position[4],
yaw=random_position[5]))
self._vehicle = self._world.try_spawn_actor(
vechile_blueprint, START_POSITION)
else:
if self._autopilot:
# from designated points
START_POSITION = random.choice(spawn_points)
else:
random_position = start_positions[
np.random.randint(start_positions.shape[0]), :]
START_POSITION = carla.Transform(
carla.Location(x=random_position[0],
y=random_position[1],
z=random_position[2] + 1.0),
carla.Rotation(pitch=random_position[3],
roll=random_position[4],
yaw=random_position[5]))
self._vehicle.set_transform(START_POSITION)
print("after spawning the ego vehicle")
print("warm up process to make the vehicle ego location correct")
wd = self._world
for i in range(25):
wd.tick()
if not wd.wait_for_tick(10.0):
continue
print("warmup finished")
if self._autopilot:
# Nope: self._vehicle.set_autopilot()
print("before roaming agent")
self._agent_autopilot = RoamingAgent(self._vehicle)
print("after roaming agent")
if self.collision_sensor is not None:
print("before destroying the sensor")
self.collision_sensor.sensor.destroy()
print("after destroying the sensor")
else:
print("collision sensor is None")
self.collision_sensor = CollisionSensor(self._vehicle, self)
print("after spawning the collision sensor")
# set weather
weather = getattr(carla.WeatherParameters, self._current_weather)
self._vehicle.get_world().set_weather(weather)
self._spectator = self._world.get_spectator()
cam_blueprint = self._world.get_blueprint_library().find(
'sensor.camera.rgb')
cam_blueprint.set_attribute('image_size_x', str(WINDOW_WIDTH))
cam_blueprint.set_attribute('image_size_y', str(WINDOW_HEIGHT))
cam_blueprint.set_attribute('fov', str(CAMERA_FOV))
if self._camera_center is not None:
self._camera_center.destroy()
self._camera_left.destroy()
self._camera_right.destroy()
self._camera_center = None
if self._camera_center == None:
self._camera_center = self._world.spawn_actor(
cam_blueprint,
CAMERA_CENTER_TRANSFORM,
attach_to=self._vehicle)
self._camera_left = self._world.spawn_actor(
cam_blueprint,
CAMERA_LEFT_TRANSFORM,
attach_to=self._vehicle)
self._camera_right = self._world.spawn_actor(
cam_blueprint,
CAMERA_RIGHT_TRANSFORM,
attach_to=self._vehicle)
self._camera_center.listen(CallBack('CameraMiddle', self))
self._camera_left.listen(CallBack('CameraLeft', self))
self._camera_right.listen(CallBack('CameraRight', self))
# spectator server camera
self._spectator = self._world.get_spectator()
self._skiped_frames = 0
self._stucked_counter = 0
self._start_time = time.time()
def get_recording(self):
if self._autopilot:
# debug: 0 for debugging
if self._skiped_frames >= 20:
return True
else:
self._skiped_frames += 1
return False
else:
'''
if (self.joystick.get_button(8)):
self._recording = True
if (self.joystick.get_button(9)):
self._recording = False
'''
if (self.joystick.get_button(6)):
self._recording = True
print("start recording!!!!!!!!!!!!!!!!!!!!!!!!1")
if (self.joystick.get_button(7)):
self._recording = False
print("end recording!!!!!!!!!!!!!!!!!!!!!!!!1")
return self._recording
def initialize_map(self, city_name):
self.city_name_demo = city_name
if city_name == "RFS_MAP":
path = get_current_folder() + "/maps_demo_area/rfs_demo_area.png"
im = cv2.imread(path)
im = im[:, :, :3]
im = im[:, :, ::-1]
self.demo_area_map = im
else:
print("do nothing since not a city with demo area")
#raise ValueError("wrong city name: " + city_name)
def loc_to_pix_rfs_sim(self, loc):
u = 3.6090651558073654 * loc[1] + 2500.541076487252
v = -3.6103367739019054 * loc[0] + 2501.862578166202
return [int(v), int(u)]
def in_valid_area(self, x, y):
if self.city_name_demo == "RFS_MAP":
pos = self.loc_to_pix_rfs_sim([x, y])
locality = 50 # 100 pixels
local_area = self.demo_area_map[pos[0] - locality:pos[0] +
locality, pos[1] -
locality:pos[1] + locality, 0] > 0
valid = np.sum(local_area) > 0
if not valid:
print(
"detect the vehicle is not in the valid demonstrated area")
return valid
else:
return True
def get_reset(self):
if self._autopilot:
if __CARLA_VERSION__ == '0.8.X':
# increase the stuck detector if conditions satisfy
if self._latest_measurements.player_measurements.forward_speed < 0.1:
self._stucked_counter += 1
else:
self._stucked_counter = 0
# if within auto pilot, reset if long enough or has collisions
if time.time() - self._start_time > self._reset_period \
or self._latest_measurements.player_measurements.collision_vehicles > 0.0 \
or self._latest_measurements.player_measurements.collision_pedestrians > 0.0 \
or self._latest_measurements.player_measurements.collision_other > 0.0 \
or (self._latest_measurements.player_measurements.intersection_otherlane > 0.0 and self._latest_measurements.player_measurements.autopilot_control.steer < -0.99) \
or self._stucked_counter > 250:
if self._stucked_counter > 250:
reset_because_stuck = True
else:
reset_because_stuck = False
# TODO: commenting out this for debugging issue
self._reset()
if reset_because_stuck:
print("resetting because getting stucked.....")
return True
else:
# TODO: implement the collision detection algorithm, based on the new API
if self.last_estimated_speed < 0.1:
self._stucked_counter += 1
else:
self._stucked_counter = 0
if time.time() - self._start_time > self._reset_period \
or self._last_collided \
or self._stucked_counter > 250 \
or not self.in_valid_area(self._latest_measurements.player_measurements.transform.location.x,
self._latest_measurements.player_measurements.transform.location.y):
#or np.abs(self._vehicle.get_vehicle_control().steer) > 0.95:
#or np.abs(self._vehicle.get_vehicle_control().brake) > 1:
# TODO intersection other lane is not available, so omit from the condition right now
if self._stucked_counter > 250:
reset_because_stuck = True
else:
reset_because_stuck = False
if self._last_collided:
print("reset becuase collision")
self._reset()
if reset_because_stuck:
print("resetting because getting stucked.....")
return True
else:
pass
return False
def get_waypoints(self):
# TODO: waiting for German Ros to expose the waypoints
wp1 = [1.0, 1.0]
wp2 = [2.0, 2.0]
return [wp1, wp2]
def action_joystick(self):
# joystick
steering_axis = self.joystick.get_axis(0)
acc_axis = self.joystick.get_axis(2)
brake_axis = self.joystick.get_axis(5)
# print("axis 0 %f, axis 2 %f, axis 3 %f" % (steering_axis, acc_axis, brake_axis))
if (self.joystick.get_button(3)):
self._rear = True
if (self.joystick.get_button(2)):
self._rear = False
control = VehicleControl()
control.steer = steering_axis
control.throttle = (acc_axis + 1) / 2.0
control.brake = (brake_axis + 1) / 2.0
if control.brake < 0.001:
control.brake = 0.0
control.hand_brake = 0
control.reverse = self._rear
control.steer -= 0.0822
#print("steer %f, throttle %f, brake %f" % (control.steer, control.throttle, control.brake))
pygame.event.pump()
return control
def action_steering_wheel(self, jsInputs, jsButtons):
control = VehicleControl()
# Custom function to map range of inputs [1, -1] to outputs [0, 1] i.e 1 from inputs means nothing is pressed
# For the steering, it seems fine as it is
K1 = 1.0 # 0.55
steerCmd = K1 * math.tan(1.1 * jsInputs[self._steer_idx])
K2 = 1.6 # 1.6
throttleCmd = K2 + (
2.05 * math.log10(-0.7 * jsInputs[self._throttle_idx] + 1.4) -
1.2) / 0.92
if throttleCmd <= 0:
throttleCmd = 0
elif throttleCmd > 1:
throttleCmd = 1
brakeCmd = 1.6 + (2.05 * math.log10(-0.7 * jsInputs[self._brake_idx] +
1.4) - 1.2) / 0.92
if brakeCmd <= 0:
brakeCmd = 0
elif brakeCmd > 1:
brakeCmd = 1
#print("Steer Cmd, ", steerCmd, "Brake Cmd", brakeCmd, "ThrottleCmd", throttleCmd)
control.steer = steerCmd
control.brake = brakeCmd
control.throttle = throttleCmd
toggle = jsButtons[self._reverse_idx]
if toggle == 1:
self._is_on_reverse += 1
if self._is_on_reverse % 2 == 0:
control.reverse = False
if self._is_on_reverse > 1:
self._is_on_reverse = True
if self._is_on_reverse:
control.reverse = True
control.hand_brake = False # jsButtons[self.handbrake_idx]
return control
def compute_action(self, sensor, speed):
if not self._autopilot:
# get pygame input
for event in pygame.event.get():
# Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN
# JOYBUTTONUP JOYHATMOTION
if event.type == pygame.JOYBUTTONDOWN:
if event.__dict__['button'] == 0:
self._current_command = 2.0
if event.__dict__['button'] == 1:
self._current_command = 3.0
if event.__dict__['button'] == 2:
self._current_command = 4.0
if event.__dict__['button'] == 3:
self._current_command = 5.0
if event.__dict__['button'] == 23:
self._current_command = 0.0
if event.__dict__['button'] == 4:
self._reset()
return VehicleControl()
if event.type == pygame.JOYBUTTONUP:
self._current_command = 2.0
#pygame.event.pump()
numAxes = self.joystick.get_numaxes()
jsInputs = [
float(self.joystick.get_axis(i)) for i in range(numAxes)
]
# print (jsInputs)
jsButtons = [
float(self.joystick.get_button(i))
for i in range(self.joystick.get_numbuttons())
]
if self._steering_wheel_flag:
control = self.action_steering_wheel(jsInputs, jsButtons)
else:
control = self.action_joystick()
else:
if __CARLA_VERSION__ == '0.8.X':
# This relies on the calling of get_sensor_data, otherwise self._latest_measurements are not filled
control = self._latest_measurements.player_measurements.autopilot_control
print('[Throttle = {}] [Steering = {}] [Brake = {}]'.format(
control.throttle, control.steer, control.brake))
else:
self._world.tick()
if self._world.wait_for_tick(10.0):
control, self._current_command = self._agent_autopilot.run_step(
)
print('[Throttle = {}] [Steering = {}] [Brake = {}]'.format(
control.throttle, control.steer, control.brake))
return control
def estimate_speed(self):
vel = self._vehicle.get_velocity()
speed = m.sqrt(vel.x**2 + vel.y**2 + vel.z**2)
# speed in m/s
return speed
def get_sensor_data(self, goal_pos=None, goal_ori=None):
if __CARLA_VERSION__ == '0.8.X':
# return the latest measurement and the next direction
measurements, sensor_data = self.carla.read_data()
self._latest_measurements = measurements
if self.use_planner:
player_data = measurements.player_measurements
pos = [
player_data.transform.location.x,
player_data.transform.location.y, 0.22
]
ori = [
player_data.transform.orientation.x,
player_data.transform.orientation.y,
player_data.transform.orientation.z
]
if sldist([
player_data.transform.location.x,
player_data.transform.location.y
], [
self.positions[self.episode_config[1]].location.x,
self.positions[self.episode_config[1]].location.y
]) < self._goal_reaching_threshold:
self._reset()
direction = self.planner.get_next_command(
pos, ori, [
self.positions[self.episode_config[1]].location.x,
self.positions[self.episode_config[1]].location.y, 0.22
], (1, 0, 0))
else:
direction = 2.0
else:
self.last_timestamp.elapsed_seconds += 0.2
#self.last_timestamp = self.carla.get_world().wait_for_tick(30.0)
#print(timestamp.delta_seconds, "delta seconds")
#while self.update_once == False:
# time.sleep(0.01)
self.last_estimated_speed = self.estimate_speed()
data_buffer_lock.acquire()
sensor_data = copy.deepcopy(self._data_buffers)
data_buffer_lock.release()
#self.update_once = False
collision_lock.acquire()
colllision_event = self._collision_events
self._last_collided = len(colllision_event) > 0
self._collision_events = []
collision_lock.release()
if len(colllision_event) > 0:
print(colllision_event)
# TODO: make sure those events are actually valid
if 'Static' in colllision_event:
collision_other = 1.0
else:
collision_other = 0.0
if "Vehicles" in colllision_event:
collision_vehicles = 1.0
else:
collision_vehicles = 0.0
if "Pedestrians" in colllision_event:
collision_pedestrians = 1.0
else:
collision_pedestrians = 0.0
#current_ms_offset = int(math.ceil((datetime.now() - self._episode_t0).total_seconds() * 1000))
# TODO: get a gametime stamp, instead of os timestamp
#current_ms_offset = int(self.carla.get_timestamp().elapsed_seconds * 1000)
#print(current_ms_offset, "ms offset")
current_ms_offset = self.last_timestamp.elapsed_seconds * 1000
second_level = namedtuple('second_level', [
'forward_speed', 'transform', 'collision_other',
'collision_pedestrians', 'collision_vehicles'
])
transform = namedtuple('transform', ['location', 'orientation'])
loc = namedtuple('loc', ['x', 'y'])
ori = namedtuple('ori', ['x', 'y', 'z'])
Meas = namedtuple('Meas',
['player_measurements', 'game_timestamp'])
v_transform = self._vehicle.get_transform()
measurements = Meas(
second_level(
self.last_estimated_speed,
transform(
loc(v_transform.location.x, v_transform.location.y),
ori(v_transform.rotation.pitch,
v_transform.rotation.roll,
v_transform.rotation.yaw)), collision_other,
collision_pedestrians, collision_vehicles),
current_ms_offset)
direction = self._current_command
self._latest_measurements = measurements
#print('[Speed = {} Km/h] [Direction = {}]'.format(measurements.player_measurements.forward_speed, direction))
#print(">>>>> planner output direction: ", direction)
return measurements, sensor_data, direction
def act(self, control):
if __CARLA_VERSION__ == '0.8.X':
self.carla.send_control(control)
else:
self._vehicle.apply_control(control)
class CarlaEnv:
def __init__(self, target):
self.carla_client = CarlaClient(config.CARLA_HOST_ADDRESS,
config.CARLA_HOST_PORT,
timeout=100)
self.carla_client.connect()
self.target = target
self.pre_image = None
self.cur_image = None
self.pre_measurements = None
self.cur_measurements = None
def step(self, action):
"""
:param action: dict of control signals, such as {'steer':0, 'throttle':0.2}
:return: next_state, reward, done, info
"""
if isinstance(action, dict):
self.carla_client.send_control(**action)
print(action)
else:
self.carla_client.send_control(action)
measurements, self.cur_image = self.carla_client.read_data()
self.cur_measurements = self.extract_measurements(measurements)
# Todo: Checkup the reward function with the images
reward, done = self.cal_reward()
self.pre_measurements = self.cur_measurements
self.pre_image = self.cur_image
return self.cur_measurements, self.cur_image, reward, done, measurements
def reset(self):
"""
:return:
"""
settings = CarlaSettings()
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=20,
NumberOfPedestrians=40,
WeatherId=random.choice([1, 3, 7, 8, 14]),
QualityLevel='Epic')
# CAMERA
camera0 = Camera('CameraRGB', PostProcessing='SceneFinal')
# Set image resolution in pixels.
camera0.set_image_size(800, 600)
# Set its position relative to the car in meters.
camera0.set_position(0.30, 0, 1.30)
settings.add_sensor(camera0)
# Let's add another camera producing ground-truth depth.
# camera1 = Camera('CameraDepth', PostProcessing='Depth')
# camera1.set_image_size(800, 600)
# camera1.set_position(0.30, 0, 1.30)
# settings.add_sensor(camera1)
# LIDAR
# lidar = Lidar('Lidar32')
# lidar.set_position(0, 0, 2.50)
# lidar.set_rotation(0, 0, 0)
# lidar.set(
# Channels=32,
# Range=50,
# PointsPerSecond=100000,
# RotationFrequency=10,
# UpperFovLimit=10,
# LowerFovLimit=-30)
# settings.add_sensor(lidar)
scene = self.carla_client.load_settings(settings)
self.pre_image = None
self.cur_image = None
self.pre_measurements = None
self.cur_measurements = None
# define a random starting point of the agent for the appropriate trainning
number_of_player_starts = len(scene.player_start_spots)
player_start = random.randint(0, max(0, number_of_player_starts - 1))
# player_start = 140
self.carla_client.start_episode(player_start)
print('Starting new episode at %r, %d...' %
(scene.map_name, player_start))
# TODO: read and return status after reset
return
def extract_measurements(self, measurements):
"""
extract custom measurement data from carla measurement
:param measurements:
:return: custom measurement data dict
"""
p_meas = measurements.player_measurements
pos_x = p_meas.transform.location.x
pos_y = p_meas.transform.location.y
speed = p_meas.forward_speed * 3.6 # m/s -> km/h
col_cars = p_meas.collision_vehicles
col_ped = p_meas.collision_pedestrians
col_other = p_meas.collision_other
other_lane = 100 * p_meas.intersection_otherlane
if other_lane:
print('Intersection into other lane %.2f' % other_lane)
offroad = 100 * p_meas.intersection_offroad
if offroad:
print('offroad %.2f' % offroad)
agents_num = len(measurements.non_player_agents)
distance = np.linalg.norm(np.array([pos_x, pos_y]) - self.target)
meas = {
'pos_x': pos_x,
'pos_y': pos_y,
'speed': speed,
'col_damage': col_cars + col_ped + col_other,
'other_lane': other_lane,
'offroad': offroad,
'agents_num': agents_num,
'distance': distance
}
message = 'Vehicle at %.1f, %.1f, ' % (pos_x, pos_y)
message += '%.0f km/h, ' % (speed, )
message += 'Collision: vehicles=%.0f, pedestrians=%.0f, other=%.0f, ' % (
col_cars,
col_ped,
col_other,
)
message += '%.0f%% other lane, %.0f%% off-road, ' % (
other_lane,
offroad,
)
message += '%d non-player agents in the scene.' % (agents_num, )
# print(message)
return meas
def cal_reward(self):
"""
:param
:return: reward, done
"""
def delta(key):
return self.cur_measurements[key] - self.pre_measurements[key]
if self.pre_measurements is None:
reward = 0.0
else:
reward = 0.05 * delta('speed') - 0.002 * delta('col_damage') \
- 2 * delta('offroad') - 2 * delta('other_lane')
# print('delta speed %.3f' % delta('speed') )
# print('[pre_offroad, current_offroad] =[%.2f, %.2f], reward: %.2f' % (self.pre_measurements['offroad'], self.cur_measurements['offroad'], reward))
# 1000 * delta('distance') + ignore the distance for auto-pilot
# check distance to target
done = self.cur_measurements[
'distance'] < 1 # final state arrived or not
return reward, done
def action_discretize(self, control):
"""
discrete the control action
:param action:
:return:
"""
# print('Before processing, steer=%.5f,throttle=%.2f,brake=%.2f' % (
# action.steer, action.throttle, action.brake))
action = control
steer = int(
100 *
action.steer) + 100 #action.steer has 21 options from [-1, 1]
throttle = int(action.throttle / 0.5) # action.throttle= 0, 0.5 or 1.0
brake = int(action.brake) # action.brake=0 or 1.0
if brake:
gas = -brake # -1
else:
gas = throttle # 0, 1, 2
gas += 1
action_no = steer << 2 | gas # map the action combination into the a numerical value
#gas takes two digits, steer takes 5 digits
action.steer, action.throttle, action.brake = (
steer - 100) / 100.0, throttle * 0.5, brake * 1.0
# print('After processing, steer=%.5f,throttle=%.2f,brake=%.2f' % (
# action.steer, action.throttle, action.brake))
return action_no, action
def reverse_action(self, action_no):
"""
map the action number to the discretized action control
:param action_no:
:return:
"""
gas = action_no & 0b11
throttle = 0.0
brake = 0.0
if gas:
throttle = (gas - 1) * 0.5
else:
brake = abs(gas - 1) * 1.0
steer = (((action_no & 0b1111111100) >> 2) - 100) / 100.0
action = dict()
action['steer'], action['throttle'], action[
'brake'] = steer, throttle, brake
return action
def close(self):
"""
:return:
"""
self.carla_client.disconnect()
class CarlaEnvironmentWrapper(EnvironmentWrapper):
def __init__(self,
num_speedup_steps=30,
require_explicit_reset=True,
is_render_enabled=False,
early_termination_enabled=False,
run_offscreen=False,
save_screens=False,
port=2000,
gpu=0,
discrete_control=True,
kill_when_connection_lost=False,
city_name="Town01"):
EnvironmentWrapper.__init__(self, is_render_enabled, save_screens)
print("port:", port)
self.episode_max_time = 1000000
self.allow_braking = True
self.log_path = os.path.join(DEFAULT_CARLA_LOG_DIR, "CarlaLogs.txt")
self.num_speedup_steps = num_speedup_steps
self.is_game_ready_for_input = False
self.run_offscreen = run_offscreen
self.kill_when_connection_lost = kill_when_connection_lost
# server configuration
self.port = port
self.gpu = gpu
self.host = 'localhost'
self.level = 'town1'
self.map = CarlaLevel().get(self.level)
# experiment = basic_experiment_suite.BasicExperimentSuite(city_name)
experiment = CoRL2017(city_name)
self.experiments = experiment.get_experiments()
self.experiment_type = 0
self.planner = Planner(city_name)
self.car_speed = 0
self.is_game_setup = False # Will be true only when setup_client_and_server() is called, either explicitly, or by reset()
# action space
self.discrete_controls = discrete_control
self.action_space_size = 3
self.action_space_high = np.array([1, 1, 1])
self.action_space_low = np.array([-1, -1, -1])
self.action_space_abs_range = np.maximum(
np.abs(self.action_space_low), np.abs(self.action_space_high))
self.steering_strength = 0.35
self.gas_strength = 1.0
self.brake_strength = 0.6
self.actions = {
0: [0., 0.],
1: [0., -self.steering_strength],
2: [0., self.steering_strength],
3: [self.gas_strength - 0.15, 0.],
4: [-self.brake_strength, 0],
5: [self.gas_strength - 0.3, -self.steering_strength],
6: [self.gas_strength - 0.3, self.steering_strength],
7: [-self.brake_strength, -self.steering_strength],
8: [-self.brake_strength, self.steering_strength]
}
self.actions_description = [
'NO-OP', 'TURN_LEFT', 'TURN_RIGHT', 'GAS', 'BRAKE',
'GAS_AND_TURN_LEFT', 'GAS_AND_TURN_RIGHT', 'BRAKE_AND_TURN_LEFT',
'BRAKE_AND_TURN_RIGHT'
]
if discrete_control:
self.action_space = Discrete(len(self.actions))
else:
self.action_space = Box(low=self.action_space_low,
high=self.action_space_high)
self.observation_space = Box(low=-np.inf,
high=np.inf,
shape=[88, 200, 3])
# measurements
self.measurements_size = (1, )
self.pre_image = None
self.first_debug = True
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
print("setup server, out:", self.server_pid)
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")
return
except TCPConnectionError as error:
print(error)
time.sleep(1)
def close_client_and_server(self):
self._close_server()
print("Disconnecting the client")
self.game.disconnect()
self.game = None
self.server = None
self.is_game_setup = False
return
def _open_server(self):
# Note: There is no way to disable rendering in CARLA as of now
# https://github.com/carla-simulator/carla/issues/286
# decrease the window resolution if you want to see if performance increases
# Command: $CARLA_ROOT/CarlaUE4.sh /Game/Maps/Town02 -benchmark -carla-server -fps=15 -world-port=9876 -windowed -ResX=480 -ResY=360 -carla-no-hud
# To run off_screen: SDL_VIDEODRIVER=offscreen SDL_HINT_CUDA_DEVICE=0 <command> #https://github.com/carla-simulator/carla/issues/225
my_env = None
if self.run_offscreen:
my_env = {
**os.environ,
'SDL_VIDEODRIVER': 'offscreen',
'SDL_HINT_CUDA_DEVICE': '0',
}
with open(self.log_path, "wb") as out:
#docker <19.03
# p = subprocess.Popen(['docker', 'run', '--rm', '-d', '-p',
# str(self.port) + '-' + str(self.port + 2) + ':' + str(self.port) + '-' + str(self.port + 2),
# '--runtime=nvidia', '-e', 'NVIDIA_VISIBLE_DEVICES='+str(self.gpu), "carlasim/carla:0.8.4",
# '/bin/bash', 'CarlaUE4.sh', self.map, '-windowed',
# '-benchmark', '-fps=10', '-world-port=' + str(self.port)], shell=False,
# stdout=subprocess.PIPE)
#docker=19.03
# p = subprocess.Popen(['docker', 'run', '--rm', '-d', '-p',
# str(self.port) + '-' + str(self.port + 2) + ':' + str(self.port) + '-' + str(
# self.port + 2),
# '--gpus='+str(self.gpu), "lin-ai-27:5000/carla:0.8.4",
# '/bin/bash', 'CarlaUE4.sh', self.map, '-windowed',
# '-benchmark', '-fps=10', '-world-port=' + str(self.port)], shell=False,
# stdout=subprocess.PIPE)
#
# docker_out, err = p.communicate()
cmd = [
path.join(environ.get('CARLA_ROOT'),
'CarlaUE4.sh'), self.map, "-benchmark",
"-carla-server", "-fps=10", "-world-port={}".format(self.port),
"-windowed -ResX={} -ResY={}".format(
carla_config.server_width,
carla_config.server_height), "-carla-no-hud"
]
# p = subprocess.Popen(cmd, stdout=out, stderr=out, env=my_env, preexec_fn=os.setsid)
p = subprocess.Popen(cmd, env=my_env, preexec_fn=os.setsid)
return p
def _close_server(self):
# if self.kill_when_connection_lost:
# print("kill before")
# os.kill(self.server.pid, signal.SIGKILL)
# self.server.kill()
# # subprocess.call(['docker', 'stop', self.out[:-1]])
#
# print("kill after")
# return
no_of_attempts = 0
while is_process_alive(self.server_pid):
try:
self.server.terminate()
self.server.wait()
os.killpg(self.server.pid, signal.SIGTERM)
time.sleep(10)
except Exception as error:
print(error)
def _get_directions(self, current_point, end_point):
"""
Class that should return the directions to reach a certain goal
"""
directions = self.planner.get_next_command(
(current_point.location.x, current_point.location.y, 0.22),
(current_point.orientation.x, current_point.orientation.y,
current_point.orientation.z),
(end_point.location.x, end_point.location.y, 0.22),
(end_point.orientation.x, end_point.orientation.y,
end_point.orientation.z))
return directions
def _update_state(self):
# get measurements and observations
measurements = []
while type(measurements) == list:
try:
measurements, sensor_data = self.game.read_data()
except:
print(
"Connection to server lost while reading state. Reconnecting..........."
)
# self.game.disconnect()
# self.setup_client_and_server(reconnect_client_only=True)
self.close_client_and_server()
self.setup_client_and_server(reconnect_client_only=False)
self.game.start_episode(self.pose[0])
self.done = True
current_point = measurements.player_measurements.transform
direction = self._get_directions(current_point, self.end_point)
speed = measurements.player_measurements.forward_speed
self.reward = 0
dist = sldist([current_point.location.x, current_point.location.y],
[self.end_point.location.x, self.end_point.location.y])
if direction == 5: #go straight
if abs(self.control.steer) > 0.2:
self.reward -= 20
self.reward += min(35, speed * 3.6)
elif direction == 2: #follow lane
self.reward += min(35, speed * 3.6)
elif direction == 3: #turn left ,steer negtive
if self.control.steer > 0:
self.reward -= 15
if speed * 3.6 <= 20:
self.reward += speed * 3.6
else:
self.reward += 40 - speed * 3.6
elif direction == 4: #turn right
if self.control.steer < 0:
self.reward -= 15
if speed * 3.6 <= 20:
self.reward += speed * 3.6
else:
self.reward += 40 - speed * 3.6
elif direction == 0:
self.done = True
self.reward += 100
direction = 2
print("success", dist)
else:
print("error direction")
direction = 5
direction -= 2
direction = int(direction)
intersection_offroad = measurements.player_measurements.intersection_offroad
intersection_otherlane = measurements.player_measurements.intersection_otherlane
collision_veh = measurements.player_measurements.collision_vehicles
collision_ped = measurements.player_measurements.collision_pedestrians
collision_other = measurements.player_measurements.collision_other
if intersection_otherlane > 0 or intersection_offroad > 0 or collision_ped > 0 or collision_veh > 0:
self.reward -= 100
elif collision_other > 0:
self.reward -= 50
if collision_other > 0 or collision_veh > 0 or collision_ped > 0:
self.done = True
if intersection_offroad > 0.2 or intersection_otherlane > 0.2:
self.done = True
if speed * 3.6 <= 1:
self.nospeed_time += 1
if self.nospeed_time > 100:
self.done = True
self.reward -= 1
else:
self.nospeed_time = 0
# speed = min(1, speed/10.0)
speed = speed / 25
self.observation = {
"img": self.process_image(sensor_data['CameraRGB'].data),
"speed": speed,
"direction": direction
}
return self.observation, self.reward, self.done
def encode_obs(self, image):
if self.pre_image is None:
self.pre_image = image
img = np.concatenate([self.pre_image, image], axis=2)
self.pre_image = image
return img
def process_image(self, data):
rgb_img = data[115:510, :]
rgb_img = cv2.resize(rgb_img, (200, 88), interpolation=cv2.INTER_AREA)
# return cv2.resize(
# data, (80, 80),
# interpolation=cv2.INTER_AREA)
return rgb_img
def _take_action(self, action_idx):
if not self.is_game_setup:
print("Reset the environment duh by reset() before calling step()")
sys.exit(1)
if type(action_idx) == np.int64 or type(action_idx) == np.int:
action = self.actions[action_idx]
else:
action = action_idx
self.control = VehicleControl()
if len(action) == 3:
self.control.throttle = np.clip(action[1], 0, 1)
self.control.steer = np.clip(action[0], -1, 1)
self.control.brake = np.clip(action[2], 0, 1)
else:
self.control.throttle = np.clip(action[0], 0, 1)
self.control.steer = np.clip(action[1], -1, 1)
self.control.brake = np.abs(np.clip(action[0], -1, 0))
if not self.allow_braking:
self.control.brake = 0
self.control.hand_brake = False
self.control.reverse = False
controls_sent = False
while not controls_sent:
try:
self.game.send_control(self.control)
controls_sent = True
# rand = random.randint(0, 4)
# if rand == 0 and self.first_debug:
# self.first_debug = False
# raise Exception
except:
print(
"Connection to server lost while sending controls. Reconnecting..........."
)
self.close_client_and_server()
self.setup_client_and_server(reconnect_client_only=False)
scene = self.game.load_settings(self.cur_experiment.conditions)
# self.positions = scene.player_start_spots
# self.start_point = self.positions[self.pose[0]]
# self.end_point = self.positions[self.pose[1]]
self.game.start_episode(self.pose[0])
self.done = True
controls_sent = False
return
def _restart_environment_episode(self, force_environment_reset=True):
if not force_environment_reset and not self.done and self.is_game_setup:
print("Can't reset dude, episode ain't over yet")
return None # User should handle this
self.is_game_ready_for_input = False
if not self.is_game_setup:
self.setup_client_and_server()
experiment_type = random.randint(0, 2)
self.cur_experiment = self.experiments[experiment_type]
self.pose = random.choice(self.cur_experiment.poses)
# self.cur_experiment = self.experiments[0]
# self.pose = self.cur_experiment.poses[0]
scene = self.game.load_settings(self.cur_experiment.conditions)
self.positions = scene.player_start_spots
self.start_point = self.positions[self.pose[0]]
self.end_point = self.positions[self.pose[1]]
# print("start episode")
while True:
try:
self.game.start_episode(self.pose[0])
# start the game with some initial speed
self.car_speed = 0
self.nospeed_time = 0
observation = None
for i in range(self.num_speedup_steps):
observation, reward, done, _ = self.step([0, 1.0, 0])
self.observation = observation
self.is_game_ready_for_input = True
break
except Exception as error:
print(error)
self.game.connect()
self.game.start_episode(self.pose[0])
return observation
class CarlaEnv(gym.Env):
def __init__(self, config=ENV_CONFIG):
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)
# 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.positions = None
self.distance_collect = np.array([])
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
# 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 = scene.player_start_spots
self.start_pos = self.positions[self.scenario["start_pos_id"]]
self.end_pos = self.positions[self.scenario["end_pos_id"]]
self.start_coord = [
self.start_pos.location.x, self.start_pos.location.y
]
self.end_coord = [self.end_pos.location.x, self.end_pos.location.y]
print("Start pos {} ({}), end {} ({})".format(
self.scenario["start_pos_id"], 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
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"]
# ])
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, {})
# 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.config["squash_action_logits"]:
forward = 2 * float(sigmoid(action[0]) - 0.5)
throttle = float(np.clip(forward, 0, 1))
brake = float(np.abs(np.clip(forward, -1, 0)))
steer = 2 * float(sigmoid(action[1]) - 0.5)
else:
throttle = float(np.clip(action[0], 0, 1))
brake = float(np.abs(np.clip(action[0], -1, 0)))
steer = float(np.clip(action[1], -1, 1))
# reverse and hand_brake are disabled.
reverse = False
hand_brake = False
# if self.config["verbose"]:
# print("steer", steer, "throttle", throttle, "brake", brake,
# "reverse", reverse)
self.client.send_control(steer=steer,
throttle=throttle,
brake=brake,
hand_brake=hand_brake,
reverse=reverse)
# Process observations: self._read_observation() returns image and py_measurements.
image, py_measurements = self._read_observation()
if self.config["verbose"]:
print("Next command", py_measurements["next_command"])
if type(action) is np.ndarray:
py_measurements["action"] = [float(a) for a in action]
else:
py_measurements["action"] = action
py_measurements["control"] = {
"steer": steer,
"throttle": throttle,
"brake": brake,
"reverse": reverse,
"hand_brake": hand_brake,
}
# compute reward
reward = compute_reward(self, self.prev_measurement, py_measurements)
self.total_reward += reward
py_measurements["reward"] = reward
py_measurements["total_reward"] = self.total_reward
# done or not
done = False
# 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()
# 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"]:
plan = 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
])
next_command = COMMANDS_ENUM[plan]
if next_command == "REACH_GOAL":
self.scenario = random.choice(self.config["scenarios"])
self.end_pos = self.positions[self.scenario["end_pos_id"]]
next_command = "LANE_FOLLOW"
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
])
self.distance_collect = np.append(self.distance_collect,
distance_to_goal)
if len(self.distance_collect) > 2:
# compute distance change between distance-to-goal at each point
# e.g. distance collect [1, 2, 3...] [3, 2 ,1...] -->
# distance_change [1, 1] [-1, -1]
# the former means we make mistake
# because the distance between agent and goal is getting larger
distance_change = self.distance_collect[
1:] - self.distance_collect[:-1]
if len(distance_change) > 50:
s = 50
else:
s = 1
if np.sum(distance_change[s:] > 0) > 0:
# if distance change is greater than zero it means we on driving on the wrong route
next_command = "WRONG_ROUTE"
else:
distance_to_goal = -1
distance_to_goal_euclidean = float(
np.linalg.norm([
cur.transform.location.x - self.end_pos.location.x,
cur.transform.location.y - self.end_pos.location.y
]))
py_measurements = {
"episode_id": self.episode_id,
"step": self.num_steps,
"x": cur.transform.location.x,
"y": cur.transform.location.y,
"x_orient": cur.transform.orientation.x,
"y_orient": cur.transform.orientation.y,
"forward_speed": cur.forward_speed,
"distance_to_goal": distance_to_goal,
"distance_to_goal_euclidean": distance_to_goal_euclidean,
"collision_vehicles": cur.collision_vehicles,
"collision_pedestrians": cur.collision_pedestrians,
"collision_other": cur.collision_other,
"intersection_offroad": cur.intersection_offroad,
"intersection_otherlane": cur.intersection_otherlane,
"weather": self.weather,
"map": self.config["server_map"],
"start_coord": self.start_coord,
"end_coord": self.end_coord,
"current_scenario": self.scenario,
"x_res": self.config["x_res"],
"y_res": self.config["y_res"],
"num_vehicles": self.scenario["num_vehicles"],
"num_pedestrians": self.scenario["num_pedestrians"],
"max_steps": self.scenario["max_steps"],
"next_command": next_command,
}
if CARLA_OUT_PATH and self.config["log_images"]:
for name, image in sensor_data.items():
out_dir = os.path.join(CARLA_OUT_PATH, name)
if not os.path.exists(out_dir):
os.makedirs(out_dir)
out_file = os.path.join(
out_dir, "{}_{:>04}.jpg".format(self.episode_id,
self.num_steps))
scipy.misc.imsave(out_file,
image.data) # image.data without preprocess.
assert observation is not None, sensor_data
return observation, py_measurements
class CarlaEnv(gym.Env):
def __init__(self, config=ENV_CONFIG):
self.config = config
if config["discrete_actions"]:
self.action_space = Discrete(10)
else:
self.action_space = Box(-1.0, 1.0, shape=(3, ))
if config["use_depth_camera"]:
self.observation_space = Box(-1.0,
1.0,
shape=(config["x_res"],
config["y_res"], 1))
else:
self.observation_space = Box(0.0,
255.0,
shape=(config["x_res"],
config["y_res"], 3))
self._spec = lambda: None
self._spec.id = "Carla-v0"
self.server_port = None
self.server_process = None
self.client = None
self.num_steps = 0
self.prev_measurement = None
def init_server(self):
print("Initializing new Carla server...")
# Create a new server process and start the client.
self.server_port = random.randint(10000, 60000)
self.server_process = subprocess.Popen([
SERVER_BINARY, "/Game/Maps/Town02", "-windowed", "-ResX=400",
"-ResY=300", "-carla-server", "-carla-world-port={}".format(
self.server_port)
],
preexec_fn=os.setsid,
stdout=open(os.devnull, "w"))
self.client = CarlaClient("localhost", self.server_port)
self.client.connect()
def clear_server_state(self):
print("Clearing Carla server state")
try:
if self.client:
self.client.disconnect()
self.client = None
except Exception as e:
print("Error disconnecting client: {}".format(e))
pass
if self.server_process:
os.killpg(os.getpgid(self.server_process.pid), signal.SIGKILL)
self.server_port = None
self.server_process = None
def __del__(self):
self.clear_server_state()
def reset(self):
error = None
for _ in range(RETRIES_ON_ERROR):
try:
if not self.server_process:
self.init_server()
return self._reset()
except Exception as e:
print("Error during reset: {}".format(traceback.format_exc()))
self.clear_server_state()
error = e
raise error
def _reset(self):
self.num_steps = 0
self.prev_measurement = None
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self.config["num_vehicles"],
NumberOfPedestrians=self.config["num_pedestrians"],
WeatherId=random.choice(self.config["weather"]))
settings.randomize_seeds()
if self.config["use_depth_camera"]:
camera = Camera("CameraDepth", PostProcessing="Depth")
camera.set_image_size(self.config["x_res"], self.config["y_res"])
camera.set_position(30, 0, 130)
settings.add_sensor(camera)
else:
camera = Camera("CameraRGB")
camera.set_image_size(self.config["x_res"], self.config["y_res"])
camera.set_position(30, 0, 130)
settings.add_sensor(camera)
scene = self.client.load_settings(settings)
# Choose one player start at random.
number_of_player_starts = len(scene.player_start_spots)
if self.config["random_starting_location"]:
player_start = random.randint(0, max(0,
number_of_player_starts - 1))
else:
player_start = 0
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print("Starting new episode...")
self.client.start_episode(player_start)
image, measurements = self._read_observation()
self.prev_measurement = measurements
return self.preprocess_image(image)
def step(self, action):
try:
obs = self._step(action)
return obs
except Exception:
print("Error during step, terminating episode early",
traceback.format_exc())
self.clear_server_state()
return np.zeros(self.observation_space.shape), 0.0, True, {}
def _step(self, action):
if self.config["discrete_actions"]:
action = int(action)
assert action in range(10)
if action == 9:
brake = 1.0
steer = 0.0
throttle = 0.0
reverse = False
else:
brake = 0.0
if action >= 6:
steer = -1.0
elif action >= 3:
steer = 1.0
else:
steer = 0.0
action %= 3
if action == 0:
throttle = 0.0
reverse = False
elif action == 1:
throttle = 1.0
reverse = False
elif action == 2:
throttle = 1.0
reverse = True
else:
assert len(action) == 3, "Invalid action {}".format(action)
steer = action[0]
throttle = min(1.0, abs(action[1]))
brake = max(0.0, min(1.0, action[2]))
reverse = action[1] < 0.0
print("steer", steer, "throttle", throttle, "brake", brake, "reverse",
reverse)
self.client.send_control(steer=steer,
throttle=throttle,
brake=brake,
hand_brake=False,
reverse=reverse)
image, measurements = self._read_observation()
reward, done = compute_reward(self.config, self.prev_measurement,
measurements)
self.prev_measurement = measurements
if self.num_steps > self.config["max_steps"]:
done = True
self.num_steps += 1
info = {}
image = self.preprocess_image(image)
return image, reward, done, info
def preprocess_image(self, image):
if self.config["use_depth_camera"]:
data = (image.data - 0.5) * 2
return data.reshape(self.config["x_res"], self.config["y_res"], 1)
else:
return image.data.reshape(self.config["x_res"],
self.config["y_res"], 3)
def _read_observation(self):
# Read the data produced by the server this frame.
measurements, sensor_data = self.client.read_data()
# Print some of the measurements.
print_measurements(measurements)
observation = None
if self.config["use_depth_camera"]:
camera_name = "CameraDepth"
else:
camera_name = "CameraRGB"
for name, image in sensor_data.items():
if name == camera_name:
observation = image
if IMAGE_OUT_PATH:
for name, image in sensor_data.items():
scipy.misc.imsave(
"{}/{}-{}.jpg".format(IMAGE_OUT_PATH, name,
self.num_steps), image.data)
assert observation is not None, sensor_data
return observation, measurements
class CarlaEnv(gym.Env):
def __init__(self, config=ENV_CONFIG):
self.config = config
self.city = self.config["server_map"].split("/")[-1]
if self.config["enable_planner"]:
self.planner = Planner(self.city)
self.action_space = Discrete(len(DISCRETE_ACTIONS))
# RGB Camera
self.frame_shape = (config["y_res"], config["x_res"])
image_space = Box(
0, 1, shape=(
config["y_res"], config["x_res"],
FRAME_DEPTH * config["framestack"]), dtype=np.float32)
self.observation_space = image_space
# TODO(ekl) this isn't really a proper gym spec
self._spec = lambda: None
self._spec.id = "Carla-v0"
self.server_port = None
self.server_process = None
self.client = None
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.prev_image = None
self.episode_id = None
self.measurements_file = None
self.weather = None
self.scenario = None
self.start_pos = None
self.end_pos = None
self.start_coord = None
self.end_coord = None
self.last_obs = None
self.last_full_obs = None
self.framestack = [None] * config["framestack"]
self.framestack_index = 0
self.running_restarts = 0
self.on_step = None
self.on_next = None
self.photo_index = 1
self.episode_index = 0
def init_server(self):
print("Initializing new Carla server...")
# Create a new server process and start the client.
self.server_port = random.randint(10000, 60000)
self.server_process = subprocess.Popen(
[self.config["server_binary"], self.config["server_map"],
"-windowed",
"-ResX={}".format(self.config["render_x_res"]),
"-ResY={}".format(self.config["render_y_res"]),
"-fps={}".format(self.config["fps"]),
"-carla-server",
"-carla-world-port={}".format(self.server_port)],
preexec_fn=os.setsid, stdout=open(os.devnull, "w"))
live_carla_processes.add(os.getpgid(self.server_process.pid))
for i in range(RETRIES_ON_ERROR):
try:
self.client = CarlaClient("localhost", self.server_port)
return self.client.connect()
except Exception as e:
print("Error connecting: {}, attempt {}".format(e, i))
time.sleep(2)
def clear_server_state(self):
print("Clearing Carla server state")
try:
if self.client:
self.client.disconnect()
self.client = None
except Exception as e:
print("Error disconnecting client: {}".format(e))
pass
if self.server_process:
pgid = os.getpgid(self.server_process.pid)
os.killpg(pgid, signal.SIGKILL)
live_carla_processes.remove(pgid)
self.server_port = None
self.server_process = None
def __del__(self):
self.clear_server_state()
def reset(self):
if self.on_next is not None:
self.on_next()
error = None
self.running_restarts += 1
for _ in range(RETRIES_ON_ERROR):
try:
# Force a full reset of Carla after some number of restarts
if self.running_restarts > self.config["server_restart_interval"]:
print("Shutting down carla server...")
self.running_restarts = 0
self.clear_server_state()
# If server down, initialize
if not self.server_process:
self.init_server()
# Run episode reset
return self._reset()
except Exception as e:
print("Error during reset: {}".format(traceback.format_exc()))
self.clear_server_state()
error = e
time.sleep(5)
raise error
def _build_camera(self, name, post):
camera = Camera(name, PostProcessing=post)
camera.set_image_size(
self.config["render_x_res"], self.config["render_y_res"])
camera.set_position(x=2.4, y=0, z=0.8)
camera.set_rotation(pitch=-40, roll=0, yaw=0)
# camera.FOV = 100
return camera
def _reset(self):
self.num_steps = 0
self.total_reward = 0
self.episode_index += 1
self.photo_index = 1
self.prev_measurement = None
self.prev_image = None
self.episode_id = datetime.today().strftime("%Y-%m-%d_%H-%M-%S_%f")
self.measurements_file = None
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
self.scenario = random.choice(self.config["scenarios"])
assert self.scenario["city"] == self.city, (self.scenario, self.city)
self.weather = random.choice(self.scenario["weather_distribution"])
settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=self.scenario["num_vehicles"],
NumberOfPedestrians=self.scenario["num_pedestrians"],
WeatherId=self.weather,
QualityLevel=self.config["quality"])
settings.randomize_seeds()
# Add the cameras
if self.config["save_images_rgb"]:
settings.add_sensor(self._build_camera(name="CameraRGB", post="SceneFinal"))
settings.add_sensor(self._build_camera(name="CameraDepth", post="Depth"))
settings.add_sensor(self._build_camera(name="CameraClass", post="SemanticSegmentation"))
# Setup start and end positions
scene = self.client.load_settings(settings)
positions = scene.player_start_spots
self.start_pos = positions[self.scenario["start_pos_id"]]
self.end_pos = positions[self.scenario["end_pos_id"]]
self.start_coord = [
self.start_pos.location.x // 100, self.start_pos.location.y // 100]
self.end_coord = [
self.end_pos.location.x // 100, self.end_pos.location.y // 100]
print(
"Start pos {} ({}), end {} ({})".format(
self.scenario["start_pos_id"], self.start_coord,
self.scenario["end_pos_id"], self.end_coord))
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print("Starting new episode...")
self.client.start_episode(self.scenario["start_pos_id"])
image, py_measurements = self._read_observation()
py_measurements["control"] = {
"throttle_brake": 0,
"steer": 0,
"throttle": 0,
"brake": 0,
"reverse": False,
"hand_brake": False,
}
self.prev_measurement = py_measurements
return self.encode_obs(self.preprocess_image(image), py_measurements)
def encode_obs(self, obs, py_measurements):
new_obs = obs
num_frames = int(self.config["framestack"])
if num_frames > 1:
# Spread out to number of frames
frame_array = [obs] * num_frames
for frame_index in range(1, num_frames):
index = (self.framestack_index - frame_index) % num_frames
if self.framestack[index] is not None:
frame_array[frame_index] = self.framestack[index]
# Concatenate into a single np array
new_obs = np.concatenate(frame_array, axis=2)
# Store frame
self.framestack[self.framestack_index] = obs
self.framestack_index = (self.framestack_index + 1) % num_frames
self.last_obs = obs
# Return
return new_obs
def step(self, action):
try:
obs = self._step(action)
self.last_full_obs = obs
return obs
except Exception:
print(
"Error during step, terminating episode early",
traceback.format_exc())
self.clear_server_state()
return (self.last_full_obs, 0.0, True, {})
def _step(self, action):
action = DISCRETE_ACTIONS[int(action)]
assert len(action) == 2, "Invalid action {}".format(action)
if self.config["squash_action_logits"]:
forward = 2 * float(sigmoid(action[0]) - 0.5)
throttle = float(np.clip(forward, 0, 1))
brake = float(np.abs(np.clip(forward, -1, 0)))
steer = 2 * float(sigmoid(action[1]) - 0.5)
else:
throttle = float(np.clip(action[0], 0, 1))
brake = float(np.abs(np.clip(action[0], -1, 0)))
steer = float(np.clip(action[1], -1, 1))
reverse = False
hand_brake = False
if self.config["verbose"]:
print(
"steer", steer, "throttle", throttle, "brake", brake,
"reverse", reverse)
self.client.send_control(
steer=steer, throttle=throttle, brake=brake, hand_brake=hand_brake,
reverse=reverse)
# Process observations
image, py_measurements = self._read_observation()
if self.config["verbose"]:
print("Next command", py_measurements["next_command"])
if type(action) is np.ndarray:
py_measurements["action"] = [float(a) for a in action]
else:
py_measurements["action"] = action
py_measurements["control"] = {
"throttle_brake": action[0],
"steer": steer,
"throttle": throttle,
"brake": brake,
"reverse": reverse,
"hand_brake": hand_brake,
}
# Done?
finished = self.num_steps > self.scenario["max_steps"]
# py_measurements["next_command"] == "REACH_GOAL"
failed = TERM.compute_termination(self, py_measurements, self.prev_measurement)
done = finished or failed
py_measurements["finished"] = finished
py_measurements["failed"] = failed
py_measurements["done"] = done
# Determine Reward
reward = compute_reward(self, self.prev_measurement, py_measurements)
self.total_reward += reward
py_measurements["reward"] = reward
py_measurements["total_reward"] = self.total_reward
self.prev_measurement = py_measurements
# Callback
if self.on_step is not None:
self.on_step(py_measurements)
# Write out measurements to file
if self.config["carla_out_path"]:
# Ensure measurements dir exists
measurements_dir = os.path.join(self.config["carla_out_path"], self.config["measurements_subdir"])
if not os.path.exists(measurements_dir):
os.mkdir(measurements_dir)
if not self.measurements_file:
self.measurements_file = open(
os.path.join(
measurements_dir,
"m_{}.json".format(self.episode_id)),
"w")
self.measurements_file.write(json.dumps(py_measurements))
self.measurements_file.write("\n")
if done:
self.measurements_file.close()
self.measurements_file = None
if self.config["convert_images_to_video"]:
self.images_to_video(camera_name="RGB")
self.images_to_video(camera_name="Depth")
self.images_to_video(camera_name="Class")
self.num_steps += 1
image = self.preprocess_image(image)
return (
self.encode_obs(image, py_measurements), reward, done,
py_measurements)
def preprocess_image(self, image):
return image
def _fuse_observations(self, depth, clazz, speed):
base_shape = (self.config["render_y_res"], self.config["render_x_res"])
new_shape = (self.config["y_res"], self.config["x_res"])
# Reduce class
clazz = reduce_classifications(clazz)
# Do we need to resize?
if base_shape[0] is not new_shape[0] and base_shape[1] is not new_shape[1]:
depth_reshape = depth.reshape(*depth.shape)
depth = cv2.resize(depth_reshape, (new_shape[1], new_shape[0]))
clazz = resize_classifications(clazz, new_shape)
# Fuse with depth!
obs = fuse_with_depth(clazz, depth, extra_layers=1)
# Fuse with speed!
obs[:, :, KEEP_CLASSIFICATIONS] = speed
return obs, clazz
def _read_observation(self):
# Read the data produced by the server this frame.
measurements, sensor_data = self.client.read_data()
cur = measurements.player_measurements
# Print some of the measurements.
if self.config["verbose"]:
print_measurements(measurements)
# Fuse the observation data to create a single observation
observation, clazzes = self._fuse_observations(
sensor_data['CameraDepth'].data,
sensor_data['CameraClass'].data,
cur.forward_speed)
if self.config["enable_planner"]:
next_command = COMMANDS_ENUM[
self.planner.get_next_command(
[cur.transform.location.x, cur.transform.location.y,
GROUND_Z],
[cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z],
[self.end_pos.location.x, self.end_pos.location.y,
GROUND_Z],
[self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z])
]
else:
next_command = "LANE_FOLLOW"
if next_command == "REACH_GOAL":
distance_to_goal = 0.0 # avoids crash in planner
elif self.config["enable_planner"]:
distance_to_goal = self.planner.get_shortest_path_distance(
[cur.transform.location.x, cur.transform.location.y, GROUND_Z],
[cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z],
[self.end_pos.location.x, self.end_pos.location.y, GROUND_Z],
[self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z]) / 100
else:
distance_to_goal = -1
distance_to_goal_euclidean = float(np.linalg.norm(
[cur.transform.location.x - self.end_pos.location.x,
cur.transform.location.y - self.end_pos.location.y]) / 100)
py_measurements = {
"episode_id": self.episode_id,
"step": self.num_steps,
"x": cur.transform.location.x,
"y": cur.transform.location.y,
"x_orient": cur.transform.orientation.x,
"y_orient": cur.transform.orientation.y,
"forward_speed": cur.forward_speed,
"distance_to_goal": distance_to_goal,
"distance_to_goal_euclidean": distance_to_goal_euclidean,
"collision_vehicles": cur.collision_vehicles,
"collision_pedestrians": cur.collision_pedestrians,
"collision_other": cur.collision_other,
"intersection_offroad": cur.intersection_offroad,
"intersection_otherlane": cur.intersection_otherlane,
"weather": self.weather,
"map": self.config["server_map"],
"start_coord": self.start_coord,
"end_coord": self.end_coord,
"current_scenario": self.scenario,
"x_res": self.config["x_res"],
"y_res": self.config["y_res"],
"num_vehicles": self.scenario["num_vehicles"],
"num_pedestrians": self.scenario["num_pedestrians"],
"max_steps": self.scenario["max_steps"],
"next_command": next_command,
"applied_penalty": False,
"total_reward": self.total_reward,
}
if self.config["carla_out_path"] \
and self.num_steps % self.config["save_image_frequency"] == 0\
and self.num_steps > 15:
self.take_photo(
sensor_data=sensor_data,
class_data=clazzes,
fused_data=observation
)
assert observation is not None, sensor_data
return observation, py_measurements
def images_dir(self):
dir = os.path.join(self.config["carla_out_path"], "images")
if not os.path.exists(dir):
os.mkdir(dir)
return dir
def episode_dir(self):
episode_dir = os.path.join(self.images_dir(), "episode_" + str(self.episode_index))
if not os.path.exists(episode_dir):
os.mkdir(episode_dir)
return episode_dir
def take_photo(self, sensor_data, class_data, fused_data):
# Get the proper locations\
episode_dir = self.episode_dir()
photo_index = self.photo_index
name_prefix = "episode_{:>04}_step_{:>04}".format(self.episode_index, photo_index)
self.photo_index += 1
# Save the image
if self.config["save_images_rgb"]:
rgb_image = sensor_data["CameraRGB"]
image_path = os.path.join(episode_dir, name_prefix + "_rgb.png")
scipy.misc.imsave(image_path, rgb_image.data)
# Save the classes
if self.config["save_images_class"]:
classes_path = os.path.join(episode_dir, name_prefix + "_class.png")
scipy.misc.imsave(classes_path, class_data.data)
# Save the class images
if self.config["save_images_fusion"]:
fused_images = fused_to_rgb(fused_data)
fused_path = os.path.join(episode_dir, name_prefix + "_fused.png")
scipy.misc.imsave(fused_path, fused_images.data)
def images_to_video(self, camera_name):
# Video directory
videos_dir = os.path.join(self.config["carla_out_path"], "Videos" + camera_name)
if not os.path.exists(videos_dir):
os.makedirs(videos_dir)
# Build command
video_fps = self.config["fps"] / self.config["saveimage_frequency"]
ffmpeg_cmd = (
"ffmpeg -loglevel -8 -r 10 -f image2 -s {x_res}x{y_res} "
"-start_number 0 -i "
"{img}_%04d.jpg -vcodec libx264 {vid}.mp4 && rm -f {img}_*.jpg "
).format(
x_res=self.config["render_x_res"],
y_res=self.config["render_y_res"],
vid=os.path.join(videos_dir, self.episode_id),
img=os.path.join(self.config["carla_out_path"], "Camera" + camera_name, self.episode_id))
# Execute command
print("Executing ffmpeg command: ", ffmpeg_cmd)
try:
subprocess.call(ffmpeg_cmd, shell=True, timeout=60)
except Exception as ex:
print("FFMPEG EXPIRED")
print(ex)
class CarlaOperator(Op):
"""Provides an ERDOS interface to the CARLA simulator.
Args:
synchronous_mode (bool): whether the simulator will wait for control
input from the client.
"""
def __init__(self,
name,
flags,
camera_setups=[],
lidar_stream_names=[],
log_file_name=None,
csv_file_name=None):
super(CarlaOperator, self).__init__(name)
self._flags = flags
self._logger = setup_logging(self.name, log_file_name)
self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name)
self.message_num = 0
if self._flags.carla_high_quality:
quality = 'Epic'
else:
quality = 'Low'
self.settings = CarlaSettings()
self.settings.set(
SynchronousMode=self._flags.carla_synchronous_mode,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self._flags.carla_num_vehicles,
NumberOfPedestrians=self._flags.carla_num_pedestrians,
WeatherId=self._flags.carla_weather,
QualityLevel=quality)
self.settings.randomize_seeds()
self.lidar_streams = []
for (camera_stream_name, camera_type, image_size,
pos) in camera_setups:
self.__add_camera(name=camera_stream_name,
postprocessing=camera_type,
image_size=image_size,
position=pos)
for lidar_stream_name in lidar_stream_names:
self.__add_lidar(name=lidar_stream_name)
self.agent_id_map = {}
self.pedestrian_count = 0
@staticmethod
def setup_streams(input_streams, camera_setups, lidar_stream_names):
input_streams.add_callback(CarlaOperator.update_control)
camera_streams = [
DataStream(name=camera,
labels={
'sensor_type': 'camera',
'camera_type': camera_type
}) for (camera, camera_type, _, _) in camera_setups
]
lidar_streams = [
DataStream(name=lidar, labels={'sensor_type': 'lidar'})
for lidar in lidar_stream_names
]
return [
DataStream(name='world_transform'),
DataStream(name='vehicle_pos'),
DataStream(name='acceleration'),
DataStream(data_type=Float64, name='forward_speed'),
DataStream(data_type=Float64, name='vehicle_collisions'),
DataStream(data_type=Float64, name='pedestrian_collisions'),
DataStream(data_type=Float64, name='other_collisions'),
DataStream(data_type=Float64, name='other_lane'),
DataStream(data_type=Float64, name='offroad'),
DataStream(name='traffic_lights'),
DataStream(name='pedestrians'),
DataStream(name='vehicles'),
DataStream(name='traffic_signs'),
] + camera_streams + lidar_streams
def __add_camera(self,
name,
postprocessing,
image_size=(800, 600),
field_of_view=90.0,
position=(0.3, 0, 1.3),
rotation_pitch=0,
rotation_roll=0,
rotation_yaw=0):
"""Adds a camera and a corresponding output stream.
Args:
name: A string naming the camera.
postprocessing: "SceneFinal", "Depth", "SemanticSegmentation".
"""
camera = Camera(name,
PostProcessing=postprocessing,
FOV=field_of_view,
ImageSizeX=image_size[0],
ImageSizeY=image_size[1],
PositionX=position[0],
PositionY=position[1],
PositionZ=position[2],
RotationPitch=rotation_pitch,
RotationRoll=rotation_roll,
RotationYaw=rotation_yaw)
self.settings.add_sensor(camera)
def __add_lidar(self,
name,
channels=32,
max_range=50,
points_per_second=100000,
rotation_frequency=10,
upper_fov_limit=10,
lower_fov_limit=-30,
position=(0, 0, 1.4),
rotation_pitch=0,
rotation_yaw=0,
rotation_roll=0):
"""Adds a LIDAR sensor and a corresponding output stream.
Args:
name: A string naming the camera.
"""
lidar = Lidar(name,
Channels=channels,
Range=max_range,
PointsPerSecond=points_per_second,
RotationFrequency=rotation_frequency,
UpperFovLimit=upper_fov_limit,
LowerFovLimit=lower_fov_limit,
PositionX=position[0],
PositionY=position[1],
PositionZ=position[2],
RotationPitch=rotation_pitch,
RotationYaw=rotation_yaw,
RotationRoll=rotation_roll)
self.settings.add_sensor(lidar)
output_stream = DataStream(name=name, labels={"sensor_type": "lidar"})
self.lidar_streams.append(output_stream)
def read_carla_data(self):
read_start_time = time.time()
measurements, sensor_data = self.client.read_data()
measure_time = time.time()
self._logger.info(
'Got readings for game time {} and platform time {}'.format(
measurements.game_timestamp, measurements.platform_timestamp))
# Send measurements
player_measurements = measurements.player_measurements
vehicle_pos = ((player_measurements.transform.location.x,
player_measurements.transform.location.y,
player_measurements.transform.location.z),
(player_measurements.transform.orientation.x,
player_measurements.transform.orientation.y,
player_measurements.transform.orientation.z))
world_transform = Transform(player_measurements.transform)
timestamp = Timestamp(
coordinates=[measurements.game_timestamp, self.message_num])
self.message_num += 1
ray.register_custom_serializer(Message, use_pickle=True)
ray.register_custom_serializer(WatermarkMessage, use_pickle=True)
watermark = WatermarkMessage(timestamp)
self.get_output_stream('world_transform').send(
Message(world_transform, timestamp))
self.get_output_stream('world_transform').send(watermark)
self.get_output_stream('vehicle_pos').send(
Message(vehicle_pos, timestamp))
self.get_output_stream('vehicle_pos').send(watermark)
acceleration = (player_measurements.acceleration.x,
player_measurements.acceleration.y,
player_measurements.acceleration.z)
self.get_output_stream('acceleration').send(
Message(acceleration, timestamp))
self.get_output_stream('acceleration').send(watermark)
self.get_output_stream('forward_speed').send(
Message(player_measurements.forward_speed, timestamp))
self.get_output_stream('forward_speed').send(watermark)
self.get_output_stream('vehicle_collisions').send(
Message(player_measurements.collision_vehicles, timestamp))
self.get_output_stream('vehicle_collisions').send(watermark)
self.get_output_stream('pedestrian_collisions').send(
Message(player_measurements.collision_pedestrians, timestamp))
self.get_output_stream('pedestrian_collisions').send(watermark)
self.get_output_stream('other_collisions').send(
Message(player_measurements.collision_other, timestamp))
self.get_output_stream('other_collisions').send(watermark)
self.get_output_stream('other_lane').send(
Message(player_measurements.intersection_otherlane, timestamp))
self.get_output_stream('other_lane').send(watermark)
self.get_output_stream('offroad').send(
Message(player_measurements.intersection_offroad, timestamp))
self.get_output_stream('offroad').send(watermark)
vehicles = []
pedestrians = []
traffic_lights = []
speed_limit_signs = []
for agent in measurements.non_player_agents:
if agent.HasField('vehicle'):
pos = messages.Transform(agent.vehicle.transform)
bb = messages.BoundingBox(agent.vehicle.bounding_box)
forward_speed = agent.vehicle.forward_speed
vehicle = messages.Vehicle(pos, bb, forward_speed)
vehicles.append(vehicle)
elif agent.HasField('pedestrian'):
if not self.agent_id_map.get(agent.id):
self.pedestrian_count += 1
self.agent_id_map[agent.id] = self.pedestrian_count
pedestrian_index = self.agent_id_map[agent.id]
pos = messages.Transform(agent.pedestrian.transform)
bb = messages.BoundingBox(agent.pedestrian.bounding_box)
forward_speed = agent.pedestrian.forward_speed
pedestrian = messages.Pedestrian(pedestrian_index, pos, bb,
forward_speed)
pedestrians.append(pedestrian)
elif agent.HasField('traffic_light'):
transform = messages.Transform(agent.traffic_light.transform)
traffic_light = messages.TrafficLight(
transform, agent.traffic_light.state)
traffic_lights.append(traffic_light)
elif agent.HasField('speed_limit_sign'):
transform = messages.Transform(
agent.speed_limit_sign.transform)
speed_sign = messages.SpeedLimitSign(
transform, agent.speed_limit_sign.speed_limit)
speed_limit_signs.append(speed_sign)
vehicles_msg = Message(vehicles, timestamp)
self.get_output_stream('vehicles').send(vehicles_msg)
self.get_output_stream('vehicles').send(watermark)
pedestrians_msg = Message(pedestrians, timestamp)
self.get_output_stream('pedestrians').send(pedestrians_msg)
self.get_output_stream('pedestrians').send(watermark)
traffic_lights_msg = Message(traffic_lights, timestamp)
self.get_output_stream('traffic_lights').send(traffic_lights_msg)
self.get_output_stream('traffic_lights').send(watermark)
traffic_sings_msg = Message(speed_limit_signs, timestamp)
self.get_output_stream('traffic_signs').send(traffic_sings_msg)
self.get_output_stream('traffic_signs').send(watermark)
# Send sensor data
for name, measurement in sensor_data.items():
data_stream = self.get_output_stream(name)
if data_stream.get_label('camera_type') == 'SceneFinal':
# Transform the Carla RGB images to BGR.
data_stream.send(
Message(
pylot_utils.bgra_to_bgr(to_bgra_array(measurement)),
timestamp))
else:
data_stream.send(Message(measurement, timestamp))
data_stream.send(watermark)
self.client.send_control(**self.control)
end_time = time.time()
measurement_runtime = (measure_time - read_start_time) * 1000
total_runtime = (end_time - read_start_time) * 1000
self._logger.info('Carla measurement time {}; total time {}'.format(
measurement_runtime, total_runtime))
self._csv_logger.info('{},{},{},{}'.format(time_epoch_ms(), self.name,
measurement_runtime,
total_runtime))
def read_data_at_frequency(self):
period = 1.0 / self._flags.carla_step_frequency
trigger_at = time.time() + period
while True:
time_until_trigger = trigger_at - time.time()
if time_until_trigger > 0:
time.sleep(time_until_trigger)
else:
self._logger.error(
'Cannot read Carla data at frequency {}'.format(
self._flags.carla_step_frequency))
self.read_carla_data()
trigger_at += period
def update_control(self, msg):
"""Updates the control dict"""
self.control.update(msg.data)
def execute(self):
# Subscribe to control streams
self.control = {
'steer': 0.0,
'throttle': 0.0,
'break': 0.0,
'hand_break': False,
'reverse': False
}
self.client = CarlaClient(self._flags.carla_host,
self._flags.carla_port,
timeout=10)
self.client.connect()
scene = self.client.load_settings(self.settings)
# Choose one player start at random.
number_of_player_starts = len(scene.player_start_spots)
player_start = self._flags.carla_start_player_num
if self._flags.carla_random_player_start:
player_start = np.random.randint(
0, max(0, number_of_player_starts - 1))
self.client.start_episode(player_start)
self.read_data_at_frequency()
self.spin()
class 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
]
self.distance_from_goal = np.linalg.norm(
np.array(self.location[:2]) -
[self.current_goal.location.x, self.current_goal.location.y])
is_collision = measurements.player_measurements.collision_vehicles != 0 \
or measurements.player_measurements.collision_pedestrians != 0 \
or measurements.player_measurements.collision_other != 0
speed_reward = measurements.player_measurements.forward_speed - 1
if speed_reward > 30.:
speed_reward = 30.
self.reward = speed_reward \
- (measurements.player_measurements.intersection_otherlane * 5) \
- (measurements.player_measurements.intersection_offroad * 5) \
- is_collision * 100 \
- np.abs(self.control.steer) * 10
# update measurements
self.measurements = [measurements.player_measurements.forward_speed
] + self.location
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:
self.done = True
self.state['measurements'] = np.array(self.measurements)
def _take_action(self, action):
self.control = VehicleControl()
if self.separate_actions_for_throttle_and_brake:
self.control.steer = np.clip(action[0], -1, 1)
self.control.throttle = np.clip(action[1], 0, 1)
self.control.brake = np.clip(action[2], 0, 1)
else:
# transform the 2 value action (steer, throttle - brake) into a 3 value action (steer, throttle, brake)
self.control.steer = np.clip(action[0], -1, 1)
self.control.throttle = np.clip(action[1], 0, 1)
self.control.brake = np.abs(np.clip(action[1], -1, 0))
# prevent braking
if not self.allow_braking or self.control.brake < 0.1 or self.control.throttle > self.control.brake:
self.control.brake = 0
# prevent over speeding
if hasattr(self, 'measurements') and self.measurements[
0] * 3.6 > self.max_speed and self.control.brake == 0:
self.control.throttle = 0.0
self.control.hand_brake = False
self.control.reverse = False
self.game.send_control(self.control)
def _load_experiment(self, experiment_idx):
self.current_experiment = self.experiment_suite.get_experiments(
)[experiment_idx]
self.scene = self.game.load_settings(
self.current_experiment.conditions)
self.positions = self.scene.player_start_spots
self.num_positions = len(self.positions)
self.current_start_position_idx = 0
self.current_pose = 0
def _restart_environment_episode(self, force_environment_reset=False):
# select start and end positions
if self.experiment_suite:
# if an expeirent suite is available, follow its given poses
if self.current_pose >= len(self.current_experiment.poses):
# load a new experiment
self.current_experiment_idx = (
self.current_experiment_idx + 1) % len(
self.experiment_suite.get_experiments())
self._load_experiment(self.current_experiment_idx)
self.current_start_position_idx = self.current_experiment.poses[
self.current_pose][0]
self.current_goal = self.positions[self.current_experiment.poses[
self.current_pose][1]]
self.current_pose += 1
else:
# go over all the possible positions in a cyclic manner
self.current_start_position_idx = (
self.current_start_position_idx + 1) % self.num_positions
# choose a random goal destination
self.current_goal = random.choice(self.positions)
try:
self.game.start_episode(self.current_start_position_idx)
except:
self.game.connect()
self.game.start_episode(self.current_start_position_idx)
# start the game with some initial speed
for i in range(self.num_speedup_steps):
self.control = VehicleControl(throttle=1.0,
brake=0,
steer=0,
hand_brake=False,
reverse=False)
self.game.send_control(VehicleControl())
def get_rendered_image(self) -> np.ndarray:
"""
Return a numpy array containing the image that will be rendered to the screen.
This can be different from the observation. For example, mujoco's observation is a measurements vector.
:return: numpy array containing the image that will be rendered to the screen
"""
image = [self.state[camera.name] for camera in self.scene.sensors]
image = np.vstack(image)
return image
class CarlaEnvironment(Environment):
def __init__(self, level: LevelSelection, seed: int, frame_skip: int,
human_control: bool, custom_reward_threshold: Union[int,
float],
visualization_parameters: VisualizationParameters,
server_height: int, server_width: int, camera_height: int,
camera_width: int, verbose: bool, config: str,
episode_max_time: int, allow_braking: bool,
quality: CarlaEnvironmentParameters.Quality,
cameras: List[CameraTypes], weather_id: List[int], **kwargs):
super().__init__(level, seed, frame_skip, human_control,
custom_reward_threshold, visualization_parameters)
# server configuration
self.server_height = server_height
self.server_width = server_width
self.port = get_open_port()
self.host = 'localhost'
self.map = self.env_id
# client configuration
self.verbose = verbose
self.quality = quality
self.cameras = cameras
self.weather_id = weather_id
self.episode_max_time = episode_max_time
self.allow_braking = allow_braking
self.camera_width = camera_width
self.camera_height = camera_height
# state space
self.state_space = StateSpace({
"measurements":
VectorObservationSpace(
4, measurements_names=["forward_speed", "x", "y", "z"])
})
for camera in self.cameras:
self.state_space[camera.value] = ImageObservationSpace(
shape=np.array([self.camera_height, self.camera_width, 3]),
high=255)
# setup server settings
self.config = config
if self.config:
# load settings from file
with open(self.config, 'r') as fp:
self.settings = fp.read()
else:
# hard coded settings
self.settings = CarlaSettings()
self.settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=15,
NumberOfPedestrians=30,
WeatherId=random.choice(
force_list(self.weather_id)),
QualityLevel=self.quality.value)
self.settings.randomize_seeds()
self.settings = self._add_cameras(self.settings, self.cameras,
self.camera_width,
self.camera_height)
# open the server
self.server = self._open_server()
logging.disable(40)
# open the client
self.game = CarlaClient(self.host, self.port, timeout=99999999)
self.game.connect()
scene = self.game.load_settings(self.settings)
# get available start positions
positions = scene.player_start_spots
self.num_pos = len(positions)
self.iterator_start_positions = 0
# action space
self.action_space = BoxActionSpace(shape=2,
low=np.array([-1, -1]),
high=np.array([1, 1]))
# human control
if self.human_control:
# convert continuous action space to discrete
self.steering_strength = 0.5
self.gas_strength = 1.0
self.brake_strength = 0.5
self.action_space = PartialDiscreteActionSpaceMap(
target_actions=[[0., 0.], [0., -self.steering_strength],
[0., self.steering_strength],
[self.gas_strength, 0.],
[-self.brake_strength, 0],
[self.gas_strength, -self.steering_strength],
[self.gas_strength, self.steering_strength],
[self.brake_strength, -self.steering_strength],
[self.brake_strength, self.steering_strength]],
target_action_space=self.action_space,
descriptions=[
'NO-OP', 'TURN_LEFT', 'TURN_RIGHT', 'GAS', 'BRAKE',
'GAS_AND_TURN_LEFT', 'GAS_AND_TURN_RIGHT',
'BRAKE_AND_TURN_LEFT', 'BRAKE_AND_TURN_RIGHT'
])
# map keyboard keys to actions
for idx, action in enumerate(self.action_space.descriptions):
for key in key_map.keys():
if action == key:
self.key_to_action[key_map[key]] = idx
self.num_speedup_steps = 30
# measurements
self.autopilot = None
# env initialization
self.reset_internal_state(True)
# render
if self.is_rendered:
image = self.get_rendered_image()
self.renderer.create_screen(image.shape[1], image.shape[0])
def _add_cameras(self, settings, cameras, camera_width, camera_height):
# add a front facing camera
if CameraTypes.FRONT in cameras:
camera = Camera(CameraTypes.FRONT.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, 0, 0)
settings.add_sensor(camera)
# add a left facing camera
if CameraTypes.LEFT in cameras:
camera = Camera(CameraTypes.LEFT.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, -30, 0)
settings.add_sensor(camera)
# add a right facing camera
if CameraTypes.RIGHT in cameras:
camera = Camera(CameraTypes.RIGHT.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, 30, 0)
settings.add_sensor(camera)
# add a front facing depth camera
if CameraTypes.DEPTH in cameras:
camera = Camera(CameraTypes.DEPTH.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, 30, 0)
camera.PostProcessing = 'Depth'
settings.add_sensor(camera)
# add a front facing semantic segmentation camera
if CameraTypes.SEGMENTATION in cameras:
camera = Camera(CameraTypes.SEGMENTATION.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, 30, 0)
camera.PostProcessing = 'SemanticSegmentation'
settings.add_sensor(camera)
return settings
def _open_server(self):
# TODO: get experiment path
log_path = path.join('./logs/', "CARLA_LOG_{}.txt".format(self.port))
with open(log_path, "wb") as out:
cmd = [
path.join(environ.get('CARLA_ROOT'),
'CarlaUE4.sh'), self.map, "-benchmark",
"-carla-server", "-fps={}".format(30 / self.frame_skip),
"-world-port={}".format(self.port),
"-windowed -ResX={} -ResY={}".format(self.server_width,
self.server_height),
"-carla-no-hud"
]
if self.config:
cmd.append("-carla-settings={}".format(self.config))
p = subprocess.Popen(cmd, stdout=out, stderr=out)
return p
def _close_server(self):
os.killpg(os.getpgid(self.server.pid), signal.SIGKILL)
def _update_state(self):
# get measurements and observations
measurements = []
while type(measurements) == list:
measurements, sensor_data = self.game.read_data()
self.state = {}
for camera in self.cameras:
self.state[camera.value] = sensor_data[camera.value].data
self.location = [
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
]
is_collision = measurements.player_measurements.collision_vehicles != 0 \
or measurements.player_measurements.collision_pedestrians != 0 \
or measurements.player_measurements.collision_other != 0
speed_reward = measurements.player_measurements.forward_speed - 1
if speed_reward > 30.:
speed_reward = 30.
self.reward = speed_reward \
- (measurements.player_measurements.intersection_otherlane * 5) \
- (measurements.player_measurements.intersection_offroad * 5) \
- is_collision * 100 \
- np.abs(self.control.steer) * 10
# update measurements
self.measurements = [measurements.player_measurements.forward_speed
] + self.location
self.autopilot = measurements.player_measurements.autopilot_control
# action_p = ['%.2f' % member for member in [self.control.throttle, self.control.steer]]
# screen.success('REWARD: %.2f, ACTIONS: %s' % (self.reward, action_p))
if (measurements.game_timestamp >=
self.episode_max_time) or is_collision:
# screen.success('EPISODE IS DONE. GameTime: {}, Collision: {}'.format(str(measurements.game_timestamp),
# str(is_collision)))
self.done = True
self.state['measurements'] = self.measurements
def _take_action(self, action):
self.control = VehicleControl()
self.control.throttle = np.clip(action[0], 0, 1)
self.control.steer = np.clip(action[1], -1, 1)
self.control.brake = np.abs(np.clip(action[0], -1, 0))
if not self.allow_braking:
self.control.brake = 0
self.control.hand_brake = False
self.control.reverse = False
self.game.send_control(self.control)
def _restart_environment_episode(self, force_environment_reset=False):
self.iterator_start_positions += 1
if self.iterator_start_positions >= self.num_pos:
self.iterator_start_positions = 0
try:
self.game.start_episode(self.iterator_start_positions)
except:
self.game.connect()
self.game.start_episode(self.iterator_start_positions)
# start the game with some initial speed
for i in range(self.num_speedup_steps):
self._take_action([1.0, 0])
def get_rendered_image(self) -> np.ndarray:
"""
Return a numpy array containing the image that will be rendered to the screen.
This can be different from the observation. For example, mujoco's observation is a measurements vector.
:return: numpy array containing the image that will be rendered to the screen
"""
image = [self.state[camera.value] for camera in self.cameras]
image = np.vstack(image)
return image
class CarlaEnv(gym.Env):
def __init__(self, config=ENV_CONFIG):
"""
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
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_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(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)
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
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 CarlaEnvironment(EnvironmentInterface):
def __init__(self,
experiment_path=None,
frame_skip=1,
server_height=512,
server_width=720,
camera_height=88,
camera_width=200,
experiment_suite=None,
quality="low",
cameras=[CameraTypes.FRONT],
weather_id=[1],
episode_max_time=30000,
max_speed=35.0,
port=2000,
map_name="Town01",
verbose=True,
seed=None,
is_rendered=True,
num_speedup_steps=30,
separate_actions_for_throttle_and_brake=False,
rendred_image_type='forward_camera'):
self.frame_skip = frame_skip # the frame skip affects the fps of the server directly. fps = 30 / frameskip
self.server_height = server_height
self.server_width = server_width
self.camera_height = camera_height
self.camera_width = camera_width
self.experiment_suite = experiment_suite # an optional CARLA experiment suite to use
self.quality = quality
self.cameras = cameras
self.weather_id = weather_id
self.episode_max_time = episode_max_time # miliseconds for each episode
self.max_speed = max_speed # km/h
self.port = port
self.host = 'localhost'
self.map_name = map_name
self.map_path = map_path_mapper[self.map_name]
self.experiment_path = experiment_path
self.current_episode_steps_counter = 1
# client configuration
self.verbose = verbose
self.episode_idx = 0
self.num_speedup_steps = num_speedup_steps
self.max_speed = max_speed
self.is_rendered = is_rendered
# setup server settings
self.experiment_suite = experiment_suite
self.separate_actions_for_throttle_and_brake = separate_actions_for_throttle_and_brake
self.rendred_image_type = rendred_image_type
self.left_poses = []
self.right_poses = []
self.follow_poses = []
self.Straight_poses = []
self.settings = CarlaSettings()
self.settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=15,
NumberOfPedestrians=30,
WeatherId=self.weather_id,
QualityLevel=self.quality,
SeedVehicles=seed,
SeedPedestrians=seed)
if seed is None:
self.settings.randomize_seeds()
self.settings = self.add_cameras(self.settings, self.cameras,
self.camera_width, self.camera_height)
# open the server
self.server = self.open_server()
logging.disable(40)
print("Successfully opened the server")
# open the client
self.game = CarlaClient(self.host, self.port, timeout=99999999)
print("Successfully opened the client")
self.game.connect()
print("Successfull Connection")
if self.experiment_suite:
self.current_experiment_idx = 0
self.current_experiment = self.experiment_suite.get_experiments()[
self.current_experiment_idx]
self.scene = self.game.load_settings(
self.current_experiment.conditions)
else:
self.scene = self.game.load_settings(self.settings)
# get available start positions
self.positions = self.scene.player_start_spots
self.num_positions = len(self.positions)
self.current_start_position_idx = 0
self.current_pose = 0
self.action_space = ActionSpace(
shape=2,
low=np.array([-1, -1]),
high=np.array([1, 1]),
descriptions=["steer", "gas_and_brake"])
# state space
#define all measurments
self.state_space = {
"measurements": {
"forward_speed": np.array([1]),
"x": np.array([1]),
"y": np.array([1]),
"z": np.array([1])
}
}
#define all cameras
for camera in self.scene.sensors:
self.state_space[camera.name] = {
"data": np.array([self.camera_height, self.camera_width, 3])
}
print("Define ", camera.name, " Camera")
# measurements
self.autopilot = None
self.planner = Planner(self.map_name)
#rendering
if self.is_rendered:
pygame.init()
pygame.font.init()
self.display = pygame.display.set_mode(
(self.camera_width, self.camera_height))
# env initialization
self.reset(True)
def reset(self, force_environment_reset=False):
"""
Reset the environment and all the variable of the wrapper
:param force_environment_reset: forces environment reset even when the game did not end
:return: A dictionary containing the observation, reward, done flag, action and measurements
"""
self.reset_ep = True
self.restart_environment_episode(force_environment_reset)
self.last_episode_time = time.time()
if self.current_episode_steps_counter > 0:
self.episode_idx += 1
self.done = False
self.total_reward_in_current_episode = self.reward = 0.0
self.last_action = 0
self.current_episode_steps_counter = 0
self.last_episode_images = []
self.step([0, 1, 0])
self.last_env_response = {
"reward": self.reward,
"next_state": self.state,
"goal": self.current_goal,
"game_over": self.done
}
return self.last_env_response
def add_cameras(self, settings, cameras, camera_width, camera_height):
# add a front facing camera
print("Available Cameras are ", cameras)
if CameraTypes.FRONT in cameras:
camera = Camera(CameraTypes.FRONT.value)
camera.set(FOV=100)
camera.set_image_size(camera_width, camera_height)
camera.set_position(2.0, 0, 1.4)
camera.set_rotation(-15.0, 0, 0)
settings.add_sensor(camera)
# add a left facing camera
if CameraTypes.LEFT in cameras:
camera = Camera(CameraTypes.LEFT.value)
camera.set(FOV=100)
camera.set_image_size(camera_width, camera_height)
camera.set_position(2.0, 0, 1.4)
camera.set_rotation(-15.0, -30, 0)
settings.add_sensor(camera)
# add a right facing camera
if CameraTypes.RIGHT in cameras:
camera = Camera(CameraTypes.RIGHT.value)
camera.set(FOV=100)
camera.set_image_size(camera_width, camera_height)
camera.set_position(2.0, 0, 1.4)
camera.set_rotation(-15.0, 30, 0)
settings.add_sensor(camera)
# add a front facing depth camera
if CameraTypes.DEPTH in cameras:
camera = Camera(CameraTypes.DEPTH.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, 30, 0)
camera.PostProcessing = 'Depth'
settings.add_sensor(camera)
# add a front facing semantic segmentation camera
if CameraTypes.SEGMENTATION in cameras:
camera = Camera(CameraTypes.SEGMENTATION.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, 30, 0)
camera.PostProcessing = 'SemanticSegmentation'
settings.add_sensor(camera)
print("Successfully adding a SemanticSegmentation camera")
return settings
def get_directions(self, current_point, end_point):
"""
Class that should return the directions to reach a certain goal
"""
directions = self.planner.get_next_command(
(current_point.location.x, current_point.location.y, 0.22),
(current_point.orientation.x, current_point.orientation.y,
current_point.orientation.z),
(end_point.location.x, end_point.location.y, 0.22),
(end_point.orientation.x, end_point.orientation.y,
end_point.orientation.z))
return directions
def open_server(self):
log_path = path.join(
self.experiment_path if self.experiment_path is not None else '.',
'logs', "CARLA_LOG_{}.txt".format(self.port))
if not os.path.exists(os.path.dirname(log_path)):
os.makedirs(os.path.dirname(log_path))
with open(log_path, "wb") as out:
cmd = [
path.join(environ.get('CARLA_ROOT'),
'CarlaUE4.sh'), self.map_path, "-benchmark",
"-carla-server", "-fps={}".format(30 / self.frame_skip),
"-world-port={}".format(self.port),
"-windowed -ResX={} -ResY={}".format(self.server_width,
self.server_height),
"-carla-no-hud"
]
print("CMD is : ", cmd)
# if self.config:
# cmd.append("-carla-settings={}".format(self.config))
p = subprocess.Popen(cmd, stdout=out, stderr=out)
return p
def close_server(self):
os.killpg(os.getpgid(self.server.pid), signal.SIGKILL)
def step(self, action):
# get measurements and observations
measurements = []
while type(measurements) == list:
measurements, sensor_data = self.game.read_data()
self.state = {}
for camera in self.scene.sensors:
if camera.name == 'segmentation':
#labels_to_road_noroad taker Sensor.Image not numpy array
self.state[camera.name] = labels_to_road_noroad(
sensor_data[camera.name])
else:
self.state[camera.name] = sensor_data[camera.name].data
#self.state[camera.name] = sensor_data[camera.name].data
self.location = [
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
]
self.distance_from_goal = np.linalg.norm(
np.array(self.location[:2]) -
[self.current_goal.location.x, self.current_goal.location.y])
is_collision = measurements.player_measurements.collision_vehicles != 0 \
or measurements.player_measurements.collision_pedestrians != 0 \
or measurements.player_measurements.collision_other != 0
speed_reward = measurements.player_measurements.forward_speed - 1
if speed_reward > 30.:
speed_reward = 30.
self.reward = speed_reward \
- (measurements.player_measurements.intersection_otherlane * 5) \
- (measurements.player_measurements.intersection_offroad * 5) \
- is_collision * 100 \
- np.abs(self.control.steer) * 10
# update measurements
#self.measurements = [measurements.player_measurements.forward_speed] + self.location
#TODO, Add control signals to measurements
control_signals = [
np.clip(action[0], -1, 1),
np.clip(action[1], 0, 1),
np.clip(action[2], 0, 1)
] #steer, throttle, brake
self.measurements = [measurements.player_measurements.forward_speed
] + self.location + control_signals
self.autopilot = measurements.player_measurements.autopilot_control
# The directions to reach the goal (0 Follow lane, 1 Left, 2 Right, 3 Straight)
directions = int(
self.get_directions(measurements.player_measurements.transform,
self.current_goal) - 2)
# if directions == 0:
# if self.reset_ep:
# self.follow_poses.append((self.current_start,self.current_goal))
# self.reset_ep = False
# elif directions == 1:
# self.left_poses.append((self.current_start,self.current_goal))
# elif directions == 2:
# self.right_poses.append((self.current_start,self.current_goal))
# elif directions == 1:
# if self.reset_ep:
# self.left_poses.append((self.current_start,self.current_goal))
# self.reset_ep = False
# elif directions == 2:
# if self.reset_ep:
# self.right_poses.append((self.current_start,self.current_goal))
# self.reset_ep = False
# elif directions == 3:
# if self.reset_ep:
# self.Straight_poses.append((self.current_start,self.current_goal))
# self.reset_ep = False
# else:
# self.reset_ep = False
self.state['high_level_command'] = directions
if (measurements.game_timestamp >=
self.episode_max_time) or is_collision:
self.done = True
self.state['measurements'] = np.array(self.measurements)
#prepare rendered image and update display
if self.is_rendered:
#check if user wants to close rendering, else continue rendering
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
self.is_rendered = False
self.surface = pygame.surfarray.make_surface(
self.state[self.rendred_image_type].swapaxes(0, 1))
self.display.blit(self.surface, (0, 0))
pygame.display.flip()
self.take_action(action)
def take_action(self, action):
self.control = VehicleControl()
# transform the 2 value action (steer, throttle - brake) into a 3 value action (steer, throttle, brake)
self.control.steer = np.clip(action[0], -1, 1)
self.control.throttle = np.clip(action[1], 0, 1)
self.control.brake = np.abs(np.clip(action[2], 0, 1))
# prevent braking
# if not self.allow_braking or self.control.brake < 0.1 or self.control.throttle > self.control.brake:
# self.control.brake = 0
# prevent over speeding
if hasattr(self, 'measurements') and self.measurements[
0] * 3.6 > self.max_speed and self.control.brake == 0:
self.control.throttle = 0.0
self.control.hand_brake = False
self.control.reverse = False
self.game.send_control(self.control)
def load_experiment(self, experiment_idx):
print("Loading the experiment")
self.current_experiment = self.experiment_suite.get_experiments(
)[experiment_idx]
self.scene = self.game.load_settings(
self.current_experiment.conditions)
self.positions = self.scene.player_start_spots
self.num_positions = len(self.positions)
self.current_start_position_idx = 0
self.current_pose = 0
def restart_environment_episode(self, force_environment_reset=False):
# select start and end positions
print("restarting new Episode")
if self.experiment_suite:
# if an expeirent suite is available, follow its given poses
if self.current_pose >= len(self.current_experiment.poses):
# load a new experiment
self.current_experiment_idx = (
self.current_experiment_idx + 1) % len(
self.experiment_suite.get_experiments())
self.load_experiment(self.current_experiment_idx)
self.current_start_position_idx = self.current_experiment.poses[
self.current_pose][0]
self.current_start = self.positions[self.current_experiment.poses[
self.current_pose][0]]
self.current_goal = self.positions[self.current_experiment.poses[
self.current_pose][1]]
self.current_pose += 1
else:
# go over all the possible positions in a cyclic manner
self.current_start_position_idx = (
self.current_start_position_idx + 1) % self.num_positions
self.current_start = self.positions[
self.current_start_position_idx]
# choose a random goal destination
self.current_goal = random.choice(self.positions)
try:
self.game.start_episode(self.current_start_position_idx)
except:
self.game.connect()
self.game.start_episode(self.current_start_position_idx)
# start the game with some initial speed
for i in range(self.num_speedup_steps):
self.control = VehicleControl(throttle=1.0,
brake=0,
steer=0,
hand_brake=False,
reverse=False)
self.game.send_control(VehicleControl())
def get_rendered_image(self) -> np.ndarray:
"""
Return a numpy array containing the image that will be rendered to the screen.
This can be different from the observation. For example, mujoco's observation is a measurements vector.
:return: numpy array containing the image that will be rendered to the screen
"""
image = [self.state[camera.name] for camera in self.scene.sensors]
image = np.vstack(image)
return image
class CarlaEnv(gym.Env):
def __init__(self, config=ENV_CONFIG):
self.config = config
self.city = self.config["server_map"].split("/")[-1]
if self.config["enable_planner"]:
self.planner = Planner(self.city)
if config["discrete_actions"]:
self.action_space = Discrete(len(DISCRETE_ACTIONS))
else:
self.action_space = Box(-1.0, 1.0, shape=(2, ), dtype=np.float32)
if config["use_depth_camera"]:
image_space = Box(
-1.0,
1.0,
shape=(config["y_res"], config["x_res"],
1 * config["framestack"]),
dtype=np.float32)
else:
image_space = Box(
0,
255,
shape=(config["y_res"], config["x_res"],
3 * config["framestack"]),
dtype=np.uint8)
self.observation_space = Tuple( # forward_speed, dist to goal
[
image_space,
Discrete(len(COMMANDS_ENUM)), # next_command
Box(-128.0, 128.0, shape=(2, ), dtype=np.float32)
])
# TODO(ekl) this isn't really a proper gym spec
self._spec = lambda: None
self._spec.id = "Carla-v0"
self.server_port = None
self.server_process = None
self.client = None
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.prev_image = None
self.episode_id = None
self.measurements_file = None
self.weather = None
self.scenario = None
self.start_pos = None
self.end_pos = None
self.start_coord = None
self.end_coord = None
self.last_obs = None
def init_server(self):
print("Initializing new Carla server...")
# Create a new server process and start the client.
self.server_port = random.randint(10000, 60000)
self.server_process = subprocess.Popen(
[
SERVER_BINARY, self.config["server_map"], "-windowed",
"-ResX=400", "-ResY=300", "-carla-server",
"-carla-world-port={}".format(self.server_port)
],
preexec_fn=os.setsid,
stdout=open(os.devnull, "w"))
live_carla_processes.add(os.getpgid(self.server_process.pid))
for i in range(RETRIES_ON_ERROR):
try:
self.client = CarlaClient("localhost", self.server_port)
return self.client.connect()
except Exception as e:
print("Error connecting: {}, attempt {}".format(e, i))
time.sleep(2)
def clear_server_state(self):
print("Clearing Carla server state")
try:
if self.client:
self.client.disconnect()
self.client = None
except Exception as e:
print("Error disconnecting client: {}".format(e))
pass
if self.server_process:
pgid = os.getpgid(self.server_process.pid)
os.killpg(pgid, signal.SIGKILL)
live_carla_processes.remove(pgid)
self.server_port = None
self.server_process = None
def __del__(self):
self.clear_server_state()
def reset(self):
error = None
for _ in range(RETRIES_ON_ERROR):
try:
if not self.server_process:
self.init_server()
return self._reset()
except Exception as e:
print("Error during reset: {}".format(traceback.format_exc()))
self.clear_server_state()
error = e
raise error
def _reset(self):
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.prev_image = None
self.episode_id = datetime.today().strftime("%Y-%m-%d_%H-%M-%S_%f")
self.measurements_file = None
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
self.scenario = random.choice(self.config["scenarios"])
assert self.scenario["city"] == self.city, (self.scenario, self.city)
self.weather = random.choice(self.scenario["weather_distribution"])
settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self.scenario["num_vehicles"],
NumberOfPedestrians=self.scenario["num_pedestrians"],
WeatherId=self.weather)
settings.randomize_seeds()
if self.config["use_depth_camera"]:
camera1 = Camera("CameraDepth", PostProcessing="Depth")
camera1.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
camera1.set_position(30, 0, 130)
settings.add_sensor(camera1)
camera2 = Camera("CameraRGB")
camera2.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
camera2.set_position(30, 0, 130)
settings.add_sensor(camera2)
# Setup start and end positions
scene = self.client.load_settings(settings)
positions = scene.player_start_spots
self.start_pos = positions[self.scenario["start_pos_id"]]
self.end_pos = positions[self.scenario["end_pos_id"]]
self.start_coord = [
self.start_pos.location.x // 100, self.start_pos.location.y // 100
]
self.end_coord = [
self.end_pos.location.x // 100, self.end_pos.location.y // 100
]
print("Start pos {} ({}), end {} ({})".format(
self.scenario["start_pos_id"], self.start_coord,
self.scenario["end_pos_id"], self.end_coord))
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print("Starting new episode...")
self.client.start_episode(self.scenario["start_pos_id"])
image, py_measurements = self._read_observation()
self.prev_measurement = py_measurements
return self.encode_obs(self.preprocess_image(image), py_measurements)
def encode_obs(self, image, py_measurements):
assert self.config["framestack"] in [1, 2]
prev_image = self.prev_image
self.prev_image = image
if prev_image is None:
prev_image = image
if self.config["framestack"] == 2:
image = np.concatenate([prev_image, image], axis=2)
obs = (image, COMMAND_ORDINAL[py_measurements["next_command"]], [
py_measurements["forward_speed"],
py_measurements["distance_to_goal"]
])
self.last_obs = obs
return obs
def step(self, action):
try:
obs = self._step(action)
return obs
except Exception:
print("Error during step, terminating episode early",
traceback.format_exc())
self.clear_server_state()
return (self.last_obs, 0.0, True, {})
def _step(self, action):
if self.config["discrete_actions"]:
action = DISCRETE_ACTIONS[int(action)]
assert len(action) == 2, "Invalid action {}".format(action)
if self.config["squash_action_logits"]:
forward = 2 * float(sigmoid(action[0]) - 0.5)
throttle = float(np.clip(forward, 0, 1))
brake = float(np.abs(np.clip(forward, -1, 0)))
steer = 2 * float(sigmoid(action[1]) - 0.5)
else:
throttle = float(np.clip(action[0], 0, 1))
brake = float(np.abs(np.clip(action[0], -1, 0)))
steer = float(np.clip(action[1], -1, 1))
reverse = False
hand_brake = False
if self.config["verbose"]:
print("steer", steer, "throttle", throttle, "brake", brake,
"reverse", reverse)
self.client.send_control(
steer=steer,
throttle=throttle,
brake=brake,
hand_brake=hand_brake,
reverse=reverse)
# Process observations
image, py_measurements = self._read_observation()
if self.config["verbose"]:
print("Next command", py_measurements["next_command"])
if type(action) is np.ndarray:
py_measurements["action"] = [float(a) for a in action]
else:
py_measurements["action"] = action
py_measurements["control"] = {
"steer": steer,
"throttle": throttle,
"brake": brake,
"reverse": reverse,
"hand_brake": hand_brake,
}
reward = compute_reward(self, self.prev_measurement, py_measurements)
self.total_reward += reward
py_measurements["reward"] = reward
py_measurements["total_reward"] = self.total_reward
done = (self.num_steps > self.scenario["max_steps"]
or py_measurements["next_command"] == "REACH_GOAL"
or (self.config["early_terminate_on_collision"]
and collided_done(py_measurements)))
py_measurements["done"] = done
self.prev_measurement = py_measurements
# Write out measurements to file
if CARLA_OUT_PATH:
if not self.measurements_file:
self.measurements_file = open(
os.path.join(
CARLA_OUT_PATH,
"measurements_{}.json".format(self.episode_id)), "w")
self.measurements_file.write(json.dumps(py_measurements))
self.measurements_file.write("\n")
if done:
self.measurements_file.close()
self.measurements_file = None
if self.config["convert_images_to_video"]:
self.images_to_video()
self.num_steps += 1
image = self.preprocess_image(image)
return (self.encode_obs(image, py_measurements), reward, done,
py_measurements)
def images_to_video(self):
videos_dir = os.path.join(CARLA_OUT_PATH, "Videos")
if not os.path.exists(videos_dir):
os.makedirs(videos_dir)
ffmpeg_cmd = (
"ffmpeg -loglevel -8 -r 60 -f image2 -s {x_res}x{y_res} "
"-start_number 0 -i "
"{img}_%04d.jpg -vcodec libx264 {vid}.mp4 && rm -f {img}_*.jpg "
).format(
x_res=self.config["render_x_res"],
y_res=self.config["render_y_res"],
vid=os.path.join(videos_dir, self.episode_id),
img=os.path.join(CARLA_OUT_PATH, "CameraRGB", self.episode_id))
print("Executing ffmpeg command", ffmpeg_cmd)
subprocess.call(ffmpeg_cmd, shell=True)
def preprocess_image(self, image):
if self.config["use_depth_camera"]:
assert self.config["use_depth_camera"]
data = (image.data - 0.5) * 2
data = data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 1)
data = cv2.resize(
data, (self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = np.expand_dims(data, 2)
else:
data = image.data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 3)
data = cv2.resize(
data, (self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = (data.astype(np.float32) - 128) / 128
return data
def _read_observation(self):
# Read the data produced by the server this frame.
measurements, sensor_data = self.client.read_data()
# Print some of the measurements.
if self.config["verbose"]:
print_measurements(measurements)
observation = None
if self.config["use_depth_camera"]:
camera_name = "CameraDepth"
else:
camera_name = "CameraRGB"
for name, image in sensor_data.items():
if name == camera_name:
observation = image
cur = measurements.player_measurements
if self.config["enable_planner"]:
next_command = COMMANDS_ENUM[self.planner.get_next_command(
[cur.transform.location.x, cur.transform.location.y, GROUND_Z],
[
cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z
],
[self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [
self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z
])]
else:
next_command = "LANE_FOLLOW"
if next_command == "REACH_GOAL":
distance_to_goal = 0.0 # avoids crash in planner
elif self.config["enable_planner"]:
distance_to_goal = self.planner.get_shortest_path_distance([
cur.transform.location.x, cur.transform.location.y, GROUND_Z
], [
cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z
], [self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [
self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z
]) / 100
else:
distance_to_goal = -1
distance_to_goal_euclidean = float(
np.linalg.norm([
cur.transform.location.x - self.end_pos.location.x,
cur.transform.location.y - self.end_pos.location.y
]) / 100)
py_measurements = {
"episode_id": self.episode_id,
"step": self.num_steps,
"x": cur.transform.location.x,
"y": cur.transform.location.y,
"x_orient": cur.transform.orientation.x,
"y_orient": cur.transform.orientation.y,
"forward_speed": cur.forward_speed,
"distance_to_goal": distance_to_goal,
"distance_to_goal_euclidean": distance_to_goal_euclidean,
"collision_vehicles": cur.collision_vehicles,
"collision_pedestrians": cur.collision_pedestrians,
"collision_other": cur.collision_other,
"intersection_offroad": cur.intersection_offroad,
"intersection_otherlane": cur.intersection_otherlane,
"weather": self.weather,
"map": self.config["server_map"],
"start_coord": self.start_coord,
"end_coord": self.end_coord,
"current_scenario": self.scenario,
"x_res": self.config["x_res"],
"y_res": self.config["y_res"],
"num_vehicles": self.scenario["num_vehicles"],
"num_pedestrians": self.scenario["num_pedestrians"],
"max_steps": self.scenario["max_steps"],
"next_command": next_command,
}
if CARLA_OUT_PATH and self.config["log_images"]:
for name, image in sensor_data.items():
out_dir = os.path.join(CARLA_OUT_PATH, name)
if not os.path.exists(out_dir):
os.makedirs(out_dir)
out_file = os.path.join(
out_dir, "{}_{:>04}.jpg".format(self.episode_id,
self.num_steps))
scipy.misc.imsave(out_file, image.data)
assert observation is not None, sensor_data
return observation, py_measurements
class CarlaEnv(gym.Env):
def __init__(self, config=ENV_CONFIG):
self.config = config
if config["discrete_actions"]:
self.action_space = Discrete(10)
else:
self.action_space = Box(-1.0, 1.0, shape=(3, ))
if config["use_depth_camera"]:
self.observation_space = Box(-1.0,
1.0,
shape=(config["y_res"],
config["x_res"], 1))
else:
self.observation_space = Box(0.0,
255.0,
shape=(config["y_res"],
config["x_res"], 3))
self._spec = lambda: None
self._spec.id = "Carla-v0"
self.server_port = None
self.server_process = None
self.client = None
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.episode_id = None
self.measurements_file = None
self.weather = None
self.player_start = None
def init_server(self):
print("Initializing new Carla server...")
# Create a new server process and start the client.
self.server_port = random.randint(10000, 60000)
self.server_process = subprocess.Popen([
SERVER_BINARY, self.config["map"], "-windowed", "-ResX=400",
"-ResY=300", "-carla-server", "-carla-world-port={}".format(
self.server_port)
],
preexec_fn=os.setsid,
stdout=open(os.devnull, "w"))
self.client = CarlaClient("localhost", self.server_port)
self.client.connect()
def clear_server_state(self):
print("Clearing Carla server state")
try:
if self.client:
self.client.disconnect()
self.client = None
except Exception as e:
print("Error disconnecting client: {}".format(e))
pass
if self.server_process:
os.killpg(os.getpgid(self.server_process.pid), signal.SIGKILL)
self.server_port = None
self.server_process = None
def __del__(self):
self.clear_server_state()
def reset(self):
error = None
for _ in range(RETRIES_ON_ERROR):
try:
if not self.server_process:
self.init_server()
# reset twice since the first time a server is initialized,
# the starting location is different
self._reset()
return self._reset()
except Exception as e:
print("Error during reset: {}".format(traceback.format_exc()))
self.clear_server_state()
error = e
raise error
def _reset(self):
self.num_steps = 0
self.prev_measurement = None
self.episode_id = datetime.today().strftime("%Y-%m-%d_%H-%M-%S_%f")
self.measurements_file = None
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
self.weather = random.choice(self.config["weather"])
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self.config["num_vehicles"],
NumberOfPedestrians=self.config["num_pedestrians"],
WeatherId=self.weather)
settings.randomize_seeds()
camera1 = Camera("CameraDepth", PostProcessing="Depth")
camera1.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
camera1.set_position(30, 0, 130)
settings.add_sensor(camera1)
camera2 = Camera("CameraRGB")
camera2.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
camera2.set_position(30, 0, 130)
settings.add_sensor(camera2)
scene = self.client.load_settings(settings)
# Choose one player start at random.
number_of_player_starts = len(scene.player_start_spots)
if self.config["random_starting_location"]:
self.player_start = random.randint(
0, max(0, number_of_player_starts - 1))
else:
self.player_start = 0
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print("Starting new episode...")
self.client.start_episode(self.player_start)
image, py_measurements = self._read_observation()
self.prev_measurement = py_measurements
return self.preprocess_image(image)
def step(self, action):
try:
obs = self._step(action)
return obs
except Exception:
print("Error during step, terminating episode early",
traceback.format_exc())
self.clear_server_state()
return np.zeros(self.observation_space.shape), 0.0, True, {}
def _step(self, action):
if self.config["discrete_actions"]:
action = int(action)
assert action in range(10)
if action == 9:
brake = 1.0
steer = 0.0
throttle = 0.0
reverse = False
else:
brake = 0.0
if action >= 6:
steer = -1.0
elif action >= 3:
steer = 1.0
else:
steer = 0.0
action %= 3
if action == 0:
throttle = 0.0
reverse = False
elif action == 1:
throttle = 1.0
reverse = False
elif action == 2:
throttle = 1.0
reverse = True
else:
assert len(action) == 3, "Invalid action {}".format(action)
steer = action[0]
throttle = min(1.0, abs(action[1]))
brake = max(0.0, min(1.0, action[2]))
reverse = action[1] < 0.0
hand_brake = False
if self.config["verbose"]:
print("steer", steer, "throttle", throttle, "brake", brake,
"reverse", reverse)
self.client.send_control(steer=steer,
throttle=throttle,
brake=brake,
hand_brake=hand_brake,
reverse=reverse)
# Process observations
image, py_measurements = self._read_observation()
reward, done = compute_reward(self.config, self.prev_measurement,
py_measurements)
if self.num_steps > self.config["max_steps"]:
done = True
self.total_reward += reward
py_measurements["reward"] = reward
py_measurements["total_reward"] = self.total_reward
py_measurements["done"] = done
py_measurements["action"] = action
py_measurements["control"] = {
"steer": steer,
"throttle": throttle,
"brake": brake,
"reverse": reverse,
"hand_brake": hand_brake,
}
self.prev_measurement = py_measurements
# Write out measurements to file
if CARLA_OUT_PATH:
if not self.measurements_file:
self.measurements_file = open(
os.path.join(
CARLA_OUT_PATH,
"measurements_{}.json".format(self.episode_id)), "w")
self.measurements_file.write(json.dumps(py_measurements))
self.measurements_file.write("\n")
if done:
self.measurements_file.close()
self.measurements_file = None
self.num_steps += 1
image = self.preprocess_image(image)
return image, reward, done, py_measurements
def preprocess_image(self, image):
if self.config["use_depth_camera"]:
data = (image.data - 0.5) * 2
data = data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 1)
data = cv2.resize(data,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
else:
data = image.data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 3)
data = cv2.resize(data,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = (data.astype(np.float32) - 128) / 128
return data
def _read_observation(self):
# Read the data produced by the server this frame.
measurements, sensor_data = self.client.read_data()
# Print some of the measurements.
if self.config["verbose"]:
print_measurements(measurements)
observation = None
if self.config["use_depth_camera"]:
camera_name = "CameraDepth"
else:
camera_name = "CameraRGB"
for name, image in sensor_data.items():
if name == camera_name:
observation = image
cur = measurements.player_measurements
py_measurements = {
"episode_id": self.episode_id,
"step": self.num_steps,
"x": cur.transform.location.x,
"y": cur.transform.location.y,
"forward_speed": cur.forward_speed,
"collision_vehicles": cur.collision_vehicles,
"collision_pedestrians": cur.collision_pedestrians,
"collision_other": cur.collision_other,
"intersection_offroad": cur.intersection_offroad,
"intersection_otherlane": cur.intersection_otherlane,
"weather": self.weather,
"map": self.config["map"],
"target_x": self.config["target_x"],
"target_y": self.config["target_y"],
"x_res": self.config["x_res"],
"y_res": self.config["y_res"],
"num_vehicles": self.config["num_vehicles"],
"num_pedestrians": self.config["num_pedestrians"],
"max_steps": self.config["max_steps"],
}
if CARLA_OUT_PATH:
for name, image in sensor_data.items():
out_dir = os.path.join(CARLA_OUT_PATH, name)
if not os.path.exists(out_dir):
os.makedirs(out_dir)
out_file = os.path.join(
out_dir, "{}_{:>04}.jpg".format(self.episode_id,
self.num_steps))
scipy.misc.imsave(out_file, image.data)
assert observation is not None, sensor_data
return observation, py_measurements
