class CarlaClientConsole(cmd.Cmd):
def __init__(self, args):
cmd.Cmd.__init__(self)
self.args = args
self.prompt = '\x1b[1;34m%s\x1b[0m ' % '(carla)'
self.client = CarlaClient(args.host, args.port)
self.settings = get_default_carla_settings(args)
self.print_measurements = False
self.control = _Control()
self.thread = threading.Thread(target=self._agent_thread_worker)
self.done = False
self.thread.start()
def cleanup(self):
self.do_disconnect()
self.done = True
if self.thread.is_alive():
self.thread.join()
def default(self, line):
logging.error('unknown command \'%s\'!', line)
def emptyline(self):
pass
def precmd(self, line):
return line.strip().lower()
def can_exit(self):
return True
def do_exit(self, line=None):
"""Exit the console."""
return True
def do_eof(self, line=None):
"""Exit the console."""
print('exit')
return self.do_exit(line)
def help_help(self):
print('usage: help [topic]')
def do_disconnect(self, line=None):
"""Disconnect from the server."""
self.client.disconnect()
def do_new_episode(self, line=None):
"""Request a new episode. Connect to the server if not connected."""
try:
self.control = _Control()
if not self.client.connected():
self.client.connect()
self.client.load_settings(self.settings)
self.client.start_episode(0)
logging.info('new episode started')
except Exception as exception:
logging.error(exception)
def do_control(self, line=None):
"""Set control message:\nusage: control [reset|<param> <value>]\n(e.g., control throttle 0.5)"""
try:
if line == 'reset':
self.control = _Control()
else:
self.control.set(line)
logging.debug('control: %s', self.control.kwargs())
except Exception as exception:
logging.error(exception)
def complete_control(self, text, *args, **kwargs):
options = self.control.action_list()
options.append('reset')
return [x + ' ' for x in options if x.startswith(text)]
def do_settings(self, line=None):
"""Open a text editor to edit CARLA settings."""
result = edit_text(self.settings)
if result is not None:
self.settings = result
def do_print_measurements(self, line):
"""Print received measurements to console.\nusage: print_measurements [t/f]"""
self.print_measurements = True if not line else _Control._parse(
bool, line)
def _agent_thread_worker(self):
filename = '_images/console/camera_{:0>3d}/image_{:0>8d}.png'
while not self.done:
try:
measurements, sensor_data = self.client.read_data()
if self.print_measurements:
print(measurements)
if self.args.images_to_disk:
images = [
x for x in sensor_data.values()
if isinstance(x, Image)
]
for n, image in enumerate(images):
path = filename.format(n, measurements.game_timestamp)
image.save_to_disk(path)
self.client.send_control(**self.control.kwargs())
except Exception as exception:
# logging.exception(exception)
time.sleep(1)
class CarlaEnv(gym.Env):
def __init__(self,
host="localhost",
num_vehicles=1,
vehicles_seed=lambda: 200,
player_starts=2,
goals=0):
self.num_vehicles = num_vehicles
self.vehicles_seed = vehicles_seed
self.starts = list_from_scalar(player_starts)
self.goals = list_from_scalar(goals)
self.port = get_open_port()
self.host = host
self.metadata = {
'render.modes': ['rgb_array'],
#'video.frames_per_second': int(np.round(1.0 / self.dt))
}
self.server = self.open_server()
print(f"Connecting to CARLA Client on port {self.port}")
self.client = CarlaClient(self.host, self.port, timeout=99999999)
time.sleep(3)
self.client.connect(connection_attempts=1000)
print(f"Connected on port: {self.port}")
self.action_space = gym.spaces.Box(
-2,
2, (len(self._map_controls([1, 2, 3, 4, 5]).items()), ),
dtype=np.float32)
obs_size = len(self.reset())
self.observation_space = gym.spaces.Box(-float("inf"),
float("inf"), (obs_size, ),
dtype=np.float32)
def dist_from_goal(self, measurements):
x = array_from_loc(measurements.player_measurements.transform.location)
y = array_from_loc(self.scene.player_start_spots[self.goal].location)
return np.sqrt(np.sum((x - y)**2))
def open_server(self):
#os.devnull
log_file = f"./logs/carla_logs/carla_{self.port}.txt"
os.makedirs(os.path.dirname(log_file), exist_ok=True)
with open(log_file, "wb+") as out:
"""
true_script_name = os.path.join(CARLA_PATH, 'CarlaUE4.sh')
if os.path.islink(true_script_name):
true_script_name = os.readlink(execute_path)
project_root = os.path.dirname(true_script_name)
execute_path = f"{project_root}/CarlaUE4/Binaries/Linux/CarlaUE4"
# chmod +x
st = os.stat(execute_path)
os.chmod(execute_path, st.st_mode | stat.S_IEXEC)
"""
cmd = [
os.path.join(CARLA_PATH,
'CarlaUE4.sh'), #execute_path, "CarlaUE4",
"-carla-server",
"-fps=60",
f"-world-port={self.port}",
f"-windowed -ResX={500} -ResY={500}",
"-carla-no-hud"
]
# - benchmark
p = subprocess.Popen(cmd,
stdout=out,
stderr=out,
stdin=subprocess.PIPE,
preexec_fn=os.setpgrp)
return p
def close_server(self):
#self.server.terminate()
pid = self.server.pid
no_of_attempts = 0
def is_process_alive(pid):
## Source: https://stackoverflow.com/questions/568271/how-to-check-if-there-exists-a-process-with-a-given-pid-in-python
try:
os.kill(pid, 0)
except OSError:
return False
return True
while is_process_alive(pid):
pgroup = os.getpgid(self.server.pid)
self.server.terminate()
os.killpg(pgroup, signal.SIGTERM)
_, _ = os.waitpid(pid, os.WNOHANG)
time.sleep(5)
def _add_settings(self):
self.settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self.num_vehicles,
NumberOfPedestrians=0,
WeatherId=1,
QualityLevel="Low",
SeedVehicles=self.vehicles_seed())
#settings.randomize_seeds()
def _add_sensors(self):
camera0 = Camera('RenderCamera0')
# Set image resolution in pixels.
camera0.set_image_size(800, 600)
# Set its position relative to the car in meters.
camera0.set_position(-4.30, 0, 2.60)
camera0.set_rotation(pitch=-25, yaw=0, roll=0)
self.settings.add_sensor(camera0)
def _map_controls(self, a):
return dict(steer=a[0], throttle=a[1])
def _process_observation(self, measurements, sensor_data):
return array_from_measurements(measurements)
def _get_reward_and_termination(self):
measurements, sensor_data = self.current_state
collision_penalty = (
measurements.player_measurements.collision_vehicles +
measurements.player_measurements.collision_other)
is_collided = collision_penalty > 2
offroad_penalty = measurements.player_measurements.intersection_offroad
is_offroad = offroad_penalty > 0.5
dist_from_goal = self.dist_from_goal(measurements)
is_at_target = dist_from_goal < 5
is_done = is_collided or is_at_target or is_offroad
distance_reward = 300 - dist_from_goal
collision_cost = collision_penalty / 300
offroad_cost = (offroad_penalty * 10)
reward = distance_reward - collision_cost - offroad_cost
reward = reward / 20
return reward, is_done
def get_new_start_goal(self):
start = np.random.choice(self.starts)
goal = np.random.choice(self.goals)
while goal == start:
start = np.random.choice(self.starts)
goal = np.random.choice(self.goals)
return start, goal
def reset(self):
self.settings = CarlaSettings()
self._add_settings()
self._add_sensors()
self.scene = self.client.load_settings(self.settings)
start, goal = self.get_new_start_goal()
self.goal = goal
for i in range(100):
try:
self.client.start_episode(start)
self.current_state = self.client.read_data()
if i > 0:
print("Reconnected.")
break
except carla.tcp.TCPConnectionError:
if i % 10 == 0:
print(f"There was a TCP Error (Attempt {i}). Retrying. ")
time.sleep(3)
measurements, sensor_data = self.current_state
return self._process_observation(measurements, sensor_data)
def step(self, a):
control = self._map_controls(a)
for i in range(100):
try:
self.client.send_control(**control)
break
except carla.tcp.TCPConnectionError:
time.sleep(0.5)
self.current_state = self.client.read_data()
measurements, sensor_data = self.current_state
reward, is_done = self._get_reward_and_termination()
obs = self._process_observation(measurements, sensor_data)
return obs, reward, is_done, {}
def render(self, mode='human', **kwargs):
if mode == 'rgb_array':
return np.concatenate([
sensor.data for name, sensor in self.current_state[1].items()
if "Render" in name
],
axis=1)
super().render(mode=mode, **kwargs)
def _close(self):
self.close()
def close(self):
print(
f"Disconnecting from CARLA Client (port: {self.port}, pid: {self.server.pid})"
)
self.close_server()
time.sleep(3)
while self.client.connected():
self.client.disconnect()
print(
f"Disconnected from CARLA Client (port: {self.port}, pid: {self.server.pid})."
)
class 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
