def _setup_carla_client(self):
carla_client = CarlaClient(self._params['host'], self._params['port'], timeout=None)
carla_client.connect()
### create initial settings
carla_settings = CarlaSettings()
carla_settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=self._params['number_of_vehicles'],
NumberOfPedestrians=self._params['number_of_pedestrians'],
WeatherId=self._params['weather'])
carla_settings.randomize_seeds()
### add cameras
for camera_name in self._params['cameras']:
camera_params = self._params[camera_name]
camera_postprocessing = camera_params['postprocessing']
camera = Camera(camera_name, PostProcessing=camera_postprocessing)
camera.set_image_size(CarlaCollSpeedEnv.CAMERA_HEIGHT * CarlaCollSpeedEnv.WIDTH_TO_HEIGHT_RATIO, CarlaCollSpeedEnv.CAMERA_HEIGHT)
camera.set_position(*camera_params['position'])
camera.set(**{'FOV': camera_params['fov']})
carla_settings.add_sensor(camera)
### load settings
carla_scene = carla_client.load_settings(carla_settings)
self._carla_client = carla_client
self._carla_settings = carla_settings
self._carla_scene = carla_scene
def reset(self):
print('reset!')
print('=.' * 40)
a = 0
log_level = logging.INFO
logging.basicConfig(format='%(levelname)s: %(message)s',
level=log_level)
if os.path.exists('port.txt'):
with open('port.txt', 'r') as f:
a = int(f.readlines()[0].strip())
print(a)
print('=.' * 40)
with open('port.txt', 'w') as f:
f.write(str(a + 1) + '\n')
logging.info('listening to server %s:%s', 'localhost', 7899)
while True:
try:
client = CarlaClient('localhost', 7899, timeout=1000)
client.connect()
self.carla_game = CarlaGame(client)
self.carla_game.reset()
obs = self.carla_game.get_obs()
self.carla_game.display()
return obs[0]
except TCPConnectionError as error:
logging.error(error)
time.sleep(1)
class CarlaEnv(gym.Env):
metadata = {'render.modes': ['human', 'rgb_array']}
reward_range = (-np.inf, np.inf)
spec = None
action_space = None
observation_space = None
def __init__(self, target=(158.08, 27.18)):
# action space range: steer, throttle, brake
min_steer, max_steer = -1, 1
min_throttle, max_throttle = 0, 1
min_brake, max_brake = 0, 1
self.action_space = spaces.Box(low=np.array([min_steer, -max_brake]),
high=np.array([max_steer,
max_throttle]),
dtype=np.float32)
# observation, 3 channel image
self.observation_space = spaces.Box(
low=0,
high=1.0,
shape=(3, carla_config.CARLA_IMG_HEIGHT,
carla_config.CARLA_IMG_WIDTH),
dtype=np.float32)
self.viewer = ImageViewer()
self.rng = np.random.RandomState() # used for random number generators
self.target = np.array(target)
self.cur_image = None # image with (H, W, C)
self.cur_measurements = None
self.carla_client = CarlaClient(carla_config.CARLA_HOST_ADDRESS,
carla_config.CARLA_HOST_PORT,
timeout=100)
self.carla_client.connect()
if self.carla_client.connected():
print("Successfully connected to", end=" ")
else:
print("Failed to connect", end=" ")
print(
f"Carla on {carla_config.CARLA_HOST_ADDRESS}:{carla_config.CARLA_HOST_PORT}"
)
def reset(self):
"""
Resets the state of the environment and returns an initial observation.
:return: observation (object): the initial observation of the space.
"""
# load Carla settings
settings = CarlaSettings()
settings = self._load_settings(settings)
scene = self.carla_client.load_settings(settings)
# define a random starting point of the agent for the appropriate training
number_of_player_starts = len(scene.player_start_spots)
player_start = self.rng.randint(0, max(0, number_of_player_starts - 1))
self.carla_client.start_episode(player_start)
print(f'Starting new episode at {scene.map_name}, {player_start}...')
# read and return status after reset
self.cur_measurements, self.cur_image = self._read_data()
state = self._state(
self.cur_image, self.cur_image
) #possibly revise the state to be some operation of several images.
meas = self.cur_measurements
return state, meas
def render(self, mode='human'):
"""Renders the environment.
:param mode: - human: render to the current display or terminal and
return nothing. Usually for human consumption.
- rgb_array: Return an numpy.ndarray with shape (x, y, 3),
representing RGB values for an x-by-y pixel image, suitable
for turning into a video.
:return:
"""
if mode == 'rgb_array':
return self.cur_image
elif mode == 'human':
self.viewer.imshow(arr=self.cur_image)
return self.viewer.isopen
else:
super(CarlaEnv, self).render(mode=mode) # just raise an exception
def step(self, action):
"""
Run one time step of the environment's dynamics. When end of
episode is reached, you are responsible for calling `reset()`
to reset this environment's state.
Accepts an action and returns a tuple (observation, reward, done, info).
:param action: an action provided by the environment
:return: observation (object): agent's observation of the current environment
reward (float) : amount of reward returned after previous action
done (boolean): whether the episode has ended, in which case further step() calls will return undefined results
info (dict): contains auxiliary diagnostic information (helpful for debugging, and sometimes learning)
"""
if isinstance(action, dict):
self.carla_client.send_control(**action)
elif isinstance(action, np.ndarray): # parse control from array
throttle = max(0, action[1])
brake = max(0, -action[1])
self.carla_client.send_control(steer=action[0],
throttle=throttle,
brake=brake)
else:
self.carla_client.send_control(action)
# read measurements and image from Carla
measurements, image = self._read_data()
# calculate reward
reward = self._reward(self.cur_measurements, measurements)
done = self._done(measurements)
state = self._state(self.cur_image, image)
# save current measurements for next use
self.cur_measurements = measurements
self.cur_image = image
return state, reward, done, measurements
def close(self):
"""
Close connection to Carla
:return:
"""
self.carla_client.disconnect()
self.viewer.close()
def seed(self, seed=None):
"""Set seed for random number generators used in the code
Set seed so that results are consistent in multi runs
:param seed: seed number, defaults to None
"""
self.rng = np.random.RandomState(seed)
def _state(self, pre_image, cur_image):
def image_proc(img):
img = img.transpose(2, 0, 1)
img = np.ascontiguousarray(img, dtype=np.float32) / 255
return img
# pre_image = image_proc(pre_image)
cur_image = image_proc(cur_image)
# CHW, use current image as state
return cur_image
def _load_settings(self, settings):
"""Load Carla settings
Override to customize settings
:param settings: default settings
:return: new settings
"""
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=20,
NumberOfPedestrians=40,
WeatherId=self.rng.choice([1, 3, 7, 8, 14]),
QualityLevel='Low')
# CAMERA
camera0 = Camera('CameraRGB', PostProcessing='SceneFinal')
# Set image resolution in pixels.
camera0.set_image_size(carla_config.CARLA_IMG_HEIGHT,
carla_config.CARLA_IMG_WIDTH)
# Set its position relative to the car in meters.
camera0.set_position(0.30, 0, 1.30)
settings.add_sensor(camera0)
return settings
def _get_sensor_data(self, sensor_data):
"""Extract sensor data from multi sensor dict
Override to customize which sensor (Camera/LiDar) to use
:param sensor_data: multi sensor dict
:type sensor_data: extracted sensor data
"""
return sensor_data['CameraRGB'].data
def _read_data(self, ):
"""
read data from carla
:return: custom measurement data dict, camera image
"""
measurements, sensor_data = self.carla_client.read_data()
p_meas = measurements.player_measurements
pos_x = p_meas.transform.location.x
pos_y = p_meas.transform.location.y
speed = p_meas.forward_speed * 3.6 # m/s -> km/h
col_cars = p_meas.collision_vehicles
col_ped = p_meas.collision_pedestrians
col_other = p_meas.collision_other
other_lane = 100 * p_meas.intersection_otherlane
if other_lane:
print('Intersection into other lane %.2f' % other_lane)
offroad = 100 * p_meas.intersection_offroad
if offroad:
print('offroad %.2f' % offroad)
agents_num = len(measurements.non_player_agents)
distance = np.linalg.norm(np.array([pos_x, pos_y]) - self.target)
meas = {
'pos_x': pos_x,
'pos_y': pos_y,
'speed': speed,
'col_damage': col_cars + col_ped + col_other,
'other_lane': other_lane,
'offroad': offroad,
'agents_num': agents_num,
'distance': distance,
'autopilot_control': p_meas.autopilot_control
}
return meas, self._get_sensor_data(sensor_data)
def _reward(self, pre_measurements, cur_measurements):
"""
Calculate reward
:param pre_measurements: previous measurement
:param cur_measurements: latest measurement
:return: reward
"""
def delta(key):
return cur_measurements[key] - pre_measurements[key]
if pre_measurements is None:
rwd = 0.0
else:
rwd = 0.15 * delta('speed') - 0.002 * delta('col_damage') \
- 2 * delta('offroad') - 2 * delta('other_lane')
return rwd
def _done(self, cur_measurements):
"""
Check done or not
:param cur_measurements: latest measurement
:return:
"""
# check distance to target
d = cur_measurements['distance'] < 1 # final state arrived or not
return d
class CarlaEnv(gym.Env):
def __init__(self, config=ENV_CONFIG):
self.config = config
self.city = self.config["server_map"].split("/")[-1]
if self.config["enable_planner"]:
self.planner = Planner(self.city)
if config["discrete_actions"]:
self.action_space = Discrete(len(DISCRETE_ACTIONS))
else:
self.action_space = Box(-1.0, 1.0, shape=(2, ), dtype=np.float32)
if config["use_depth_camera"]:
image_space = Box(-1.0,
1.0,
shape=(config["y_res"], config["x_res"],
1 * config["framestack"]),
dtype=np.float32)
else:
image_space = Box(0,
255,
shape=(config["y_res"], config["x_res"],
3 * config["framestack"]),
dtype=np.uint8)
self.observation_space = Tuple( # forward_speed, dist to goal
[
image_space,
Discrete(len(COMMANDS_ENUM)), # next_command
Box(-128.0, 128.0, shape=(2, ), dtype=np.float32)
])
# TODO(ekl) this isn't really a proper gym spec
self._spec = lambda: None
self._spec.id = "Carla-v0"
self.server_port = None
self.server_process = None
self.client = None
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.prev_image = None
self.episode_id = None
self.measurements_file = None
self.weather = None
self.scenario = None
self.start_pos = None
self.end_pos = None
self.start_coord = None
self.end_coord = None
self.last_obs = None
def init_server(self):
print("Initializing new Carla server...")
# Create a new server process and start the client.
self.server_port = random.randint(10000, 60000)
self.server_process = subprocess.Popen([
SERVER_BINARY, self.config["server_map"], "-windowed", "-ResX=800",
"-ResY=600", "-carla-server", "-benchmark -fps=10",
"-carla-world-port={}".format(self.server_port)
],
preexec_fn=os.setsid,
stdout=open(os.devnull, "w"))
live_carla_processes.add(os.getpgid(self.server_process.pid))
for i in range(RETRIES_ON_ERROR):
try:
self.client = CarlaClient("localhost", self.server_port)
return self.client.connect()
except Exception as e:
print("Error connecting: {}, attempt {}".format(e, i))
time.sleep(2)
def clear_server_state(self):
print("Clearing Carla server state")
try:
if self.client:
self.client.disconnect()
self.client = None
except Exception as e:
print("Error disconnecting client: {}".format(e))
pass
if self.server_process:
pgid = os.getpgid(self.server_process.pid)
os.killpg(pgid, signal.SIGKILL)
live_carla_processes.remove(pgid)
self.server_port = None
self.server_process = None
def __del__(self):
self.clear_server_state()
def reset(self):
error = None
for _ in range(RETRIES_ON_ERROR):
try:
if not self.server_process:
self.init_server()
return self._reset()
except Exception as e:
print("Error during reset: {}".format(traceback.format_exc()))
self.clear_server_state()
error = e
raise error
def _reset(self):
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.prev_image = None
self.episode_id = datetime.today().strftime("%Y-%m-%d_%H-%M-%S_%f")
self.measurements_file = None
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
self.scenario = random.choice(self.config["scenarios"])
assert self.scenario["city"] == self.city, (self.scenario, self.city)
self.weather = random.choice(self.scenario["weather_distribution"])
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self.scenario["num_vehicles"],
NumberOfPedestrians=self.scenario["num_pedestrians"],
WeatherId=self.weather)
settings.randomize_seeds()
if self.config["use_depth_camera"]:
camera1 = Camera("CameraDepth", PostProcessing="Depth")
camera1.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
camera1.set_position(0.1, 0, 1.7)
settings.add_sensor(camera1)
camera2 = Camera("CameraRGB")
camera2.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
camera2.set_position(0.1, 0, 1.7)
settings.add_sensor(camera2)
# Setup start and end positions
scene = self.client.load_settings(settings)
positions = scene.player_start_spots
self.start_pos = positions[self.scenario["start_pos_id"]]
self.end_pos = positions[self.scenario["end_pos_id"]]
self.start_coord = [
self.start_pos.location.x // 100, self.start_pos.location.y // 100
]
self.end_coord = [
self.end_pos.location.x // 100, self.end_pos.location.y // 100
]
print("Start pos {} ({}), end {} ({})".format(
self.scenario["start_pos_id"], self.start_coord,
self.scenario["end_pos_id"], self.end_coord))
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print("Starting new episode...")
self.client.start_episode(self.scenario["start_pos_id"])
image, py_measurements = self._read_observation()
self.prev_measurement = py_measurements
return self.encode_obs(self.preprocess_image(image), py_measurements)
def encode_obs(self, image, py_measurements):
assert self.config["framestack"] in [1, 2]
prev_image = self.prev_image
self.prev_image = image
if prev_image is None:
prev_image = image
if self.config["framestack"] == 2:
image = np.concatenate([prev_image, image], axis=2)
obs = (image, COMMAND_ORDINAL[py_measurements["next_command"]], [
py_measurements["forward_speed"],
py_measurements["distance_to_goal"]
])
self.last_obs = obs
return obs
def step(self, action):
try:
obs = self._step(action)
return obs
except Exception:
print("Error during step, terminating episode early",
traceback.format_exc())
self.clear_server_state()
return (self.last_obs, 0.0, True, {})
def _step(self, action):
if self.config["discrete_actions"]:
action = DISCRETE_ACTIONS[int(action)]
assert len(action) == 2, "Invalid action {}".format(action)
if self.config["squash_action_logits"]:
forward = 2 * float(sigmoid(action[0]) - 0.5)
throttle = float(np.clip(forward, 0, 1))
brake = float(np.abs(np.clip(forward, -1, 0)))
steer = 2 * float(sigmoid(action[1]) - 0.5)
else:
throttle = float(np.clip(action[0], 0, 1))
brake = float(np.abs(np.clip(action[0], -1, 0)))
steer = float(np.clip(action[1], -1, 1))
reverse = False
hand_brake = False
if self.config["verbose"]:
print("steer", steer, "throttle", throttle, "brake", brake,
"reverse", reverse)
self.client.send_control(steer=steer,
throttle=throttle,
brake=brake,
hand_brake=hand_brake,
reverse=reverse)
# Process observations
image, py_measurements = self._read_observation()
if self.config["verbose"]:
print("Next command", py_measurements["next_command"])
if type(action) is np.ndarray:
py_measurements["action"] = [float(a) for a in action]
else:
py_measurements["action"] = action
py_measurements["control"] = {
"steer": steer,
"throttle": throttle,
"brake": brake,
"reverse": reverse,
"hand_brake": hand_brake,
}
reward = compute_reward(self, self.prev_measurement, py_measurements)
self.total_reward += reward
py_measurements["reward"] = reward
py_measurements["total_reward"] = self.total_reward
done = (self.num_steps > self.scenario["max_steps"]
or py_measurements["next_command"] == "REACH_GOAL"
or (self.config["early_terminate_on_collision"]
and collided_done(py_measurements)))
py_measurements["done"] = done
self.prev_measurement = py_measurements
# Write out measurements to file
if CARLA_OUT_PATH:
if not self.measurements_file:
self.measurements_file = open(
os.path.join(
CARLA_OUT_PATH,
"measurements_{}.json".format(self.episode_id)), "w")
self.measurements_file.write(json.dumps(py_measurements))
self.measurements_file.write("\n")
if done:
self.measurements_file.close()
self.measurements_file = None
if self.config["convert_images_to_video"]:
self.images_to_video()
self.num_steps += 1
image = self.preprocess_image(image)
return (self.encode_obs(image, py_measurements), reward, done,
py_measurements)
def images_to_video(self):
videos_dir = os.path.join(CARLA_OUT_PATH, "Videos")
if not os.path.exists(videos_dir):
os.makedirs(videos_dir)
ffmpeg_cmd = (
"ffmpeg -loglevel -8 -r 60 -f image2 -s {x_res}x{y_res} "
"-start_number 0 -i "
"{img}_%04d.jpg -vcodec libx264 {vid}.mp4 && rm -f {img}_*.jpg "
).format(x_res=self.config["render_x_res"],
y_res=self.config["render_y_res"],
vid=os.path.join(videos_dir, self.episode_id),
img=os.path.join(CARLA_OUT_PATH, "CameraRGB",
self.episode_id))
print("Executing ffmpeg command", ffmpeg_cmd)
subprocess.call(ffmpeg_cmd, shell=True)
def preprocess_image(self, image):
if self.config["use_depth_camera"]:
assert self.config["use_depth_camera"]
data = (image.data - 0.5) * 2
data = data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 1)
data = cv2.resize(data,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = np.expand_dims(data, 2)
else:
data = image.data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 3)
data = cv2.resize(data,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = (data.astype(np.float32) - 128) / 128
return data
def _read_observation(self):
# Read the data produced by the server this frame.
measurements, sensor_data = self.client.read_data()
# Print some of the measurements.
if self.config["verbose"]:
print_measurements(measurements)
observation = None
if self.config["use_depth_camera"]:
camera_name = "CameraDepth"
else:
camera_name = "CameraRGB"
for name, image in sensor_data.items():
if name == camera_name:
observation = image
cur = measurements.player_measurements
if self.config["enable_planner"]:
next_command = COMMANDS_ENUM[self.planner.get_next_command(
[cur.transform.location.x, cur.transform.location.y, GROUND_Z],
[
cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z
],
[self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [
self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z
])]
else:
next_command = "LANE_FOLLOW"
if next_command == "REACH_GOAL":
distance_to_goal = 0.0 # avoids crash in planner
elif self.config["enable_planner"]:
distance_to_goal = self.planner.get_shortest_path_distance([
cur.transform.location.x, cur.transform.location.y, GROUND_Z
], [
cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z
], [self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [
self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z
]) / 100
else:
distance_to_goal = -1
distance_to_goal_euclidean = float(
np.linalg.norm([
cur.transform.location.x - self.end_pos.location.x,
cur.transform.location.y - self.end_pos.location.y
]) / 100)
py_measurements = {
"episode_id": self.episode_id,
"step": self.num_steps,
"x": cur.transform.location.x,
"y": cur.transform.location.y,
"x_orient": cur.transform.orientation.x,
"y_orient": cur.transform.orientation.y,
"forward_speed": cur.forward_speed,
"distance_to_goal": distance_to_goal,
"distance_to_goal_euclidean": distance_to_goal_euclidean,
"collision_vehicles": cur.collision_vehicles,
"collision_pedestrians": cur.collision_pedestrians,
"collision_other": cur.collision_other,
"intersection_offroad": cur.intersection_offroad,
"intersection_otherlane": cur.intersection_otherlane,
"weather": self.weather,
"map": self.config["server_map"],
"start_coord": self.start_coord,
"end_coord": self.end_coord,
"current_scenario": self.scenario,
"x_res": self.config["x_res"],
"y_res": self.config["y_res"],
"num_vehicles": self.scenario["num_vehicles"],
"num_pedestrians": self.scenario["num_pedestrians"],
"max_steps": self.scenario["max_steps"],
"next_command":
next_command, # TODO can we figure some way using this command
}
if CARLA_OUT_PATH and self.config["log_images"]:
for name, image in sensor_data.items():
out_dir = os.path.join(CARLA_OUT_PATH, name)
if not os.path.exists(out_dir):
os.makedirs(out_dir)
out_file = os.path.join(
out_dir, "{}_{:>04}.jpg".format(self.episode_id,
self.num_steps))
scipy.misc.imsave(out_file, image.data)
assert observation is not None, sensor_data
return observation, py_measurements
class CarlaLegacyOperator(Op):
""" CarlaLegacyOperator initializes and controls the simulator.
This operator connects to the simulator, spawns actors, gets and publishes
ground info, and sends vehicle commands. The operator works with
Carla 0.8.4.
"""
def __init__(self,
name,
flags,
auto_pilot,
camera_setups=[],
lidar_setups=[],
log_file_name=None,
csv_file_name=None):
super(CarlaLegacyOperator, self).__init__(name)
self._flags = flags
self._logger = setup_logging(self.name, log_file_name)
self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name)
self._auto_pilot = auto_pilot
if self._flags.carla_high_quality:
quality = 'Epic'
else:
quality = 'Low'
self._settings = CarlaSettings()
self._settings.set(
SynchronousMode=self._flags.carla_synchronous_mode,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self._flags.carla_num_vehicles,
NumberOfPedestrians=self._flags.carla_num_pedestrians,
WeatherId=self._flags.carla_weather,
QualityLevel=quality)
self._settings.randomize_seeds()
self._transforms = {}
# Add cameras to the simulation.
for cs in camera_setups:
self.__add_camera(cs)
self._transforms[cs.name] = cs.get_transform()
# Add lidars to the simulation.
for ls in lidar_setups:
self.__add_lidar(ls)
self._transforms[ls.name] = ls.get_transform()
self.agent_id_map = {}
self.pedestrian_count = 0
# Initialize the control state.
self.control = {
'steer': 0.0,
'throttle': 0.0,
'brake': 0.0,
'hand_brake': False,
'reverse': False
}
# Register custom serializers for Messages and WatermarkMessages
ray.register_custom_serializer(Message, use_pickle=True)
ray.register_custom_serializer(WatermarkMessage, use_pickle=True)
@staticmethod
def setup_streams(input_streams, camera_setups, lidar_setups):
input_streams.add_callback(CarlaLegacyOperator.on_control_msg)
camera_streams = [
pylot.utils.create_camera_stream(cs) for cs in camera_setups
]
lidar_streams = [
pylot.utils.create_lidar_stream(ls) for ls in lidar_setups
]
return [
pylot.utils.create_can_bus_stream(),
pylot.utils.create_ground_traffic_lights_stream(),
pylot.utils.create_ground_vehicles_stream(),
pylot.utils.create_ground_pedestrians_stream(),
pylot.utils.create_ground_speed_limit_signs_stream(),
pylot.utils.create_ground_stop_signs_stream()
] + camera_streams + lidar_streams
def __add_camera(self, camera_setup):
"""Adds a camera and a corresponding output stream.
Args:
camera_setup: A camera setup object.
"""
# Transform from Carla 0.9.x postprocessing strings to Carla 0.8.4.
if camera_setup.camera_type == 'sensor.camera.rgb':
postprocessing = 'SceneFinal'
elif camera_setup.camera_type == 'sensor.camera.depth':
postprocessing = 'Depth'
elif camera_setup.camera_type == 'sensor.camera.semantic_segmentation':
postprocessing = 'SemanticSegmentation'
transform = camera_setup.get_transform()
camera = Camera(name=camera_setup.name,
PostProcessing=postprocessing,
FOV=camera_setup.fov,
ImageSizeX=camera_setup.width,
ImageSizeY=camera_setup.height,
PositionX=transform.location.x,
PositionY=transform.location.y,
PositionZ=transform.location.z,
RotationPitch=transform.rotation.pitch,
RotationRoll=transform.rotation.roll,
RotationYaw=transform.rotation.yaw)
self._settings.add_sensor(camera)
def __add_lidar(self, lidar_setup):
"""Adds a LIDAR sensor and a corresponding output stream.
Args:
lidar_setup: A LidarSetup object..
"""
transform = lidar_setup.get_transform()
lidar = Lidar(lidar_setup.name,
Channels=lidar_setup.channels,
Range=lidar_setup.range,
PointsPerSecond=lidar_setup.points_per_second,
RotationFrequency=lidar_setup.rotation_frequency,
UpperFovLimit=lidar_setup.upper_fov,
LowerFovLimit=lidar_setup.lower_fov,
PositionX=transform.location.x,
PositionY=transform.location.y,
PositionZ=transform.location.z,
RotationPitch=transform.rotation.pitch,
RotationYaw=transform.rotation.yaw,
RotationRoll=transform.rotation.roll)
self._settings.add_sensor(lidar)
def publish_world_data(self):
read_start_time = time.time()
measurements, sensor_data = self.client.read_data()
measure_time = time.time()
self._logger.info(
'Got readings for game time {} and platform time {}'.format(
measurements.game_timestamp, measurements.platform_timestamp))
timestamp = Timestamp(coordinates=[measurements.game_timestamp])
watermark = WatermarkMessage(timestamp)
# Send player data on data streams.
self.__send_player_data(measurements.player_measurements, timestamp,
watermark)
# Extract agent data from measurements.
agents = self.__get_ground_agents(measurements)
# Send agent data on data streams.
self.__send_ground_agent_data(agents, timestamp, watermark)
# Send sensor data on data stream.
self.__send_sensor_data(sensor_data, timestamp, watermark)
# Get Autopilot control data.
if self._auto_pilot:
self.control = measurements.player_measurements.autopilot_control
end_time = time.time()
measurement_runtime = (measure_time - read_start_time) * 1000
total_runtime = (end_time - read_start_time) * 1000
self._logger.info('Carla measurement time {}; total time {}'.format(
measurement_runtime, total_runtime))
self._csv_logger.info('{},{},{},{}'.format(time_epoch_ms(), self.name,
measurement_runtime,
total_runtime))
def __send_player_data(self, player_measurements, timestamp, watermark):
""" Sends hero vehicle information.
It populates a CanBus tuple.
"""
location = pylot.simulation.utils.Location(
carla_loc=player_measurements.transform.location)
rotation = pylot.simulation.utils.Rotation(
player_measurements.transform.rotation.pitch,
player_measurements.transform.rotation.yaw,
player_measurements.transform.rotation.roll)
orientation = pylot.simulation.utils.Orientation(
player_measurements.transform.orientation.x,
player_measurements.transform.orientation.y,
player_measurements.transform.orientation.z)
vehicle_transform = pylot.simulation.utils.Transform(
location, rotation, orientation=orientation)
forward_speed = player_measurements.forward_speed * 3.6
can_bus = pylot.simulation.utils.CanBus(vehicle_transform,
forward_speed)
self.get_output_stream('can_bus').send(Message(can_bus, timestamp))
self.get_output_stream('can_bus').send(watermark)
def __get_ground_agents(self, measurements):
vehicles = []
pedestrians = []
traffic_lights = []
speed_limit_signs = []
for agent in measurements.non_player_agents:
if agent.HasField('vehicle'):
transform = pylot.simulation.utils.to_pylot_transform(
agent.vehicle.transform)
bb = pylot.simulation.utils.BoundingBox(
agent.vehicle.bounding_box)
forward_speed = agent.vehicle.forward_speed
vehicle = pylot.simulation.utils.Vehicle(
transform, bb, forward_speed)
vehicles.append(vehicle)
elif agent.HasField('pedestrian'):
if not self.agent_id_map.get(agent.id):
self.pedestrian_count += 1
self.agent_id_map[agent.id] = self.pedestrian_count
pedestrian_index = self.agent_id_map[agent.id]
transform = pylot.simulation.utils.to_pylot_transform(
agent.pedestrian.transform)
bb = pylot.simulation.utils.BoundingBox(
agent.pedestrian.bounding_box)
forward_speed = agent.pedestrian.forward_speed
pedestrian = pylot.simulation.utils.Pedestrian(
pedestrian_index, transform, bb, forward_speed)
pedestrians.append(pedestrian)
elif agent.HasField('traffic_light'):
transform = pylot.simulation.utils.to_pylot_transform(
agent.traffic_light.transform)
if agent.traffic_light.state == 2:
erdos_tl_state = TrafficLightColor.RED
elif agent.traffic_light.state == 1:
erdos_tl_state = TrafficLightColor.YELLOW
elif agent.traffic_light.state == 0:
erdos_tl_state = TrafficLightColor.GREEN
else:
erdos_tl_state = TrafficLightColor.OFF
traffic_light = pylot.simulation.utils.TrafficLight(
agent.id, transform, erdos_tl_state, None)
traffic_lights.append(traffic_light)
elif agent.HasField('speed_limit_sign'):
transform = pylot.simulation.utils.to_pylot_transform(
agent.speed_limit_sign.transform)
speed_sign = pylot.simulation.utils.SpeedLimitSign(
transform, agent.speed_limit_sign.speed_limit)
speed_limit_signs.append(speed_sign)
return vehicles, pedestrians, traffic_lights, speed_limit_signs
def __send_ground_agent_data(self, agents, timestamp, watermark):
vehicles, pedestrians, traffic_lights, speed_limit_signs = agents
vehicles_msg = pylot.simulation.messages.GroundVehiclesMessage(
vehicles, timestamp)
self.get_output_stream('vehicles').send(vehicles_msg)
self.get_output_stream('vehicles').send(watermark)
pedestrians_msg = pylot.simulation.messages.GroundPedestriansMessage(
pedestrians, timestamp)
self.get_output_stream('pedestrians').send(pedestrians_msg)
self.get_output_stream('pedestrians').send(watermark)
traffic_lights_msg = pylot.simulation.messages.GroundTrafficLightsMessage(
traffic_lights, timestamp)
self.get_output_stream('traffic_lights').send(traffic_lights_msg)
self.get_output_stream('traffic_lights').send(watermark)
speed_limits_msg = pylot.simulation.messages.GroundSpeedSignsMessage(
speed_limit_signs, timestamp)
self.get_output_stream('speed_limit_signs').send(speed_limits_msg)
self.get_output_stream('speed_limit_signs').send(watermark)
# We do not have any stop signs.
stop_signs_msg = pylot.simulation.messages.GroundStopSignsMessage(
[], timestamp)
self.get_output_stream('stop_signs').send(stop_signs_msg)
self.get_output_stream('stop_signs').send(watermark)
def __send_sensor_data(self, sensor_data, timestamp, watermark):
for name, measurement in sensor_data.items():
data_stream = self.get_output_stream(name)
if data_stream.get_label('camera_type') == 'sensor.camera.rgb':
# Transform the Carla RGB images to BGR.
data_stream.send(
pylot.simulation.messages.FrameMessage(
pylot.utils.bgra_to_bgr(to_bgra_array(measurement)),
timestamp))
elif data_stream.get_label(
'camera_type') == 'sensor.camera.semantic_segmentation':
frame = labels_to_array(measurement)
data_stream.send(SegmentedFrameMessage(frame, 0, timestamp))
elif data_stream.get_label('camera_type') == 'sensor.camera.depth':
# NOTE: depth_to_array flips the image.
data_stream.send(
pylot.simulation.messages.DepthFrameMessage(
depth_to_array(measurement), self._transforms[name],
measurement.fov, timestamp))
elif data_stream.get_label(
'sensor_type') == 'sensor.lidar.ray_cast':
pc_msg = pylot.simulation.messages.PointCloudMessage(
measurement.data, self._transforms[name], timestamp)
data_stream.send(pc_msg)
else:
data_stream.send(Message(measurement, timestamp))
data_stream.send(watermark)
def _tick_simulator(self):
if (not self._flags.carla_synchronous_mode
or self._flags.carla_step_frequency == -1):
# Run as fast as possible.
self.publish_world_data()
return
time_until_tick = self._tick_at - time.time()
if time_until_tick > 0:
time.sleep(time_until_tick)
else:
self._logger.error('Cannot tick Carla at frequency {}'.format(
self._flags.carla_step_frequency))
self._tick_at += 1.0 / self._flags.carla_step_frequency
self.publish_world_data()
def on_control_msg(self, msg):
""" Invoked when a ControlMessage is received.
Args:
msg: A control.messages.ControlMessage message.
"""
if not self._auto_pilot:
# Update the control dict state.
self.control['steer'] = msg.steer
self.control['throttle'] = msg.throttle
self.control['brake'] = msg.brake
self.control['hand_brake'] = msg.hand_brake
self.control['reverse'] = msg.reverse
self.client.send_control(**self.control)
else:
self.client.send_control(self.control)
self._tick_simulator()
def execute(self):
# Connect to the simulator.
self.client = CarlaClient(self._flags.carla_host,
self._flags.carla_port,
timeout=self._flags.carla_timeout)
self.client.connect()
scene = self.client.load_settings(self._settings)
# Choose one player start at random.
number_of_player_starts = len(scene.player_start_spots)
player_start = self._flags.carla_start_player_num
if self._flags.carla_random_player_start:
player_start = np.random.randint(
0, max(0, number_of_player_starts - 1))
self.client.start_episode(player_start)
self._tick_at = time.time()
self._tick_simulator()
self.spin()
class CarlaEnvironment(Environment):
def __init__(self, level: LevelSelection, seed: int, frame_skip: int,
human_control: bool, custom_reward_threshold: Union[int,
float],
visualization_parameters: VisualizationParameters,
server_height: int, server_width: int, camera_height: int,
camera_width: int, verbose: bool,
experiment_suite: ExperimentSuite, config: str,
episode_max_time: int, allow_braking: bool,
quality: CarlaEnvironmentParameters.Quality,
cameras: List[CameraTypes], weather_id: List[int],
experiment_path: str,
separate_actions_for_throttle_and_brake: bool,
num_speedup_steps: int, max_speed: float, **kwargs):
super().__init__(level, seed, frame_skip, human_control,
custom_reward_threshold, visualization_parameters)
# server configuration
self.server_height = server_height
self.server_width = server_width
self.port = get_open_port()
self.host = 'localhost'
self.map_name = CarlaLevel[level.upper()].value['map_name']
self.map_path = CarlaLevel[level.upper()].value['map_path']
self.experiment_path = experiment_path
# client configuration
self.verbose = verbose
self.quality = quality
self.cameras = cameras
self.weather_id = weather_id
self.episode_max_time = episode_max_time
self.allow_braking = allow_braking
self.separate_actions_for_throttle_and_brake = separate_actions_for_throttle_and_brake
self.camera_width = camera_width
self.camera_height = camera_height
# setup server settings
self.experiment_suite = experiment_suite
self.config = config
if self.config:
# load settings from file
with open(self.config, 'r') as fp:
self.settings = fp.read()
else:
# hard coded settings
self.settings = CarlaSettings()
self.settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=15,
NumberOfPedestrians=30,
WeatherId=random.choice(
force_list(self.weather_id)),
QualityLevel=self.quality.value,
SeedVehicles=seed,
SeedPedestrians=seed)
if seed is None:
self.settings.randomize_seeds()
self.settings = self._add_cameras(self.settings, self.cameras,
self.camera_width,
self.camera_height)
# open the server
self.server = self._open_server()
logging.disable(40)
# open the client
self.game = CarlaClient(self.host, self.port, timeout=99999999)
self.game.connect()
if self.experiment_suite:
self.current_experiment_idx = 0
self.current_experiment = self.experiment_suite.get_experiments()[
self.current_experiment_idx]
self.scene = self.game.load_settings(
self.current_experiment.conditions)
else:
self.scene = self.game.load_settings(self.settings)
# get available start positions
self.positions = self.scene.player_start_spots
self.num_positions = len(self.positions)
self.current_start_position_idx = 0
self.current_pose = 0
# state space
self.state_space = StateSpace({
"measurements":
VectorObservationSpace(
4, measurements_names=["forward_speed", "x", "y", "z"])
})
for camera in self.scene.sensors:
self.state_space[camera.name] = ImageObservationSpace(
shape=np.array([self.camera_height, self.camera_width, 3]),
high=255)
# action space
if self.separate_actions_for_throttle_and_brake:
self.action_space = BoxActionSpace(
shape=3,
low=np.array([-1, 0, 0]),
high=np.array([1, 1, 1]),
descriptions=["steer", "gas", "brake"])
else:
self.action_space = BoxActionSpace(
shape=2,
low=np.array([-1, -1]),
high=np.array([1, 1]),
descriptions=["steer", "gas_and_brake"])
# human control
if self.human_control:
# convert continuous action space to discrete
self.steering_strength = 0.5
self.gas_strength = 1.0
self.brake_strength = 0.5
# TODO: reverse order of actions
self.action_space = PartialDiscreteActionSpaceMap(
target_actions=[[0., 0.], [0., -self.steering_strength],
[0., self.steering_strength],
[self.gas_strength, 0.],
[-self.brake_strength, 0],
[self.gas_strength, -self.steering_strength],
[self.gas_strength, self.steering_strength],
[self.brake_strength, -self.steering_strength],
[self.brake_strength, self.steering_strength]],
descriptions=[
'NO-OP', 'TURN_LEFT', 'TURN_RIGHT', 'GAS', 'BRAKE',
'GAS_AND_TURN_LEFT', 'GAS_AND_TURN_RIGHT',
'BRAKE_AND_TURN_LEFT', 'BRAKE_AND_TURN_RIGHT'
])
# map keyboard keys to actions
for idx, action in enumerate(self.action_space.descriptions):
for key in key_map.keys():
if action == key:
self.key_to_action[key_map[key]] = idx
self.num_speedup_steps = num_speedup_steps
self.max_speed = max_speed
# measurements
self.autopilot = None
self.planner = Planner(self.map_name)
# env initialization
self.reset_internal_state(True)
# render
if self.is_rendered:
image = self.get_rendered_image()
self.renderer.create_screen(image.shape[1], image.shape[0])
def _add_cameras(self, settings, cameras, camera_width, camera_height):
# add a front facing camera
if CameraTypes.FRONT in cameras:
camera = Camera(CameraTypes.FRONT.value)
camera.set(FOV=100)
camera.set_image_size(camera_width, camera_height)
camera.set_position(2.0, 0, 1.4)
camera.set_rotation(-15.0, 0, 0)
settings.add_sensor(camera)
# add a left facing camera
if CameraTypes.LEFT in cameras:
camera = Camera(CameraTypes.LEFT.value)
camera.set(FOV=100)
camera.set_image_size(camera_width, camera_height)
camera.set_position(2.0, 0, 1.4)
camera.set_rotation(-15.0, -30, 0)
settings.add_sensor(camera)
# add a right facing camera
if CameraTypes.RIGHT in cameras:
camera = Camera(CameraTypes.RIGHT.value)
camera.set(FOV=100)
camera.set_image_size(camera_width, camera_height)
camera.set_position(2.0, 0, 1.4)
camera.set_rotation(-15.0, 30, 0)
settings.add_sensor(camera)
# add a front facing depth camera
if CameraTypes.DEPTH in cameras:
camera = Camera(CameraTypes.DEPTH.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, 30, 0)
camera.PostProcessing = 'Depth'
settings.add_sensor(camera)
# add a front facing semantic segmentation camera
if CameraTypes.SEGMENTATION in cameras:
camera = Camera(CameraTypes.SEGMENTATION.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, 30, 0)
camera.PostProcessing = 'SemanticSegmentation'
settings.add_sensor(camera)
return settings
def _get_directions(self, current_point, end_point):
"""
Class that should return the directions to reach a certain goal
"""
directions = self.planner.get_next_command(
(current_point.location.x, current_point.location.y, 0.22),
(current_point.orientation.x, current_point.orientation.y,
current_point.orientation.z),
(end_point.location.x, end_point.location.y, 0.22),
(end_point.orientation.x, end_point.orientation.y,
end_point.orientation.z))
return directions
def _open_server(self):
log_path = path.join(
self.experiment_path if self.experiment_path is not None else '.',
'logs', "CARLA_LOG_{}.txt".format(self.port))
if not os.path.exists(os.path.dirname(log_path)):
os.makedirs(os.path.dirname(log_path))
with open(log_path, "wb") as out:
cmd = [
path.join(environ.get('CARLA_ROOT'),
'CarlaUE4.sh'), self.map_path, "-benchmark",
"-carla-server", "-fps={}".format(30 / self.frame_skip),
"-world-port={}".format(self.port),
"-windowed -ResX={} -ResY={}".format(self.server_width,
self.server_height),
"-carla-no-hud"
]
if self.config:
cmd.append("-carla-settings={}".format(self.config))
p = subprocess.Popen(cmd, stdout=out, stderr=out)
return p
def _close_server(self):
os.killpg(os.getpgid(self.server.pid), signal.SIGKILL)
def _update_state(self):
# get measurements and observations
measurements = []
while type(measurements) == list:
measurements, sensor_data = self.game.read_data()
self.state = {}
for camera in self.scene.sensors:
self.state[camera.name] = sensor_data[camera.name].data
self.location = [
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
]
is_collision = measurements.player_measurements.collision_vehicles != 0 \
or measurements.player_measurements.collision_pedestrians != 0 \
or measurements.player_measurements.collision_other != 0
speed_reward = measurements.player_measurements.forward_speed - 1
if speed_reward > 30.:
speed_reward = 30.
self.reward = speed_reward \
- (measurements.player_measurements.intersection_otherlane * 5) \
- (measurements.player_measurements.intersection_offroad * 5) \
- is_collision * 100 \
- np.abs(self.control.steer) * 10
# update measurements
self.measurements = [measurements.player_measurements.forward_speed
] + self.location
self.autopilot = measurements.player_measurements.autopilot_control
# The directions to reach the goal (0 Follow lane, 1 Left, 2 Right, 3 Straight)
directions = int(
self._get_directions(measurements.player_measurements.transform,
self.current_goal) - 2)
self.state['high_level_command'] = directions
if (measurements.game_timestamp >=
self.episode_max_time) or is_collision:
# screen.success('EPISODE IS DONE. GameTime: {}, Collision: {}'.format(str(measurements.game_timestamp),
# str(is_collision)))
self.done = True
self.state['measurements'] = np.array(self.measurements)
def _take_action(self, action):
self.control = VehicleControl()
if self.separate_actions_for_throttle_and_brake:
self.control.steer = np.clip(action[0], -1, 1)
self.control.throttle = np.clip(action[1], 0, 1)
self.control.brake = np.clip(action[2], 0, 1)
else:
# transform the 2 value action (steer, throttle - brake) into a 3 value action (steer, throttle, brake)
self.control.steer = np.clip(action[0], -1, 1)
self.control.throttle = np.clip(action[1], 0, 1)
self.control.brake = np.abs(np.clip(action[1], -1, 0))
# prevent braking
if not self.allow_braking or self.control.brake < 0.1 or self.control.throttle > self.control.brake:
self.control.brake = 0
# prevent over speeding
if hasattr(self, 'measurements') and self.measurements[
0] * 3.6 > self.max_speed and self.control.brake == 0:
self.control.throttle = 0.0
self.control.hand_brake = False
self.control.reverse = False
self.game.send_control(self.control)
def _load_experiment(self, experiment_idx):
self.current_experiment = self.experiment_suite.get_experiments(
)[experiment_idx]
self.scene = self.game.load_settings(
self.current_experiment.conditions)
self.positions = self.scene.player_start_spots
self.num_positions = len(self.positions)
self.current_start_position_idx = 0
self.current_pose = 0
def _restart_environment_episode(self, force_environment_reset=False):
# select start and end positions
if self.experiment_suite:
# if an expeirent suite is available, follow its given poses
if self.current_pose >= len(self.current_experiment.poses):
# load a new experiment
self.current_experiment_idx = (
self.current_experiment_idx + 1) % len(
self.experiment_suite.get_experiments())
self._load_experiment(self.current_experiment_idx)
self.current_start_position_idx = self.current_experiment.poses[
self.current_pose][0]
self.current_goal = self.positions[self.current_experiment.poses[
self.current_pose][1]]
self.current_pose += 1
else:
# go over all the possible positions in a cyclic manner
self.current_start_position_idx = (
self.current_start_position_idx + 1) % self.num_positions
# choose a random goal destination
self.current_goal = random.choice(self.positions)
try:
self.game.start_episode(self.current_start_position_idx)
except:
self.game.connect()
self.game.start_episode(self.current_start_position_idx)
# start the game with some initial speed
for i in range(self.num_speedup_steps):
self.control = VehicleControl(throttle=1.0,
brake=0,
steer=0,
hand_brake=False,
reverse=False)
self.game.send_control(VehicleControl())
def get_rendered_image(self) -> np.ndarray:
"""
Return a numpy array containing the image that will be rendered to the screen.
This can be different from the observation. For example, mujoco's observation is a measurements vector.
:return: numpy array containing the image that will be rendered to the screen
"""
image = [self.state[camera.name] for camera in self.scene.sensors]
image = np.vstack(image)
return image
class CarlaEnvironmentWrapper(EnvironmentWrapper):
def __init__(self, num_speedup_steps = 30, require_explicit_reset=True, is_render_enabled=False, early_termination_enabled=False, run_offscreen=False, cameras=['SceneFinal'], save_screens=False):
EnvironmentWrapper.__init__(self, is_render_enabled, save_screens)
self.episode_max_time = 1000000
self.allow_braking = True
self.log_path = 'logs/CarlaLogs.txt'
self.num_speedup_steps = num_speedup_steps
self.is_game_ready_for_input = False
self.run_offscreen = run_offscreen
self.kill_when_connection_lost = True
# server configuration
self.port = get_open_port()
self.host = 'localhost'
self.level = 'town2' #Why town2: https://github.com/carla-simulator/carla/issues/10#issuecomment-342483829
self.map = CarlaLevel().get(self.level)
# client configuration
self.verbose = True
self.observation = None
self.camera_settings = dict(
ImageSizeX=carla_config.server_width,
ImageSizeY=carla_config.server_height,
FOV=110.0,
PositionX=2.0, # 200 for Carla 0.7
PositionY=0.0,
PositionZ=1.40, # 140 for Carla 0.7
RotationPitch = 0.0,
RotationRoll = 0.0,
RotationYaw = 0.0,
)
self.rgb_camera_name = 'CameraRGB'
self.segment_camera_name = 'CameraSegment'
self.depth_camera_name = 'CameraDepth'
self.rgb_camera = 'SceneFinal' in cameras
self.segment_camera = 'SemanticSegmentation' in cameras
self.depth_camera = 'Depth' in cameras
self.class_grouping = carla_config.class_grouping or [(i, ) for i in range(carla_config.no_of_classes)]
self.autocolor_the_segments = False
self.color_the_depth_map = False
self.enable_coalesced_output = False
self.max_depth_value = 1.0 #255.0 for CARLA 7.0
self.min_depth_value = 0.0
self.config = None #'Environment/CarlaSettings.ini'
if self.config:
# load settings from file
with open(self.config, 'r') as fp:
self.settings = CarlaSettings(fp.read())
else:
# hard coded settings
#print("CarlaSettings.ini not found; using default settings")
self.settings = CarlaSettings()
self.settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=15,
NumberOfPedestrians=30,
WeatherId=1,
SeedVehicles = 123456789,
SeedPedestrians = 123456789)
#self.settings.randomize_seeds()
# add cameras
if self.rgb_camera: self.settings.add_sensor(self.create_camera(self.rgb_camera_name, 'SceneFinal'))
if self.segment_camera: self.settings.add_sensor(self.create_camera(self.segment_camera_name, 'SemanticSegmentation'))
if self.depth_camera: self.settings.add_sensor(self.create_camera(self.depth_camera_name, 'Depth'))
self.car_speed = 0
self.is_game_setup = False # Will be true only when setup_client_and_server() is called, either explicitly, or by reset()
# action space
self.discrete_controls = True
self.action_space_size = 2
self.action_space_high = [1, 1]
self.action_space_low = [-1, -1]
self.action_space_abs_range = np.maximum(np.abs(self.action_space_low), np.abs(self.action_space_high))
self.steering_strength = 0.35
self.gas_strength = 1.0
self.brake_strength = 0.6
self.actions = {0: [0., 0.],
1: [0., -self.steering_strength],
2: [0., self.steering_strength],
3: [self.gas_strength-0.15, 0.],
4: [-self.brake_strength, 0],
5: [self.gas_strength-0.3, -self.steering_strength],
6: [self.gas_strength-0.3, self.steering_strength],
7: [-self.brake_strength, -self.steering_strength],
8: [-self.brake_strength, self.steering_strength]}
self.actions_description = ['NO-OP', 'TURN_LEFT', 'TURN_RIGHT', 'GAS', 'BRAKE',
'GAS_AND_TURN_LEFT', 'GAS_AND_TURN_RIGHT',
'BRAKE_AND_TURN_LEFT', 'BRAKE_AND_TURN_RIGHT']
for idx, action in enumerate(self.actions_description):
for key in key_map.keys():
if action == key:
self.key_to_action[key_map[key]] = idx
# measurements
self.measurements_size = (1,)
self.autopilot = None
self.kill_if_unmoved_for_n_steps = 15
self.unmoved_steps = 0.0
self.early_termination_enabled = early_termination_enabled
if self.early_termination_enabled:
self.max_neg_steps = 70
self.cur_neg_steps = 0
self.early_termination_punishment = 20.0
# env initialization
if not require_explicit_reset: self.reset(True)
# render
if self.automatic_render:
self.init_renderer()
if self.save_screens:
create_dir(self.images_path)
self.rgb_img_path = self.images_path+"/rgb/"
create_dir(self.rgb_img_path)
self.segmented_img_path = self.images_path+"/segmented/"
create_dir(self.segmented_img_path)
self.depth_img_path = self.images_path+"/depth/"
create_dir(self.depth_img_path)
def create_camera(self, camera_name, PostProcessing):
#camera = Camera('CameraRGB')
#camera.set_image_size(carla_config.server_width, carla_config.server_height)
#camera.set_position(200, 0, 140)
#camera.set_rotation(0, 0, 0)
#self.settings.add_sensor(camera)
camera = Camera(camera_name, **dict(self.camera_settings, PostProcessing=PostProcessing))
return camera
def setup_client_and_server(self, reconnect_client_only = False):
# open the server
if not reconnect_client_only:
self.server = self._open_server()
self.server_pid = self.server.pid # To kill incase child process gets lost
# open the client
self.game = CarlaClient(self.host, self.port, timeout=99999999)
self.game.connect(connection_attempts=100) #It's taking a very long time for the server process to spawn, so the client needs to wait or try sufficient no. of times lol
scene = self.game.load_settings(self.settings)
# get available start positions
positions = scene.player_start_spots
self.num_pos = len(positions)
self.iterator_start_positions = 0
self.is_game_setup = self.server and self.game
return
def close_client_and_server(self):
self._close_server()
print("Disconnecting the client")
self.game.disconnect()
self.game = None
self.server = None
self.is_game_setup = False
return
def _open_server(self):
# Note: There is no way to disable rendering in CARLA as of now
# https://github.com/carla-simulator/carla/issues/286
# decrease the window resolution if you want to see if performance increases
# Command: $CARLA_ROOT/CarlaUE4.sh /Game/Maps/Town02 -benchmark -carla-server -fps=15 -world-port=9876 -windowed -ResX=480 -ResY=360 -carla-no-hud
# To run off_screen: SDL_VIDEODRIVER=offscreen SDL_HINT_CUDA_DEVICE=0 <command> #https://github.com/carla-simulator/carla/issues/225
my_env = None
if self.run_offscreen:
my_env = {**os.environ, 'SDL_VIDEODRIVER': 'offscreen', 'SDL_HINT_CUDA_DEVICE':'0'}
with open(self.log_path, "wb") as out:
cmd = [path.join(environ.get('CARLA_ROOT'), 'CarlaUE4.sh'), self.map,
"-benchmark", "-carla-server", "-fps=20", "-world-port={}".format(self.port),
"-windowed -ResX={} -ResY={}".format(carla_config.server_width, carla_config.server_height),
"-carla-no-hud", "-quality-level=Low"]
if self.config:
cmd.append("-carla-settings={}".format(self.config))
p = subprocess.Popen(cmd, stdout=out, stderr=out, env=my_env)
return p
def _close_server(self):
if self.kill_when_connection_lost:
os.killpg(os.getpgid(self.server.pid), signal.SIGKILL)
return
no_of_attempts = 0
while is_process_alive(self.server_pid):
print("Trying to close Carla server with pid %d" % self.server_pid)
if no_of_attempts<5:
self.server.terminate()
elif no_of_attempts<10:
self.server.kill()
elif no_of_attempts<15:
os.kill(self.server_pid, signal.SIGTERM)
else:
os.kill(self.server_pid, signal.SIGKILL)
time.sleep(10)
no_of_attempts += 1
def check_early_stop(self, player_measurements, immediate_reward):
if player_measurements.intersection_offroad>0.95 or immediate_reward < -1 or (self.control.throttle == 0.0 and player_measurements.forward_speed < 0.1 and self.control.brake != 0.0):
self.cur_neg_steps += 1
early_done = (self.cur_neg_steps > self.max_neg_steps)
if early_done:
print("Early kill the mad car")
return early_done, self.early_termination_punishment
else:
self.cur_neg_steps /= 2 #Exponentially decay
return False, 0.0
def _update_state(self):
# get measurements and observations
measurements = []
while type(measurements) == list:
try:
measurements, sensor_data = self.game.read_data()
except:
# Connection between cli and server lost; reconnect
if self.kill_when_connection_lost: raise
print("Connection to server lost while reading state. Reconnecting...........")
self.close_client_and_server()
self.setup_client_and_server(reconnect_client_only=False)
self.done = True
self.location = (measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z)
is_collision = measurements.player_measurements.collision_vehicles != 0 \
or measurements.player_measurements.collision_pedestrians != 0 \
or measurements.player_measurements.collision_other != 0
# CARLA doesn't recognize if collision occured and colliding speed is less than 5 km/h (Around 0.7 m/s)
# Ref: https://github.com/carla-simulator/carla/issues/13
# Recognize that as a collision
self.car_speed = measurements.player_measurements.forward_speed
if self.control.throttle > 0 and self.car_speed < 0.75 and self.control.brake==0.0 and self.is_game_ready_for_input:
self.unmoved_steps += 1.0
if self.unmoved_steps > self.kill_if_unmoved_for_n_steps:
is_collision = True
print("Car stuck somewhere lol")
elif self.unmoved_steps>0: self.unmoved_steps -= 0.50 #decay slowly, since it may be stuck and not accelerate few times
if is_collision: print("Collision occured")
speed_reward = self.car_speed - 1
if speed_reward > 30.:
speed_reward = 30.
self.reward = speed_reward*1.2 \
- (measurements.player_measurements.intersection_otherlane * (self.car_speed+1.5)*1.2) \
- (measurements.player_measurements.intersection_offroad * (self.car_speed+2.5)*1.5) \
- is_collision * 250 \
- np.abs(self.control.steer) * 2
# Scale down the reward by a factor
self.reward /= 10
if self.early_termination_enabled:
early_done, punishment = self.check_early_stop(measurements.player_measurements, self.reward)
if early_done:
self.done = True
self.reward -= punishment
# update measurements
self.observation = {
#'observation': sensor_data['CameraRGB'].data,
'acceleration': measurements.player_measurements.acceleration,
'forward_speed': measurements.player_measurements.forward_speed,
'intersection_otherlane': measurements.player_measurements.intersection_otherlane,
'intersection_offroad': measurements.player_measurements.intersection_offroad
}
if self.rgb_camera:
self.observation['rgb_image'] = sensor_data[self.rgb_camera_name].data
if self.segment_camera:
self.observation['segmented_image'] = sensor_data[self.segment_camera_name].data
if self.depth_camera:
self.observation['depth_map'] = sensor_data[self.depth_camera_name].data
if self.segment_camera and self.depth_camera and self.enable_coalesced_output:
self.observation['coalesced_data'] = coalesce_depth_and_segmentation(
self.observation['segmented_image'], self.class_grouping, self.observation['depth_map'], self.max_depth_value)
if self.segment_camera and (self.autocolor_the_segments or self.is_render_enabled):
self.observation['colored_segmented_image'] = convert_segmented_to_rgb(carla_config.colors_segment, self.observation['segmented_image'])
self.autopilot = measurements.player_measurements.autopilot_control
# action_p = ['%.2f' % member for member in [self.control.throttle, self.control.steer]]
# screen.success('REWARD: %.2f, ACTIONS: %s' % (self.reward, action_p))
if (measurements.game_timestamp >= self.episode_max_time) or is_collision:
# screen.success('EPISODE IS DONE. GameTime: {}, Collision: {}'.format(str(measurements.game_timestamp),
# str(is_collision)))
self.done = True
def _take_action(self, action_idx):
if not self.is_game_setup:
print("Reset the environment before calling step")
sys.exit(1)
if type(action_idx) == int:
action = self.actions[action_idx]
else:
action = action_idx
self.control = VehicleControl()
if self.car_speed>35.0 and action[0]>0:
action[0] -= 0.20*(self.car_speed/35.0)
self.control.throttle = np.clip(action[0], 0, 1)
self.control.steer = np.clip(action[1], -1, 1)
self.control.brake = np.abs(np.clip(action[0], -1, 0))
if not self.allow_braking:
self.control.brake = 0
self.control.hand_brake = False
self.control.reverse = False
controls_sent = False
while not controls_sent:
try:
self.game.send_control(self.control)
controls_sent = True
except:
if self.kill_when_connection_lost: raise
print("Connection to server lost while sending controls. Reconnecting...........")
self.close_client_and_server()
self.setup_client_and_server(reconnect_client_only=False)
self.done = True
return
def init_renderer(self):
self.num_cameras = 0
if self.rgb_camera: self.num_cameras += 1
if self.segment_camera: self.num_cameras += 1
if self.depth_camera: self.num_cameras += 1
self.renderer.create_screen(carla_config.render_width, carla_config.render_height*self.num_cameras)
def _restart_environment_episode(self, force_environment_reset=True):
if not force_environment_reset and not self.done and self.is_game_setup:
print("Can't reset dude, episode ain't over yet")
return None #User should handle this
self.is_game_ready_for_input = False
if not self.is_game_setup:
self.setup_client_and_server()
if self.is_render_enabled:
self.init_renderer()
else:
self.iterator_start_positions += 1
if self.iterator_start_positions >= self.num_pos:
self.iterator_start_positions = 0
try:
self.game.start_episode(self.iterator_start_positions)
except:
self.game.connect()
self.game.start_episode(self.iterator_start_positions)
self.unmoved_steps = 0.0
if self.early_termination_enabled:
self.cur_neg_steps = 0
# start the game with some initial speed
self.car_speed = 0
observation = None
for i in range(self.num_speedup_steps):
observation, reward, done, _ = self.step([1.0, 0])
self.observation = observation
self.is_game_ready_for_input = True
return observation
def get_rendered_image(self):
temp = []
if self.rgb_camera: temp.append(self.observation['rgb_image'])
if self.segment_camera:
temp.append(self.observation['colored_segmented_image'])
if self.depth_camera:
if self.color_the_depth_map: temp.append(depthmap_to_rgb(self.observation['depth_map']))
else: temp.append(depthmap_to_grey(self.observation['depth_map']))
return np.vstack((img for img in temp))
def save_screenshots(self):
if not self.save_screens:
print("save_screens is set False")
return
filename = str(int(time.time()*100))
if self.rgb_camera:
save_image(self.rgb_img_path+filename+".png", self.observation['rgb_image'])
if self.segment_camera:
np.save(self.segmented_img_path+filename, self.observation['segmented_image'])
if self.depth_camera:
save_depthmap_as_16bit_png(self.depth_img_path+filename+".png",self.observation['depth_map'],self.max_depth_value,0.95) #Truncating sky as 0
class CarlaEnv(object):
def __init__(
self,
host='eceftl1.ces.clemson.edu',
port=2000,
image_filename_format='_images/episode_{:0>3d}/{:s}/image_{:0>5d}.png',
save_images_to_disk=False):
self.image_filename_format = image_filename_format
self.save_images_to_disk = save_images_to_disk
self.host = host
self.port = port
self.timeout = 60
self._Observation = collections.namedtuple('Observation',
['image', 'label'])
self.frame = 0
self.episode_index = -1
self.client = CarlaClient(self.host, self.port, self.timeout)
def connect(self):
self.client.connect()
def __exit__(self, exc_type, exc_val, exc_tb):
self.client.disconnect()
def start_new_episode(self, player_index):
settings = CarlaSettings()
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=20,
NumberOfPedestrians=40,
WeatherId=random.choice([1, 3, 7, 8, 14]))
settings.randomize_seeds()
camera0 = Camera('CameraRGB')
camera0.set(CameraFOV=100)
camera0.set_image_size(800, 400)
camera0.set_position(120, 0, 130)
settings.add_sensor(camera0)
camera2 = Camera('CameraSeg', PostProcessing='SemanticSegmentation')
camera2.set(CameraFOV=100)
camera2.set_image_size(800, 400)
camera2.set_position(120, 0, 130)
settings.add_sensor(camera2)
self.client.load_settings(settings)
self.episode_index += 1
self.frame = 0
print('\nStarting episode {0}.'.format(self.episode_index))
self.client.start_episode(player_index)
def _get_observation(self, sensor_data):
camera_rgb_data = sensor_data['CameraRGB']
camera_rgb_img = PImage.frombytes(mode='RGBA',
size=(camera_rgb_data.width,
camera_rgb_data.height),
data=camera_rgb_data.raw_data,
decoder_name='raw')
b, g, r, a = camera_rgb_img.split()
camera_rgb_img = PImage.merge("RGB", (r, g, b))
camera_seg_data = sensor_data['CameraSeg']
camera_seg_gray = image_converter.labels_to_grayscale2(
camera_seg_data).astype('uint8')
camera_seg_img = PImage.fromarray(camera_seg_gray, 'L')
return self._Observation(camera_rgb_img, camera_seg_img)
def _gen_images(self, observation):
camera_rgb_img_filename = self.image_filename_format.format(
self.episode_index, 'CameraRGB', self.frame)
camera_seg_img_filename = self.image_filename_format.format(
self.episode_index, 'CameraSeg', self.frame)
rgb_folder = os.path.dirname(camera_rgb_img_filename)
if not os.path.isdir(rgb_folder):
os.makedirs(rgb_folder)
seg_folder = os.path.dirname(camera_seg_img_filename)
if not os.path.isdir(seg_folder):
os.makedirs(seg_folder)
observation.image.save(camera_rgb_img_filename)
observation.label.save(camera_seg_img_filename)
def auto_drive(self):
measurements, sensor_data = self.client.read_data()
print_measurements(measurements)
observation = self._get_observation(sensor_data)
if self.save_images_to_disk:
self._gen_images(self._get_observation(sensor_data))
control = measurements.player_measurements.autopilot_control
control.steer += random.uniform(-0.1, 0.1)
self.client.send_control(control)
self.frame += 1
def step(self, *args, **kwargs):
measurements, sensor_data = self.client.read_data()
print_measurements(measurements)
observation = self._get_observation(sensor_data)
if self.save_images_to_disk:
self._gen_images(observation)
pm = measurements.player_measurements
rewards = pm.collision_vehicles + pm.collision_pedestrians + pm.collision_other
self.client.send_control(*args, **kwargs)
self.frame += 1
return observation, rewards
class CarlaEnv:
def __init__(self, target):
self.carla_client = CarlaClient(config.CARLA_HOST_ADDRESS,
config.CARLA_HOST_PORT,
timeout=100)
self.carla_client.connect()
self.target = target
self.pre_image = None
self.cur_image = None
self.pre_measurements = None
self.cur_measurements = None
def step(self, action):
"""
:param action: dict of control signals, such as {'steer':0, 'throttle':0.2}
:return: next_state, reward, done, info
"""
if isinstance(action, dict):
self.carla_client.send_control(**action)
print(action)
else:
self.carla_client.send_control(action)
measurements, self.cur_image = self.carla_client.read_data()
self.cur_measurements = self.extract_measurements(measurements)
# Todo: Checkup the reward function with the images
reward, done = self.cal_reward()
self.pre_measurements = self.cur_measurements
self.pre_image = self.cur_image
return self.cur_measurements, self.cur_image, reward, done, measurements
def reset(self):
"""
:return:
"""
settings = CarlaSettings()
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=20,
NumberOfPedestrians=40,
WeatherId=random.choice([1, 3, 7, 8, 14]),
QualityLevel='Epic')
# CAMERA
camera0 = Camera('CameraRGB', PostProcessing='SceneFinal')
# Set image resolution in pixels.
camera0.set_image_size(800, 600)
# Set its position relative to the car in meters.
camera0.set_position(0.30, 0, 1.30)
settings.add_sensor(camera0)
# Let's add another camera producing ground-truth depth.
# camera1 = Camera('CameraDepth', PostProcessing='Depth')
# camera1.set_image_size(800, 600)
# camera1.set_position(0.30, 0, 1.30)
# settings.add_sensor(camera1)
# LIDAR
# lidar = Lidar('Lidar32')
# lidar.set_position(0, 0, 2.50)
# lidar.set_rotation(0, 0, 0)
# lidar.set(
# Channels=32,
# Range=50,
# PointsPerSecond=100000,
# RotationFrequency=10,
# UpperFovLimit=10,
# LowerFovLimit=-30)
# settings.add_sensor(lidar)
scene = self.carla_client.load_settings(settings)
self.pre_image = None
self.cur_image = None
self.pre_measurements = None
self.cur_measurements = None
# define a random starting point of the agent for the appropriate trainning
number_of_player_starts = len(scene.player_start_spots)
player_start = random.randint(0, max(0, number_of_player_starts - 1))
# player_start = 140
self.carla_client.start_episode(player_start)
print('Starting new episode at %r, %d...' %
(scene.map_name, player_start))
# TODO: read and return status after reset
return
def extract_measurements(self, measurements):
"""
extract custom measurement data from carla measurement
:param measurements:
:return: custom measurement data dict
"""
p_meas = measurements.player_measurements
pos_x = p_meas.transform.location.x
pos_y = p_meas.transform.location.y
speed = p_meas.forward_speed * 3.6 # m/s -> km/h
col_cars = p_meas.collision_vehicles
col_ped = p_meas.collision_pedestrians
col_other = p_meas.collision_other
other_lane = 100 * p_meas.intersection_otherlane
if other_lane:
print('Intersection into other lane %.2f' % other_lane)
offroad = 100 * p_meas.intersection_offroad
if offroad:
print('offroad %.2f' % offroad)
agents_num = len(measurements.non_player_agents)
distance = np.linalg.norm(np.array([pos_x, pos_y]) - self.target)
meas = {
'pos_x': pos_x,
'pos_y': pos_y,
'speed': speed,
'col_damage': col_cars + col_ped + col_other,
'other_lane': other_lane,
'offroad': offroad,
'agents_num': agents_num,
'distance': distance
}
message = 'Vehicle at %.1f, %.1f, ' % (pos_x, pos_y)
message += '%.0f km/h, ' % (speed, )
message += 'Collision: vehicles=%.0f, pedestrians=%.0f, other=%.0f, ' % (
col_cars,
col_ped,
col_other,
)
message += '%.0f%% other lane, %.0f%% off-road, ' % (
other_lane,
offroad,
)
message += '%d non-player agents in the scene.' % (agents_num, )
# print(message)
return meas
def cal_reward(self):
"""
:param
:return: reward, done
"""
def delta(key):
return self.cur_measurements[key] - self.pre_measurements[key]
if self.pre_measurements is None:
reward = 0.0
else:
reward = 0.05 * delta('speed') - 0.002 * delta('col_damage') \
- 2 * delta('offroad') - 2 * delta('other_lane')
# print('delta speed %.3f' % delta('speed') )
# print('[pre_offroad, current_offroad] =[%.2f, %.2f], reward: %.2f' % (self.pre_measurements['offroad'], self.cur_measurements['offroad'], reward))
# 1000 * delta('distance') + ignore the distance for auto-pilot
# check distance to target
done = self.cur_measurements[
'distance'] < 1 # final state arrived or not
return reward, done
def action_discretize(self, control):
"""
discrete the control action
:param action:
:return:
"""
# print('Before processing, steer=%.5f,throttle=%.2f,brake=%.2f' % (
# action.steer, action.throttle, action.brake))
action = control
steer = int(
100 *
action.steer) + 100 #action.steer has 21 options from [-1, 1]
throttle = int(action.throttle / 0.5) # action.throttle= 0, 0.5 or 1.0
brake = int(action.brake) # action.brake=0 or 1.0
if brake:
gas = -brake # -1
else:
gas = throttle # 0, 1, 2
gas += 1
action_no = steer << 2 | gas # map the action combination into the a numerical value
#gas takes two digits, steer takes 5 digits
action.steer, action.throttle, action.brake = (
steer - 100) / 100.0, throttle * 0.5, brake * 1.0
# print('After processing, steer=%.5f,throttle=%.2f,brake=%.2f' % (
# action.steer, action.throttle, action.brake))
return action_no, action
def reverse_action(self, action_no):
"""
map the action number to the discretized action control
:param action_no:
:return:
"""
gas = action_no & 0b11
throttle = 0.0
brake = 0.0
if gas:
throttle = (gas - 1) * 0.5
else:
brake = abs(gas - 1) * 1.0
steer = (((action_no & 0b1111111100) >> 2) - 100) / 100.0
action = dict()
action['steer'], action['throttle'], action[
'brake'] = steer, throttle, brake
return action
def close(self):
"""
:return:
"""
self.carla_client.disconnect()
class Env(object):
def __init__(self,
log_dir,
data_dir,
image_agent,
city="/Game/Maps/Town01"):
self.log_dir = log_dir
self.data_dir = data_dir
self.carla_server_settings = None
self.server = None
self.server_pid = -99999
self.game = None #carla client
self.map = city
self.host = 'localhost'
self.port = 2366
self.client = None
self.is_connected = False
self.render = None #TODO render with pygame
self.Image_agent = image_agent
self.timestep = 0
self.collision = 0
self.collision_vehicles = 0
self.collision_other = 0
self.stuck_cnt = 0
self.collision_cnt = 0
self.offroad_cnt = 0
self.ignite = False
#steer,throttle,brake
#self.action_space = action_space(3, (1.0, 1.0,1.0), (-1.0,0,0), SEED)
#featured image,speed,steer,other lane ,offroad,
#collision with pedestrians,vehicles,other
#self.observation_space = observation_space(512 + 7)
self.max_episode = 1000000
self.time_out_step = 10000
self.max_speed = 35
self.speed_up_steps = 20
self.current_episode = 0
self.weather = -1
self.current_step = 0
self.current_position = None
self.total_reward = 0
self.planner = None
self.carla_setting = None
self.number_of_vehicles = None
self.control = None
self.nospeed_times = 0
self.reward = 0
self.observation = None
self.done = False
self.testing = False
self.load_config()
self.setup_client_and_server()
def load_config(self):
self.vehicle_pair = carla_config.NumberOfVehicles
self.pedestrian_pair = carla_config.NumberOfPedestrians
self.weather_set = carla_config.set_of_weathers
#[straight,one_curve,navigation,navigation]
if self.map == "/Game/Maps/Town01":
self.poses = carla_config.poses_town01()
elif self.map == "/Game/Maps/Town02":
self.poses = carla_config.poses_town02()
else:
print("Unsupported Map Name")
def reset(self, vehicle_num):
#if not self.is_process_alive(self.server_pid):
#self.setup_client_and_server()
self.nospeed_times = 0
pose_type = random.choice(self.poses)
#pose_type = self.poses[0]
self.current_position = random.choice(
pose_type) #start and end index
#self.current_position = (53,67)
# self.number_of_vehicles = random.randint( self.vehicle_pair[0],self.vehicle_pair[1])
# self.number_of_pedestrians = random.randint( self.vehicle_pair[0],self.vehicle_pair[1])
self.number_of_vehicles = vehicle_num
self.number_of_pedestrians = 0
self.weather = random.choice(self.weather_set)
self.timestep = 0
self.collision = 0
self.collision_vehicles = 0
self.collision_other = 0
self.stuck_cnt = 0
self.collision_cnt = 0
self.offroad_cnt = 0
self.ignite = False
settings = carla_config.make_carla_settings()
settings.set(NumberOfVehicles=self.number_of_vehicles,
NumberOfPedestrians=self.number_of_pedestrians,
WeatherId=self.weather)
self.carla_setting = settings
self.scene = self.game.load_settings(settings)
self.game.start_episode(
self.current_position[0]) #set the start position
#print(self.current_position)
self.target_transform = self.scene.player_start_spots[
self.current_position[1]]
self.planner = Planner(self.scene.map_name)
#skip the car fall to sence frame
for i in range(self.speed_up_steps):
self.control = VehicleControl()
self.control.steer = 0
self.control.throttle = 0.025 * i
self.control.brake = 0
self.control.hand_brake = False
self.control.reverse = False
time.sleep(0.05)
send_success = self.send_control(self.control)
if not send_success:
return None
self.game.send_control(self.control)
#measurements, sensor_data = self.game.read_data() #measurements,sensor
#direction =self.get_directions(measurements,self.target_transform,self.planner)
#self.get_state(measurements,sensor_data,direction)
measurements, sensor_data = self.game.read_data() #measurements,sensor
directions = self.get_directions(measurements, self.target_transform,
self.planner)
if directions is None or measurements is None:
return None
state, _, _ = self.get_state(measurements, sensor_data, directions)
return state
def get_data(self):
measurements = None
sensor_data = None
try:
measurements, sensor_data = self.game.read_data()
except Exception:
return None, None
return measurements, sensor_data
def send_control(self, control):
send_success = False
try:
self.game.send_control(control)
send_success = True
except Exception:
print("Send Control error")
return send_success
def step(self, action):
#take action ,update state
#return: observation, reward,done
self.control = VehicleControl()
self.control.steer = np.clip(action[0], -1, 1)
self.control.throttle = np.clip(action[1], 0, 1)
self.control.brake = np.abs(np.clip(action[2], 0, 1))
self.control.hand_brake = False
self.control.reverse = False
send_success = self.send_control(self.control)
if not send_success:
return None, None, None
#recive new data
measurements, sensor_data = self.game.read_data() #measurements,sensor
directions = self.get_directions(measurements, self.target_transform,
self.planner)
if measurements is None or directions is None:
return None, None, None
state, reward, done = self.get_state(measurements, sensor_data,
directions)
self.current_step += 1
return state, reward, done
#comute new state,reward,and is done
def get_state(self, measurements, sensor_data, directions):
self.reward = 0
done = False
img_feature = self.Image_agent.compute_feature(
sensor_data) #shape = (512,)
speed = measurements.player_measurements.forward_speed # m/s
intersection_offroad = measurements.player_measurements.intersection_offroad
intersection_otherlane = measurements.player_measurements.intersection_otherlane
collision_vehicles = measurements.player_measurements.collision_vehicles
collision_pedestrians = measurements.player_measurements.collision_pedestrians
collision_other = measurements.player_measurements.collision_other
# reward for steer
if directions == 5: #go straight
if abs(self.control.steer) > 0.2:
self.reward -= 20
self.reward += min(35, speed * 3.6)
elif directions == 2: #follow lane
self.reward += min(25, speed * 3.6)
elif directions == 3: #turn left ,steer should be negtive
if self.control.steer > 0:
self.reward -= 15
if speed * 3.6 <= 20:
self.reward += speed * 3.6
else:
self.reward += 40 - speed * 3.6
elif directions == 4: #turn right
if self.control.steer < 0:
self.reward -= 15
if speed * 3.6 <= 20:
self.reward += speed * 3.6
else:
self.reward += 40 - speed * 3.6
# reward for offroad and collision
if intersection_offroad > 0:
self.reward -= 100
if intersection_otherlane > 0:
self.reward -= 100
elif collision_vehicles > 0:
self.reward -= 100
elif collision_pedestrians > 0:
self.reward -= 100
elif collision_other > 0:
self.reward -= 50
# teminal state
if collision_pedestrians > 0 or collision_vehicles > 0 or collision_other > 0:
done = True
#print("Collision~~~~~")
if intersection_offroad > 0.2 or intersection_otherlane > 0.2:
done = True
#print("Offroad~~~~~")
if speed * 3.6 <= 1.0:
self.nospeed_times += 1
if self.nospeed_times > 100:
done = True
self.reward -= 1
else:
self.nospeed_times = 0
speed = min(1, speed / 20.0)
info = self.convert_info(measurements)
self.reward = self.reward_from_info(info)
done = self.done_from_info(info)
return np.concatenate(
(img_feature, (speed, directions))), self.reward, done
def done_from_info(self, info):
if info['collision'] > 0 or (self.collision_cnt > 0
and info['speed'] < 0.5):
self.collision_cnt += 1
else:
self.collision_cnt = 0
self.ignite = self.ignite or info['speed'] > 1
stuck = int(info['speed'] < 1)
self.stuck_cnt = (self.stuck_cnt + stuck) * stuck * int(
bool(self.ignite) or self.testing)
if info['offroad'] > 0.5:
self.offroad_cnt += 1
if self.stuck_cnt > 30:
print("finish becase of stuck")
return True
elif self.offroad_cnt > 30:
print("finish because of offroad")
return True
elif self.collision_cnt > 20:
print("finish because of collision")
return True
else:
return False
def reward_from_info(self, info):
reward = dict()
reward['without_pos'] = info['speed'] / 15 - info[
'offroad'] - info['collision'] * 2.0
reward['with_pos'] = reward['without_pos'] - info['offlane'] / 5
return reward['with_pos']
def convert_info(self, measurements):
info = dict()
info['speed'] = measurements.player_measurements.forward_speed
info['collision'] = int(
measurements.player_measurements.collision_other +
measurements.player_measurements.collision_pedestrians +
measurements.player_measurements.collision_vehicles >
self.collision or (self.collision_cnt > 0 and info['speed'] < 0.5))
info['collision_other'] = int(measurements.player_measurements.
collision_other > self.collision_other)
info['collision_vehicles'] = int(
measurements.player_measurements.collision_vehicles >
self.collision_vehicles)
info['coll_veh_num'] = int(
measurements.player_measurements.collision_vehicles -
self.collision_vehicles)
self.collision_vehicles = measurements.player_measurements.collision_vehicles
self.collision_other = measurements.player_measurements.collision_other
self.collision = measurements.player_measurements.collision_other + measurements.player_measurements.collision_pedestrians + measurements.player_measurements.collision_vehicles
info['offlane'] = int(
measurements.player_measurements.intersection_otherlane > 0.01)
info['offroad'] = int(
measurements.player_measurements.intersection_offroad > 0.001)
info[
'expert_control'] = measurements.player_measurements.autopilot_control
return info
def _open_server(self):
with open(self.log_dir, "wb") as out:
cmd = [
os.path.join(os.environ.get('CARLA_ROOT'),
'CarlaUE4.sh'), self.map, "-carla-server",
"-fps=10", "-world-port={}".format(self.port),
"-windowed -ResX={} -ResY={}".format(
carla_config.WINDOW_WIDTH, carla_config.WINDOW_HEIGHT)
]
if self.carla_server_settings:
cmd.append("-carla-settings={}".format(
self.carla_server_settings))
p = subprocess.Popen(cmd, stdout=out, stderr=out)
time.sleep(20)
return p
# This is not work
def _close_server(self):
no_of_attempts = 0
try:
while self.is_process_alive(self.server_pid):
print("Trying to close Carla server with pid %d" %
self.server_pid)
if no_of_attempts < 5:
self.server.terminate()
elif no_of_attempts < 10:
self.server.kill()
elif no_of_attempts < 15:
os.kill(self.server_pid, signal.SIGTERM)
else:
os.kill(self.server_pid, signal.SIGKILL)
time.sleep(10)
no_of_attempts += 1
except Exception as e:
print(e)
def close_server(self):
sudopass = "******"
command = "echo {0} |sudo -S kill -9 {1}".format(
sudopass, self.server_pid)
os.system(command)
def is_process_alive(self, pid):
## Source: https://stackoverflow.com/questions/568271/how-to-check-if-there-exists-a-process-with-a-given-pid-in-python
try:
os.kill(pid, 0)
except OSError:
return False
return True
def setup_client_and_server(self):
# self.server = self._open_server()
# self.server_pid = self.server.pid
self.game = CarlaClient(self.host, self.port,
timeout=99999999) #carla client
self.game.connect(connection_attempts=100)
def get_directions(self, measurements, target_transform, planner):
""" Function to get the high level commands and the waypoints.
The waypoints correspond to the local planning, the near path the car has to follow.
"""
# Get the current position from the measurements
current_point = measurements.player_measurements.transform
try:
directions = planner.get_next_command(
(current_point.location.x, current_point.location.y, 0.22),
(current_point.orientation.x, current_point.orientation.y,
current_point.orientation.z),
(target_transform.location.x, target_transform.location.y,
0.22), (target_transform.orientation.x,
target_transform.orientation.y,
target_transform.orientation.z))
except Exception:
print("Route plan error ")
directions = None
return directions
class CarlaHuman(Driver):
def __init__(self, driver_conf):
Driver.__init__(self)
# some behaviors
self._autopilot = driver_conf.autopilot
self._reset_period = driver_conf.reset_period # those reset period are in the actual system time, not in simulation time
self._goal_reaching_threshold = 3
self.use_planner = driver_conf.use_planner
# we need the planner to find a valid episode, so we initialize one no matter what
self._world = None
self._vehicle = None
self._agent_autopilot = None
self._camera_center = None
self._spectator = None
# (last) images store for several cameras
self._data_buffers = dict()
self.update_once = False
self._collision_events = []
self.collision_sensor = None
if __CARLA_VERSION__ == '0.8.X':
self.planner = Planner(driver_conf.city_name)
else:
self.planner = None
self.use_planner = False
# resources
self._host = driver_conf.host
self._port = driver_conf.port
# various config files
self._driver_conf = driver_conf
self._config_path = driver_conf.carla_config
# some initializations
self._straight_button = False
self._left_button = False
self._right_button = False
self._rear = False
self._recording = False
self._skiped_frames = 20
self._stucked_counter = 0
self._prev_time = datetime.now()
self._episode_t0 = datetime.now()
self._vehicle_prev_location = namedtuple("vehicle", "x y z")
self._vehicle_prev_location.x = 0.0
self._vehicle_prev_location.y = 0.0
self._vehicle_prev_location.z = 0.0
self._camera_left = None
self._camera_right = None
self._camera_center = None
self._actor_list = []
self._sensor_list = []
self._weather_list = [
'ClearNoon', 'CloudyNoon', 'WetNoon', 'WetCloudyNoon',
'MidRainyNoon', 'HardRainNoon', 'SoftRainNoon', 'ClearSunset',
'CloudySunset', 'WetSunset', 'WetCloudySunset', 'MidRainSunset',
'HardRainSunset', 'SoftRainSunset'
]
self._current_weather = 4
self._current_command = 2.0
# steering wheel
self._steering_wheel_flag = True
if self._steering_wheel_flag:
self._is_on_reverse = False
self._control = VehicleControl()
self._parser = SafeConfigParser()
self._parser.read('wheel_config.ini')
self._steer_idx = int(
self._parser.get('G29 Racing Wheel', 'steering_wheel'))
self._throttle_idx = int(
self._parser.get('G29 Racing Wheel', 'throttle'))
self._brake_idx = int(self._parser.get('G29 Racing Wheel',
'brake'))
self._reverse_idx = int(
self._parser.get('G29 Racing Wheel', 'reverse'))
self._handbrake_idx = int(
self._parser.get('G29 Racing Wheel', 'handbrake'))
self.last_timestamp = lambda x: x
self.last_timestamp.elapsed_seconds = 0.0
self.last_timestamp.delta_seconds = 0.2
self.initialize_map(driver_conf.city_name)
def start(self):
if __CARLA_VERSION__ == '0.8.X':
self.carla = CarlaClient(self._host, int(self._port), timeout=120)
self.carla.connect()
else:
self.carla = CarlaClient(self._host, int(self._port))
self.carla.set_timeout(5000)
wd = self.carla.get_world()
self.wd = wd
settings = wd.get_settings()
settings.synchronous_mode = True
wd.apply_settings(settings)
self._reset()
if not self._autopilot:
pygame.joystick.init()
joystick_count = pygame.joystick.get_count()
if joystick_count > 1:
print("Please Connect Just One Joystick")
raise ValueError()
self.joystick = pygame.joystick.Joystick(0)
self.joystick.init()
def test_alive(self):
if not hasattr(self, "wd"):
return False
wd = self.wd
wd.tick()
try:
wd.wait_for_tick(5.0)
except:
return False
return True
def __del__(self):
if hasattr(self, 'carla'):
print("destructing the connection")
if __CARLA_VERSION__ == '0.8.X':
self.carla.disconnect()
else:
alive = self.test_alive()
# destroy old actors
print('destroying actors')
if alive:
if len(self._actor_list) > 0:
for actor in self._actor_list:
actor.destroy()
self._actor_list = []
print('done.')
if self._vehicle is not None:
if alive:
self._vehicle.destroy()
self._vehicle = None
if self._camera_center is not None:
if alive:
self._camera_center.destroy()
self._camera_center = None
if self._camera_left is not None:
if alive:
self._camera_left.destroy()
self._camera_left = None
if self._camera_right is not None:
if alive:
self._camera_right.destroy()
self._camera_right = None
if self.collision_sensor is not None:
if alive:
self.collision_sensor.sensor.destroy()
self.collision_sensor = None
# pygame.quit()
# if self._camera is not None:
# self._camera.destroy()
# self._camera = None
# if self._vehicle is not None:
# self._vehicle.destroy()
# self._vehicle = None
def try_spawn_random_vehicle_at(self,
blueprints,
transform,
auto_drive=True):
blueprint = random.choice(blueprints)
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
vehicle = self._world.try_spawn_actor(blueprint, transform)
if vehicle is not None:
self._actor_list.append(vehicle)
if auto_drive:
# TODO: this won't work in 0.9.5 with Exp_Town
# however, we don't have traffic in that town right now, so we just ignore this
vehicle.set_autopilot()
#print('spawned %r at %s' % (vehicle.type_id, transform.location))
return True
return False
def get_parking_locations(self,
filename,
z_default=0.0,
random_perturb=False):
with open(filename, "r") as f:
lines = f.readlines()
ans = []
for line in lines:
x, y, yaw = [float(v.strip()) for v in line.split(",")]
if random_perturb:
x += np.random.normal(
0, scale=self._driver_conf.extra_explore_location_std)
y += np.random.normal(
0, scale=self._driver_conf.extra_explore_location_std)
yaw += np.random.normal(
0, scale=self._driver_conf.extra_explore_yaw_std)
ans.append(
carla.Transform(location=carla.Location(x=x,
y=y,
z=z_default),
rotation=carla.Rotation(roll=0,
pitch=0,
yaw=yaw)))
return ans
def print_transform(self, t):
print(t.location.x, t.location.y, t.location.z)
print(t.rotation.roll, t.rotation.pitch, t.rotation.yaw)
def _reset(self):
self._episode_t0 = datetime.now()
if __CARLA_VERSION__ == '0.8.X':
# create the carla config based on template and the params passed in
config = ConfigParser()
config.optionxform = str
config.read(self._config_path)
config.set('CARLA/LevelSettings', 'NumberOfVehicles',
self._driver_conf.cars)
config.set('CARLA/LevelSettings', 'NumberOfPedestrians',
self._driver_conf.pedestrians)
config.set('CARLA/LevelSettings', 'WeatherId',
self._driver_conf.weather)
output = io.StringIO()
config.write(output)
scene_descriptions = self.carla.load_settings(output.getvalue())
# based on the scene descriptions, find the start and end positions
self.positions = scene_descriptions.player_start_spots
# the episode_config saves [start_index, end_index]
self.episode_config = find_valid_episode_position(
self.positions, self.planner)
self.carla.start_episode(self.episode_config[0])
print('RESET ON POSITION ', self.episode_config[0],
", the target location is: ", self.episode_config[1])
else:
# destroy old actors
print('destroying actors')
for actor in self._actor_list:
actor.destroy()
self._actor_list = []
print('done.')
# TODO: spawn pedestrains
# TODO: spawn more vehicles
if self._autopilot:
self._current_weather = self._weather_list[
int(self._driver_conf.weather) - 1]
else:
self._current_weather = random.choice(self._weather_list)
if not self._autopilot:
# select one of the random starting points previously selected
start_positions = np.loadtxt(self._driver_conf.positions_file,
delimiter=',')
if len(start_positions.shape) == 1:
start_positions = start_positions.reshape(
1, len(start_positions))
# TODO: Assign random position from file
WINDOW_WIDTH = 768
WINDOW_HEIGHT = 576
CAMERA_FOV = 103.0
CAMERA_CENTER_T = carla.Location(x=0.7, y=-0.0, z=1.60)
CAMERA_LEFT_T = carla.Location(x=0.7, y=-0.4, z=1.60)
CAMERA_RIGHT_T = carla.Location(x=0.7, y=0.4, z=1.60)
CAMERA_CENTER_ROTATION = carla.Rotation(roll=0.0,
pitch=0.0,
yaw=0.0)
CAMERA_LEFT_ROTATION = carla.Rotation(roll=0.0,
pitch=0.0,
yaw=-45.0)
CAMERA_RIGHT_ROTATION = carla.Rotation(roll=0.0,
pitch=0.0,
yaw=45.0)
CAMERA_CENTER_TRANSFORM = carla.Transform(
location=CAMERA_CENTER_T, rotation=CAMERA_CENTER_ROTATION)
CAMERA_LEFT_TRANSFORM = carla.Transform(
location=CAMERA_LEFT_T, rotation=CAMERA_LEFT_ROTATION)
CAMERA_RIGHT_TRANSFORM = carla.Transform(
location=CAMERA_RIGHT_T, rotation=CAMERA_RIGHT_ROTATION)
self._world = self.carla.get_world()
settings = self._world.get_settings()
settings.synchronous_mode = True
self._world.apply_settings(settings)
# add traffic
blueprints_vehi = self._world.get_blueprint_library().filter(
'vehicle.*')
blueprints_vehi = [
x for x in blueprints_vehi
if int(x.get_attribute('number_of_wheels')) == 4
]
blueprints_vehi = [
x for x in blueprints_vehi if not x.id.endswith('isetta')
]
# @todo Needs to be converted to list to be shuffled.
spawn_points = list(self._world.get_map().get_spawn_points())
if len(spawn_points) == 0:
if self.city_name_demo == "Exp_Town":
spawn_points = [
carla.Transform(
location=carla.Location(x=-11.5, y=-8.0, z=2.0))
]
random.shuffle(spawn_points)
print('found %d spawn points.' % len(spawn_points))
# TODO: debug change 50 to 0
count = 0
if count > 0:
for spawn_point in spawn_points:
if self.try_spawn_random_vehicle_at(
blueprints_vehi, spawn_point):
count -= 1
if count <= 0:
break
while count > 0:
time.sleep(0.5)
if self.try_spawn_random_vehicle_at(
blueprints_vehi, random.choice(spawn_points)):
count -= 1
# end traffic addition!
# begin parking addition
if hasattr(
self._driver_conf, "parking_position_file"
) and self._driver_conf.parking_position_file is not None:
parking_points = self.get_parking_locations(
self._driver_conf.parking_position_file)
random.shuffle(parking_points)
print('found %d parking points.' % len(parking_points))
count = 200
for spawn_point in parking_points:
self.try_spawn_random_vehicle_at(blueprints_vehi,
spawn_point, False)
count -= 1
if count <= 0:
break
# end of parking addition
blueprints = self._world.get_blueprint_library().filter('vehicle')
vechile_blueprint = [
e for i, e in enumerate(blueprints)
if e.id == 'vehicle.lincoln.mkz2017'
][0]
if self._vehicle == None or self._autopilot:
if self._autopilot and self._vehicle is not None:
self._vehicle.destroy()
self._vehicle = None
while self._vehicle == None:
if self._autopilot:
# from designated points
if hasattr(self._driver_conf,
"extra_explore_prob") and random.random(
) < self._driver_conf.extra_explore_prob:
extra_positions = self.get_parking_locations(
self._driver_conf.extra_explore_position_file,
z_default=3.0,
random_perturb=True)
print(
"spawning hero vehicle from the extra exploration"
)
START_POSITION = random.choice(extra_positions)
else:
START_POSITION = random.choice(spawn_points)
else:
random_position = start_positions[
np.random.randint(start_positions.shape[0]), :]
START_POSITION = carla.Transform(
carla.Location(x=random_position[0],
y=random_position[1],
z=random_position[2] + 1.0),
carla.Rotation(pitch=random_position[3],
roll=random_position[4],
yaw=random_position[5]))
self._vehicle = self._world.try_spawn_actor(
vechile_blueprint, START_POSITION)
else:
if self._autopilot:
# from designated points
START_POSITION = random.choice(spawn_points)
else:
random_position = start_positions[
np.random.randint(start_positions.shape[0]), :]
START_POSITION = carla.Transform(
carla.Location(x=random_position[0],
y=random_position[1],
z=random_position[2] + 1.0),
carla.Rotation(pitch=random_position[3],
roll=random_position[4],
yaw=random_position[5]))
self._vehicle.set_transform(START_POSITION)
print("after spawning the ego vehicle")
print("warm up process to make the vehicle ego location correct")
wd = self._world
for i in range(25):
wd.tick()
if not wd.wait_for_tick(10.0):
continue
print("warmup finished")
if self._autopilot:
# Nope: self._vehicle.set_autopilot()
print("before roaming agent")
self._agent_autopilot = RoamingAgent(self._vehicle)
print("after roaming agent")
if self.collision_sensor is not None:
print("before destroying the sensor")
self.collision_sensor.sensor.destroy()
print("after destroying the sensor")
else:
print("collision sensor is None")
self.collision_sensor = CollisionSensor(self._vehicle, self)
print("after spawning the collision sensor")
# set weather
weather = getattr(carla.WeatherParameters, self._current_weather)
self._vehicle.get_world().set_weather(weather)
self._spectator = self._world.get_spectator()
cam_blueprint = self._world.get_blueprint_library().find(
'sensor.camera.rgb')
cam_blueprint.set_attribute('image_size_x', str(WINDOW_WIDTH))
cam_blueprint.set_attribute('image_size_y', str(WINDOW_HEIGHT))
cam_blueprint.set_attribute('fov', str(CAMERA_FOV))
if self._camera_center is not None:
self._camera_center.destroy()
self._camera_left.destroy()
self._camera_right.destroy()
self._camera_center = None
if self._camera_center == None:
self._camera_center = self._world.spawn_actor(
cam_blueprint,
CAMERA_CENTER_TRANSFORM,
attach_to=self._vehicle)
self._camera_left = self._world.spawn_actor(
cam_blueprint,
CAMERA_LEFT_TRANSFORM,
attach_to=self._vehicle)
self._camera_right = self._world.spawn_actor(
cam_blueprint,
CAMERA_RIGHT_TRANSFORM,
attach_to=self._vehicle)
self._camera_center.listen(CallBack('CameraMiddle', self))
self._camera_left.listen(CallBack('CameraLeft', self))
self._camera_right.listen(CallBack('CameraRight', self))
# spectator server camera
self._spectator = self._world.get_spectator()
self._skiped_frames = 0
self._stucked_counter = 0
self._start_time = time.time()
def get_recording(self):
if self._autopilot:
# debug: 0 for debugging
if self._skiped_frames >= 20:
return True
else:
self._skiped_frames += 1
return False
else:
'''
if (self.joystick.get_button(8)):
self._recording = True
if (self.joystick.get_button(9)):
self._recording = False
'''
if (self.joystick.get_button(6)):
self._recording = True
print("start recording!!!!!!!!!!!!!!!!!!!!!!!!1")
if (self.joystick.get_button(7)):
self._recording = False
print("end recording!!!!!!!!!!!!!!!!!!!!!!!!1")
return self._recording
def initialize_map(self, city_name):
self.city_name_demo = city_name
if city_name == "RFS_MAP":
path = get_current_folder() + "/maps_demo_area/rfs_demo_area.png"
im = cv2.imread(path)
im = im[:, :, :3]
im = im[:, :, ::-1]
self.demo_area_map = im
else:
print("do nothing since not a city with demo area")
#raise ValueError("wrong city name: " + city_name)
def loc_to_pix_rfs_sim(self, loc):
u = 3.6090651558073654 * loc[1] + 2500.541076487252
v = -3.6103367739019054 * loc[0] + 2501.862578166202
return [int(v), int(u)]
def in_valid_area(self, x, y):
if self.city_name_demo == "RFS_MAP":
pos = self.loc_to_pix_rfs_sim([x, y])
locality = 50 # 100 pixels
local_area = self.demo_area_map[pos[0] - locality:pos[0] +
locality, pos[1] -
locality:pos[1] + locality, 0] > 0
valid = np.sum(local_area) > 0
if not valid:
print(
"detect the vehicle is not in the valid demonstrated area")
return valid
else:
return True
def get_reset(self):
if self._autopilot:
if __CARLA_VERSION__ == '0.8.X':
# increase the stuck detector if conditions satisfy
if self._latest_measurements.player_measurements.forward_speed < 0.1:
self._stucked_counter += 1
else:
self._stucked_counter = 0
# if within auto pilot, reset if long enough or has collisions
if time.time() - self._start_time > self._reset_period \
or self._latest_measurements.player_measurements.collision_vehicles > 0.0 \
or self._latest_measurements.player_measurements.collision_pedestrians > 0.0 \
or self._latest_measurements.player_measurements.collision_other > 0.0 \
or (self._latest_measurements.player_measurements.intersection_otherlane > 0.0 and self._latest_measurements.player_measurements.autopilot_control.steer < -0.99) \
or self._stucked_counter > 250:
if self._stucked_counter > 250:
reset_because_stuck = True
else:
reset_because_stuck = False
# TODO: commenting out this for debugging issue
self._reset()
if reset_because_stuck:
print("resetting because getting stucked.....")
return True
else:
# TODO: implement the collision detection algorithm, based on the new API
if self.last_estimated_speed < 0.1:
self._stucked_counter += 1
else:
self._stucked_counter = 0
if time.time() - self._start_time > self._reset_period \
or self._last_collided \
or self._stucked_counter > 250 \
or not self.in_valid_area(self._latest_measurements.player_measurements.transform.location.x,
self._latest_measurements.player_measurements.transform.location.y):
#or np.abs(self._vehicle.get_vehicle_control().steer) > 0.95:
#or np.abs(self._vehicle.get_vehicle_control().brake) > 1:
# TODO intersection other lane is not available, so omit from the condition right now
if self._stucked_counter > 250:
reset_because_stuck = True
else:
reset_because_stuck = False
if self._last_collided:
print("reset becuase collision")
self._reset()
if reset_because_stuck:
print("resetting because getting stucked.....")
return True
else:
pass
return False
def get_waypoints(self):
# TODO: waiting for German Ros to expose the waypoints
wp1 = [1.0, 1.0]
wp2 = [2.0, 2.0]
return [wp1, wp2]
def action_joystick(self):
# joystick
steering_axis = self.joystick.get_axis(0)
acc_axis = self.joystick.get_axis(2)
brake_axis = self.joystick.get_axis(5)
# print("axis 0 %f, axis 2 %f, axis 3 %f" % (steering_axis, acc_axis, brake_axis))
if (self.joystick.get_button(3)):
self._rear = True
if (self.joystick.get_button(2)):
self._rear = False
control = VehicleControl()
control.steer = steering_axis
control.throttle = (acc_axis + 1) / 2.0
control.brake = (brake_axis + 1) / 2.0
if control.brake < 0.001:
control.brake = 0.0
control.hand_brake = 0
control.reverse = self._rear
control.steer -= 0.0822
#print("steer %f, throttle %f, brake %f" % (control.steer, control.throttle, control.brake))
pygame.event.pump()
return control
def action_steering_wheel(self, jsInputs, jsButtons):
control = VehicleControl()
# Custom function to map range of inputs [1, -1] to outputs [0, 1] i.e 1 from inputs means nothing is pressed
# For the steering, it seems fine as it is
K1 = 1.0 # 0.55
steerCmd = K1 * math.tan(1.1 * jsInputs[self._steer_idx])
K2 = 1.6 # 1.6
throttleCmd = K2 + (
2.05 * math.log10(-0.7 * jsInputs[self._throttle_idx] + 1.4) -
1.2) / 0.92
if throttleCmd <= 0:
throttleCmd = 0
elif throttleCmd > 1:
throttleCmd = 1
brakeCmd = 1.6 + (2.05 * math.log10(-0.7 * jsInputs[self._brake_idx] +
1.4) - 1.2) / 0.92
if brakeCmd <= 0:
brakeCmd = 0
elif brakeCmd > 1:
brakeCmd = 1
#print("Steer Cmd, ", steerCmd, "Brake Cmd", brakeCmd, "ThrottleCmd", throttleCmd)
control.steer = steerCmd
control.brake = brakeCmd
control.throttle = throttleCmd
toggle = jsButtons[self._reverse_idx]
if toggle == 1:
self._is_on_reverse += 1
if self._is_on_reverse % 2 == 0:
control.reverse = False
if self._is_on_reverse > 1:
self._is_on_reverse = True
if self._is_on_reverse:
control.reverse = True
control.hand_brake = False # jsButtons[self.handbrake_idx]
return control
def compute_action(self, sensor, speed):
if not self._autopilot:
# get pygame input
for event in pygame.event.get():
# Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN
# JOYBUTTONUP JOYHATMOTION
if event.type == pygame.JOYBUTTONDOWN:
if event.__dict__['button'] == 0:
self._current_command = 2.0
if event.__dict__['button'] == 1:
self._current_command = 3.0
if event.__dict__['button'] == 2:
self._current_command = 4.0
if event.__dict__['button'] == 3:
self._current_command = 5.0
if event.__dict__['button'] == 23:
self._current_command = 0.0
if event.__dict__['button'] == 4:
self._reset()
return VehicleControl()
if event.type == pygame.JOYBUTTONUP:
self._current_command = 2.0
#pygame.event.pump()
numAxes = self.joystick.get_numaxes()
jsInputs = [
float(self.joystick.get_axis(i)) for i in range(numAxes)
]
# print (jsInputs)
jsButtons = [
float(self.joystick.get_button(i))
for i in range(self.joystick.get_numbuttons())
]
if self._steering_wheel_flag:
control = self.action_steering_wheel(jsInputs, jsButtons)
else:
control = self.action_joystick()
else:
if __CARLA_VERSION__ == '0.8.X':
# This relies on the calling of get_sensor_data, otherwise self._latest_measurements are not filled
control = self._latest_measurements.player_measurements.autopilot_control
print('[Throttle = {}] [Steering = {}] [Brake = {}]'.format(
control.throttle, control.steer, control.brake))
else:
self._world.tick()
if self._world.wait_for_tick(10.0):
control, self._current_command = self._agent_autopilot.run_step(
)
print('[Throttle = {}] [Steering = {}] [Brake = {}]'.format(
control.throttle, control.steer, control.brake))
return control
def estimate_speed(self):
vel = self._vehicle.get_velocity()
speed = m.sqrt(vel.x**2 + vel.y**2 + vel.z**2)
# speed in m/s
return speed
def get_sensor_data(self, goal_pos=None, goal_ori=None):
if __CARLA_VERSION__ == '0.8.X':
# return the latest measurement and the next direction
measurements, sensor_data = self.carla.read_data()
self._latest_measurements = measurements
if self.use_planner:
player_data = measurements.player_measurements
pos = [
player_data.transform.location.x,
player_data.transform.location.y, 0.22
]
ori = [
player_data.transform.orientation.x,
player_data.transform.orientation.y,
player_data.transform.orientation.z
]
if sldist([
player_data.transform.location.x,
player_data.transform.location.y
], [
self.positions[self.episode_config[1]].location.x,
self.positions[self.episode_config[1]].location.y
]) < self._goal_reaching_threshold:
self._reset()
direction = self.planner.get_next_command(
pos, ori, [
self.positions[self.episode_config[1]].location.x,
self.positions[self.episode_config[1]].location.y, 0.22
], (1, 0, 0))
else:
direction = 2.0
else:
self.last_timestamp.elapsed_seconds += 0.2
#self.last_timestamp = self.carla.get_world().wait_for_tick(30.0)
#print(timestamp.delta_seconds, "delta seconds")
#while self.update_once == False:
# time.sleep(0.01)
self.last_estimated_speed = self.estimate_speed()
data_buffer_lock.acquire()
sensor_data = copy.deepcopy(self._data_buffers)
data_buffer_lock.release()
#self.update_once = False
collision_lock.acquire()
colllision_event = self._collision_events
self._last_collided = len(colllision_event) > 0
self._collision_events = []
collision_lock.release()
if len(colllision_event) > 0:
print(colllision_event)
# TODO: make sure those events are actually valid
if 'Static' in colllision_event:
collision_other = 1.0
else:
collision_other = 0.0
if "Vehicles" in colllision_event:
collision_vehicles = 1.0
else:
collision_vehicles = 0.0
if "Pedestrians" in colllision_event:
collision_pedestrians = 1.0
else:
collision_pedestrians = 0.0
#current_ms_offset = int(math.ceil((datetime.now() - self._episode_t0).total_seconds() * 1000))
# TODO: get a gametime stamp, instead of os timestamp
#current_ms_offset = int(self.carla.get_timestamp().elapsed_seconds * 1000)
#print(current_ms_offset, "ms offset")
current_ms_offset = self.last_timestamp.elapsed_seconds * 1000
second_level = namedtuple('second_level', [
'forward_speed', 'transform', 'collision_other',
'collision_pedestrians', 'collision_vehicles'
])
transform = namedtuple('transform', ['location', 'orientation'])
loc = namedtuple('loc', ['x', 'y'])
ori = namedtuple('ori', ['x', 'y', 'z'])
Meas = namedtuple('Meas',
['player_measurements', 'game_timestamp'])
v_transform = self._vehicle.get_transform()
measurements = Meas(
second_level(
self.last_estimated_speed,
transform(
loc(v_transform.location.x, v_transform.location.y),
ori(v_transform.rotation.pitch,
v_transform.rotation.roll,
v_transform.rotation.yaw)), collision_other,
collision_pedestrians, collision_vehicles),
current_ms_offset)
direction = self._current_command
self._latest_measurements = measurements
#print('[Speed = {} Km/h] [Direction = {}]'.format(measurements.player_measurements.forward_speed, direction))
#print(">>>>> planner output direction: ", direction)
return measurements, sensor_data, direction
def act(self, control):
if __CARLA_VERSION__ == '0.8.X':
self.carla.send_control(control)
else:
self._vehicle.apply_control(control)
class CarlaEnv(gym.Env):
def __init__(self, config=ENV_CONFIG):
self.config = config
if config["discrete_actions"]:
self.action_space = Discrete(10)
else:
self.action_space = Box(-1.0, 1.0, shape=(3, ))
if config["use_depth_camera"]:
self.observation_space = Box(-1.0,
1.0,
shape=(config["x_res"],
config["y_res"], 1))
else:
self.observation_space = Box(0.0,
255.0,
shape=(config["x_res"],
config["y_res"], 3))
self._spec = lambda: None
self._spec.id = "Carla-v0"
self.server_port = None
self.server_process = None
self.client = None
self.num_steps = 0
self.prev_measurement = None
def init_server(self):
print("Initializing new Carla server...")
# Create a new server process and start the client.
self.server_port = random.randint(10000, 60000)
self.server_process = subprocess.Popen([
SERVER_BINARY, "/Game/Maps/Town02", "-windowed", "-ResX=400",
"-ResY=300", "-carla-server", "-carla-world-port={}".format(
self.server_port)
],
preexec_fn=os.setsid,
stdout=open(os.devnull, "w"))
self.client = CarlaClient("localhost", self.server_port)
self.client.connect()
def clear_server_state(self):
print("Clearing Carla server state")
try:
if self.client:
self.client.disconnect()
self.client = None
except Exception as e:
print("Error disconnecting client: {}".format(e))
pass
if self.server_process:
os.killpg(os.getpgid(self.server_process.pid), signal.SIGKILL)
self.server_port = None
self.server_process = None
def __del__(self):
self.clear_server_state()
def reset(self):
error = None
for _ in range(RETRIES_ON_ERROR):
try:
if not self.server_process:
self.init_server()
return self._reset()
except Exception as e:
print("Error during reset: {}".format(traceback.format_exc()))
self.clear_server_state()
error = e
raise error
def _reset(self):
self.num_steps = 0
self.prev_measurement = None
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self.config["num_vehicles"],
NumberOfPedestrians=self.config["num_pedestrians"],
WeatherId=random.choice(self.config["weather"]))
settings.randomize_seeds()
if self.config["use_depth_camera"]:
camera = Camera("CameraDepth", PostProcessing="Depth")
camera.set_image_size(self.config["x_res"], self.config["y_res"])
camera.set_position(30, 0, 130)
settings.add_sensor(camera)
else:
camera = Camera("CameraRGB")
camera.set_image_size(self.config["x_res"], self.config["y_res"])
camera.set_position(30, 0, 130)
settings.add_sensor(camera)
scene = self.client.load_settings(settings)
# Choose one player start at random.
number_of_player_starts = len(scene.player_start_spots)
if self.config["random_starting_location"]:
player_start = random.randint(0, max(0,
number_of_player_starts - 1))
else:
player_start = 0
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print("Starting new episode...")
self.client.start_episode(player_start)
image, measurements = self._read_observation()
self.prev_measurement = measurements
return self.preprocess_image(image)
def step(self, action):
try:
obs = self._step(action)
return obs
except Exception:
print("Error during step, terminating episode early",
traceback.format_exc())
self.clear_server_state()
return np.zeros(self.observation_space.shape), 0.0, True, {}
def _step(self, action):
if self.config["discrete_actions"]:
action = int(action)
assert action in range(10)
if action == 9:
brake = 1.0
steer = 0.0
throttle = 0.0
reverse = False
else:
brake = 0.0
if action >= 6:
steer = -1.0
elif action >= 3:
steer = 1.0
else:
steer = 0.0
action %= 3
if action == 0:
throttle = 0.0
reverse = False
elif action == 1:
throttle = 1.0
reverse = False
elif action == 2:
throttle = 1.0
reverse = True
else:
assert len(action) == 3, "Invalid action {}".format(action)
steer = action[0]
throttle = min(1.0, abs(action[1]))
brake = max(0.0, min(1.0, action[2]))
reverse = action[1] < 0.0
print("steer", steer, "throttle", throttle, "brake", brake, "reverse",
reverse)
self.client.send_control(steer=steer,
throttle=throttle,
brake=brake,
hand_brake=False,
reverse=reverse)
image, measurements = self._read_observation()
reward, done = compute_reward(self.config, self.prev_measurement,
measurements)
self.prev_measurement = measurements
if self.num_steps > self.config["max_steps"]:
done = True
self.num_steps += 1
info = {}
image = self.preprocess_image(image)
return image, reward, done, info
def preprocess_image(self, image):
if self.config["use_depth_camera"]:
data = (image.data - 0.5) * 2
return data.reshape(self.config["x_res"], self.config["y_res"], 1)
else:
return image.data.reshape(self.config["x_res"],
self.config["y_res"], 3)
def _read_observation(self):
# Read the data produced by the server this frame.
measurements, sensor_data = self.client.read_data()
# Print some of the measurements.
print_measurements(measurements)
observation = None
if self.config["use_depth_camera"]:
camera_name = "CameraDepth"
else:
camera_name = "CameraRGB"
for name, image in sensor_data.items():
if name == camera_name:
observation = image
if IMAGE_OUT_PATH:
for name, image in sensor_data.items():
scipy.misc.imsave(
"{}/{}-{}.jpg".format(IMAGE_OUT_PATH, name,
self.num_steps), image.data)
assert observation is not None, sensor_data
return observation, measurements
class CarlaEnv(gym.Env):
def __init__(self, config=ENV_CONFIG):
self.config = config
self.city = self.config["server_map"].split("/")[-1]
if self.config["enable_planner"]:
self.planner = Planner(self.city)
self.action_space = Discrete(len(DISCRETE_ACTIONS))
# RGB Camera
self.frame_shape = (config["y_res"], config["x_res"])
image_space = Box(
0, 1, shape=(
config["y_res"], config["x_res"],
FRAME_DEPTH * config["framestack"]), dtype=np.float32)
self.observation_space = image_space
# TODO(ekl) this isn't really a proper gym spec
self._spec = lambda: None
self._spec.id = "Carla-v0"
self.server_port = None
self.server_process = None
self.client = None
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.prev_image = None
self.episode_id = None
self.measurements_file = None
self.weather = None
self.scenario = None
self.start_pos = None
self.end_pos = None
self.start_coord = None
self.end_coord = None
self.last_obs = None
self.last_full_obs = None
self.framestack = [None] * config["framestack"]
self.framestack_index = 0
self.running_restarts = 0
self.on_step = None
self.on_next = None
self.photo_index = 1
self.episode_index = 0
def init_server(self):
print("Initializing new Carla server...")
# Create a new server process and start the client.
self.server_port = random.randint(10000, 60000)
self.server_process = subprocess.Popen(
[self.config["server_binary"], self.config["server_map"],
"-windowed",
"-ResX={}".format(self.config["render_x_res"]),
"-ResY={}".format(self.config["render_y_res"]),
"-fps={}".format(self.config["fps"]),
"-carla-server",
"-carla-world-port={}".format(self.server_port)],
preexec_fn=os.setsid, stdout=open(os.devnull, "w"))
live_carla_processes.add(os.getpgid(self.server_process.pid))
for i in range(RETRIES_ON_ERROR):
try:
self.client = CarlaClient("localhost", self.server_port)
return self.client.connect()
except Exception as e:
print("Error connecting: {}, attempt {}".format(e, i))
time.sleep(2)
def clear_server_state(self):
print("Clearing Carla server state")
try:
if self.client:
self.client.disconnect()
self.client = None
except Exception as e:
print("Error disconnecting client: {}".format(e))
pass
if self.server_process:
pgid = os.getpgid(self.server_process.pid)
os.killpg(pgid, signal.SIGKILL)
live_carla_processes.remove(pgid)
self.server_port = None
self.server_process = None
def __del__(self):
self.clear_server_state()
def reset(self):
if self.on_next is not None:
self.on_next()
error = None
self.running_restarts += 1
for _ in range(RETRIES_ON_ERROR):
try:
# Force a full reset of Carla after some number of restarts
if self.running_restarts > self.config["server_restart_interval"]:
print("Shutting down carla server...")
self.running_restarts = 0
self.clear_server_state()
# If server down, initialize
if not self.server_process:
self.init_server()
# Run episode reset
return self._reset()
except Exception as e:
print("Error during reset: {}".format(traceback.format_exc()))
self.clear_server_state()
error = e
time.sleep(5)
raise error
def _build_camera(self, name, post):
camera = Camera(name, PostProcessing=post)
camera.set_image_size(
self.config["render_x_res"], self.config["render_y_res"])
camera.set_position(x=2.4, y=0, z=0.8)
camera.set_rotation(pitch=-40, roll=0, yaw=0)
# camera.FOV = 100
return camera
def _reset(self):
self.num_steps = 0
self.total_reward = 0
self.episode_index += 1
self.photo_index = 1
self.prev_measurement = None
self.prev_image = None
self.episode_id = datetime.today().strftime("%Y-%m-%d_%H-%M-%S_%f")
self.measurements_file = None
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
self.scenario = random.choice(self.config["scenarios"])
assert self.scenario["city"] == self.city, (self.scenario, self.city)
self.weather = random.choice(self.scenario["weather_distribution"])
settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=self.scenario["num_vehicles"],
NumberOfPedestrians=self.scenario["num_pedestrians"],
WeatherId=self.weather,
QualityLevel=self.config["quality"])
settings.randomize_seeds()
# Add the cameras
if self.config["save_images_rgb"]:
settings.add_sensor(self._build_camera(name="CameraRGB", post="SceneFinal"))
settings.add_sensor(self._build_camera(name="CameraDepth", post="Depth"))
settings.add_sensor(self._build_camera(name="CameraClass", post="SemanticSegmentation"))
# Setup start and end positions
scene = self.client.load_settings(settings)
positions = scene.player_start_spots
self.start_pos = positions[self.scenario["start_pos_id"]]
self.end_pos = positions[self.scenario["end_pos_id"]]
self.start_coord = [
self.start_pos.location.x // 100, self.start_pos.location.y // 100]
self.end_coord = [
self.end_pos.location.x // 100, self.end_pos.location.y // 100]
print(
"Start pos {} ({}), end {} ({})".format(
self.scenario["start_pos_id"], self.start_coord,
self.scenario["end_pos_id"], self.end_coord))
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print("Starting new episode...")
self.client.start_episode(self.scenario["start_pos_id"])
image, py_measurements = self._read_observation()
py_measurements["control"] = {
"throttle_brake": 0,
"steer": 0,
"throttle": 0,
"brake": 0,
"reverse": False,
"hand_brake": False,
}
self.prev_measurement = py_measurements
return self.encode_obs(self.preprocess_image(image), py_measurements)
def encode_obs(self, obs, py_measurements):
new_obs = obs
num_frames = int(self.config["framestack"])
if num_frames > 1:
# Spread out to number of frames
frame_array = [obs] * num_frames
for frame_index in range(1, num_frames):
index = (self.framestack_index - frame_index) % num_frames
if self.framestack[index] is not None:
frame_array[frame_index] = self.framestack[index]
# Concatenate into a single np array
new_obs = np.concatenate(frame_array, axis=2)
# Store frame
self.framestack[self.framestack_index] = obs
self.framestack_index = (self.framestack_index + 1) % num_frames
self.last_obs = obs
# Return
return new_obs
def step(self, action):
try:
obs = self._step(action)
self.last_full_obs = obs
return obs
except Exception:
print(
"Error during step, terminating episode early",
traceback.format_exc())
self.clear_server_state()
return (self.last_full_obs, 0.0, True, {})
def _step(self, action):
action = DISCRETE_ACTIONS[int(action)]
assert len(action) == 2, "Invalid action {}".format(action)
if self.config["squash_action_logits"]:
forward = 2 * float(sigmoid(action[0]) - 0.5)
throttle = float(np.clip(forward, 0, 1))
brake = float(np.abs(np.clip(forward, -1, 0)))
steer = 2 * float(sigmoid(action[1]) - 0.5)
else:
throttle = float(np.clip(action[0], 0, 1))
brake = float(np.abs(np.clip(action[0], -1, 0)))
steer = float(np.clip(action[1], -1, 1))
reverse = False
hand_brake = False
if self.config["verbose"]:
print(
"steer", steer, "throttle", throttle, "brake", brake,
"reverse", reverse)
self.client.send_control(
steer=steer, throttle=throttle, brake=brake, hand_brake=hand_brake,
reverse=reverse)
# Process observations
image, py_measurements = self._read_observation()
if self.config["verbose"]:
print("Next command", py_measurements["next_command"])
if type(action) is np.ndarray:
py_measurements["action"] = [float(a) for a in action]
else:
py_measurements["action"] = action
py_measurements["control"] = {
"throttle_brake": action[0],
"steer": steer,
"throttle": throttle,
"brake": brake,
"reverse": reverse,
"hand_brake": hand_brake,
}
# Done?
finished = self.num_steps > self.scenario["max_steps"]
# py_measurements["next_command"] == "REACH_GOAL"
failed = TERM.compute_termination(self, py_measurements, self.prev_measurement)
done = finished or failed
py_measurements["finished"] = finished
py_measurements["failed"] = failed
py_measurements["done"] = done
# Determine Reward
reward = compute_reward(self, self.prev_measurement, py_measurements)
self.total_reward += reward
py_measurements["reward"] = reward
py_measurements["total_reward"] = self.total_reward
self.prev_measurement = py_measurements
# Callback
if self.on_step is not None:
self.on_step(py_measurements)
# Write out measurements to file
if self.config["carla_out_path"]:
# Ensure measurements dir exists
measurements_dir = os.path.join(self.config["carla_out_path"], self.config["measurements_subdir"])
if not os.path.exists(measurements_dir):
os.mkdir(measurements_dir)
if not self.measurements_file:
self.measurements_file = open(
os.path.join(
measurements_dir,
"m_{}.json".format(self.episode_id)),
"w")
self.measurements_file.write(json.dumps(py_measurements))
self.measurements_file.write("\n")
if done:
self.measurements_file.close()
self.measurements_file = None
if self.config["convert_images_to_video"]:
self.images_to_video(camera_name="RGB")
self.images_to_video(camera_name="Depth")
self.images_to_video(camera_name="Class")
self.num_steps += 1
image = self.preprocess_image(image)
return (
self.encode_obs(image, py_measurements), reward, done,
py_measurements)
def preprocess_image(self, image):
return image
def _fuse_observations(self, depth, clazz, speed):
base_shape = (self.config["render_y_res"], self.config["render_x_res"])
new_shape = (self.config["y_res"], self.config["x_res"])
# Reduce class
clazz = reduce_classifications(clazz)
# Do we need to resize?
if base_shape[0] is not new_shape[0] and base_shape[1] is not new_shape[1]:
depth_reshape = depth.reshape(*depth.shape)
depth = cv2.resize(depth_reshape, (new_shape[1], new_shape[0]))
clazz = resize_classifications(clazz, new_shape)
# Fuse with depth!
obs = fuse_with_depth(clazz, depth, extra_layers=1)
# Fuse with speed!
obs[:, :, KEEP_CLASSIFICATIONS] = speed
return obs, clazz
def _read_observation(self):
# Read the data produced by the server this frame.
measurements, sensor_data = self.client.read_data()
cur = measurements.player_measurements
# Print some of the measurements.
if self.config["verbose"]:
print_measurements(measurements)
# Fuse the observation data to create a single observation
observation, clazzes = self._fuse_observations(
sensor_data['CameraDepth'].data,
sensor_data['CameraClass'].data,
cur.forward_speed)
if self.config["enable_planner"]:
next_command = COMMANDS_ENUM[
self.planner.get_next_command(
[cur.transform.location.x, cur.transform.location.y,
GROUND_Z],
[cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z],
[self.end_pos.location.x, self.end_pos.location.y,
GROUND_Z],
[self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z])
]
else:
next_command = "LANE_FOLLOW"
if next_command == "REACH_GOAL":
distance_to_goal = 0.0 # avoids crash in planner
elif self.config["enable_planner"]:
distance_to_goal = self.planner.get_shortest_path_distance(
[cur.transform.location.x, cur.transform.location.y, GROUND_Z],
[cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z],
[self.end_pos.location.x, self.end_pos.location.y, GROUND_Z],
[self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z]) / 100
else:
distance_to_goal = -1
distance_to_goal_euclidean = float(np.linalg.norm(
[cur.transform.location.x - self.end_pos.location.x,
cur.transform.location.y - self.end_pos.location.y]) / 100)
py_measurements = {
"episode_id": self.episode_id,
"step": self.num_steps,
"x": cur.transform.location.x,
"y": cur.transform.location.y,
"x_orient": cur.transform.orientation.x,
"y_orient": cur.transform.orientation.y,
"forward_speed": cur.forward_speed,
"distance_to_goal": distance_to_goal,
"distance_to_goal_euclidean": distance_to_goal_euclidean,
"collision_vehicles": cur.collision_vehicles,
"collision_pedestrians": cur.collision_pedestrians,
"collision_other": cur.collision_other,
"intersection_offroad": cur.intersection_offroad,
"intersection_otherlane": cur.intersection_otherlane,
"weather": self.weather,
"map": self.config["server_map"],
"start_coord": self.start_coord,
"end_coord": self.end_coord,
"current_scenario": self.scenario,
"x_res": self.config["x_res"],
"y_res": self.config["y_res"],
"num_vehicles": self.scenario["num_vehicles"],
"num_pedestrians": self.scenario["num_pedestrians"],
"max_steps": self.scenario["max_steps"],
"next_command": next_command,
"applied_penalty": False,
"total_reward": self.total_reward,
}
if self.config["carla_out_path"] \
and self.num_steps % self.config["save_image_frequency"] == 0\
and self.num_steps > 15:
self.take_photo(
sensor_data=sensor_data,
class_data=clazzes,
fused_data=observation
)
assert observation is not None, sensor_data
return observation, py_measurements
def images_dir(self):
dir = os.path.join(self.config["carla_out_path"], "images")
if not os.path.exists(dir):
os.mkdir(dir)
return dir
def episode_dir(self):
episode_dir = os.path.join(self.images_dir(), "episode_" + str(self.episode_index))
if not os.path.exists(episode_dir):
os.mkdir(episode_dir)
return episode_dir
def take_photo(self, sensor_data, class_data, fused_data):
# Get the proper locations\
episode_dir = self.episode_dir()
photo_index = self.photo_index
name_prefix = "episode_{:>04}_step_{:>04}".format(self.episode_index, photo_index)
self.photo_index += 1
# Save the image
if self.config["save_images_rgb"]:
rgb_image = sensor_data["CameraRGB"]
image_path = os.path.join(episode_dir, name_prefix + "_rgb.png")
scipy.misc.imsave(image_path, rgb_image.data)
# Save the classes
if self.config["save_images_class"]:
classes_path = os.path.join(episode_dir, name_prefix + "_class.png")
scipy.misc.imsave(classes_path, class_data.data)
# Save the class images
if self.config["save_images_fusion"]:
fused_images = fused_to_rgb(fused_data)
fused_path = os.path.join(episode_dir, name_prefix + "_fused.png")
scipy.misc.imsave(fused_path, fused_images.data)
def images_to_video(self, camera_name):
# Video directory
videos_dir = os.path.join(self.config["carla_out_path"], "Videos" + camera_name)
if not os.path.exists(videos_dir):
os.makedirs(videos_dir)
# Build command
video_fps = self.config["fps"] / self.config["saveimage_frequency"]
ffmpeg_cmd = (
"ffmpeg -loglevel -8 -r 10 -f image2 -s {x_res}x{y_res} "
"-start_number 0 -i "
"{img}_%04d.jpg -vcodec libx264 {vid}.mp4 && rm -f {img}_*.jpg "
).format(
x_res=self.config["render_x_res"],
y_res=self.config["render_y_res"],
vid=os.path.join(videos_dir, self.episode_id),
img=os.path.join(self.config["carla_out_path"], "Camera" + camera_name, self.episode_id))
# Execute command
print("Executing ffmpeg command: ", ffmpeg_cmd)
try:
subprocess.call(ffmpeg_cmd, shell=True, timeout=60)
except Exception as ex:
print("FFMPEG EXPIRED")
print(ex)
class CarlaOperator(Op):
"""Provides an ERDOS interface to the CARLA simulator.
Args:
synchronous_mode (bool): whether the simulator will wait for control
input from the client.
"""
def __init__(self,
name,
flags,
camera_setups=[],
lidar_stream_names=[],
log_file_name=None,
csv_file_name=None):
super(CarlaOperator, self).__init__(name)
self._flags = flags
self._logger = setup_logging(self.name, log_file_name)
self._csv_logger = setup_csv_logging(self.name + '-csv', csv_file_name)
self.message_num = 0
if self._flags.carla_high_quality:
quality = 'Epic'
else:
quality = 'Low'
self.settings = CarlaSettings()
self.settings.set(
SynchronousMode=self._flags.carla_synchronous_mode,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self._flags.carla_num_vehicles,
NumberOfPedestrians=self._flags.carla_num_pedestrians,
WeatherId=self._flags.carla_weather,
QualityLevel=quality)
self.settings.randomize_seeds()
self.lidar_streams = []
for (camera_stream_name, camera_type, image_size,
pos) in camera_setups:
self.__add_camera(name=camera_stream_name,
postprocessing=camera_type,
image_size=image_size,
position=pos)
for lidar_stream_name in lidar_stream_names:
self.__add_lidar(name=lidar_stream_name)
self.agent_id_map = {}
self.pedestrian_count = 0
@staticmethod
def setup_streams(input_streams, camera_setups, lidar_stream_names):
input_streams.add_callback(CarlaOperator.update_control)
camera_streams = [
DataStream(name=camera,
labels={
'sensor_type': 'camera',
'camera_type': camera_type
}) for (camera, camera_type, _, _) in camera_setups
]
lidar_streams = [
DataStream(name=lidar, labels={'sensor_type': 'lidar'})
for lidar in lidar_stream_names
]
return [
DataStream(name='world_transform'),
DataStream(name='vehicle_pos'),
DataStream(name='acceleration'),
DataStream(data_type=Float64, name='forward_speed'),
DataStream(data_type=Float64, name='vehicle_collisions'),
DataStream(data_type=Float64, name='pedestrian_collisions'),
DataStream(data_type=Float64, name='other_collisions'),
DataStream(data_type=Float64, name='other_lane'),
DataStream(data_type=Float64, name='offroad'),
DataStream(name='traffic_lights'),
DataStream(name='pedestrians'),
DataStream(name='vehicles'),
DataStream(name='traffic_signs'),
] + camera_streams + lidar_streams
def __add_camera(self,
name,
postprocessing,
image_size=(800, 600),
field_of_view=90.0,
position=(0.3, 0, 1.3),
rotation_pitch=0,
rotation_roll=0,
rotation_yaw=0):
"""Adds a camera and a corresponding output stream.
Args:
name: A string naming the camera.
postprocessing: "SceneFinal", "Depth", "SemanticSegmentation".
"""
camera = Camera(name,
PostProcessing=postprocessing,
FOV=field_of_view,
ImageSizeX=image_size[0],
ImageSizeY=image_size[1],
PositionX=position[0],
PositionY=position[1],
PositionZ=position[2],
RotationPitch=rotation_pitch,
RotationRoll=rotation_roll,
RotationYaw=rotation_yaw)
self.settings.add_sensor(camera)
def __add_lidar(self,
name,
channels=32,
max_range=50,
points_per_second=100000,
rotation_frequency=10,
upper_fov_limit=10,
lower_fov_limit=-30,
position=(0, 0, 1.4),
rotation_pitch=0,
rotation_yaw=0,
rotation_roll=0):
"""Adds a LIDAR sensor and a corresponding output stream.
Args:
name: A string naming the camera.
"""
lidar = Lidar(name,
Channels=channels,
Range=max_range,
PointsPerSecond=points_per_second,
RotationFrequency=rotation_frequency,
UpperFovLimit=upper_fov_limit,
LowerFovLimit=lower_fov_limit,
PositionX=position[0],
PositionY=position[1],
PositionZ=position[2],
RotationPitch=rotation_pitch,
RotationYaw=rotation_yaw,
RotationRoll=rotation_roll)
self.settings.add_sensor(lidar)
output_stream = DataStream(name=name, labels={"sensor_type": "lidar"})
self.lidar_streams.append(output_stream)
def read_carla_data(self):
read_start_time = time.time()
measurements, sensor_data = self.client.read_data()
measure_time = time.time()
self._logger.info(
'Got readings for game time {} and platform time {}'.format(
measurements.game_timestamp, measurements.platform_timestamp))
# Send measurements
player_measurements = measurements.player_measurements
vehicle_pos = ((player_measurements.transform.location.x,
player_measurements.transform.location.y,
player_measurements.transform.location.z),
(player_measurements.transform.orientation.x,
player_measurements.transform.orientation.y,
player_measurements.transform.orientation.z))
world_transform = Transform(player_measurements.transform)
timestamp = Timestamp(
coordinates=[measurements.game_timestamp, self.message_num])
self.message_num += 1
ray.register_custom_serializer(Message, use_pickle=True)
ray.register_custom_serializer(WatermarkMessage, use_pickle=True)
watermark = WatermarkMessage(timestamp)
self.get_output_stream('world_transform').send(
Message(world_transform, timestamp))
self.get_output_stream('world_transform').send(watermark)
self.get_output_stream('vehicle_pos').send(
Message(vehicle_pos, timestamp))
self.get_output_stream('vehicle_pos').send(watermark)
acceleration = (player_measurements.acceleration.x,
player_measurements.acceleration.y,
player_measurements.acceleration.z)
self.get_output_stream('acceleration').send(
Message(acceleration, timestamp))
self.get_output_stream('acceleration').send(watermark)
self.get_output_stream('forward_speed').send(
Message(player_measurements.forward_speed, timestamp))
self.get_output_stream('forward_speed').send(watermark)
self.get_output_stream('vehicle_collisions').send(
Message(player_measurements.collision_vehicles, timestamp))
self.get_output_stream('vehicle_collisions').send(watermark)
self.get_output_stream('pedestrian_collisions').send(
Message(player_measurements.collision_pedestrians, timestamp))
self.get_output_stream('pedestrian_collisions').send(watermark)
self.get_output_stream('other_collisions').send(
Message(player_measurements.collision_other, timestamp))
self.get_output_stream('other_collisions').send(watermark)
self.get_output_stream('other_lane').send(
Message(player_measurements.intersection_otherlane, timestamp))
self.get_output_stream('other_lane').send(watermark)
self.get_output_stream('offroad').send(
Message(player_measurements.intersection_offroad, timestamp))
self.get_output_stream('offroad').send(watermark)
vehicles = []
pedestrians = []
traffic_lights = []
speed_limit_signs = []
for agent in measurements.non_player_agents:
if agent.HasField('vehicle'):
pos = messages.Transform(agent.vehicle.transform)
bb = messages.BoundingBox(agent.vehicle.bounding_box)
forward_speed = agent.vehicle.forward_speed
vehicle = messages.Vehicle(pos, bb, forward_speed)
vehicles.append(vehicle)
elif agent.HasField('pedestrian'):
if not self.agent_id_map.get(agent.id):
self.pedestrian_count += 1
self.agent_id_map[agent.id] = self.pedestrian_count
pedestrian_index = self.agent_id_map[agent.id]
pos = messages.Transform(agent.pedestrian.transform)
bb = messages.BoundingBox(agent.pedestrian.bounding_box)
forward_speed = agent.pedestrian.forward_speed
pedestrian = messages.Pedestrian(pedestrian_index, pos, bb,
forward_speed)
pedestrians.append(pedestrian)
elif agent.HasField('traffic_light'):
transform = messages.Transform(agent.traffic_light.transform)
traffic_light = messages.TrafficLight(
transform, agent.traffic_light.state)
traffic_lights.append(traffic_light)
elif agent.HasField('speed_limit_sign'):
transform = messages.Transform(
agent.speed_limit_sign.transform)
speed_sign = messages.SpeedLimitSign(
transform, agent.speed_limit_sign.speed_limit)
speed_limit_signs.append(speed_sign)
vehicles_msg = Message(vehicles, timestamp)
self.get_output_stream('vehicles').send(vehicles_msg)
self.get_output_stream('vehicles').send(watermark)
pedestrians_msg = Message(pedestrians, timestamp)
self.get_output_stream('pedestrians').send(pedestrians_msg)
self.get_output_stream('pedestrians').send(watermark)
traffic_lights_msg = Message(traffic_lights, timestamp)
self.get_output_stream('traffic_lights').send(traffic_lights_msg)
self.get_output_stream('traffic_lights').send(watermark)
traffic_sings_msg = Message(speed_limit_signs, timestamp)
self.get_output_stream('traffic_signs').send(traffic_sings_msg)
self.get_output_stream('traffic_signs').send(watermark)
# Send sensor data
for name, measurement in sensor_data.items():
data_stream = self.get_output_stream(name)
if data_stream.get_label('camera_type') == 'SceneFinal':
# Transform the Carla RGB images to BGR.
data_stream.send(
Message(
pylot_utils.bgra_to_bgr(to_bgra_array(measurement)),
timestamp))
else:
data_stream.send(Message(measurement, timestamp))
data_stream.send(watermark)
self.client.send_control(**self.control)
end_time = time.time()
measurement_runtime = (measure_time - read_start_time) * 1000
total_runtime = (end_time - read_start_time) * 1000
self._logger.info('Carla measurement time {}; total time {}'.format(
measurement_runtime, total_runtime))
self._csv_logger.info('{},{},{},{}'.format(time_epoch_ms(), self.name,
measurement_runtime,
total_runtime))
def read_data_at_frequency(self):
period = 1.0 / self._flags.carla_step_frequency
trigger_at = time.time() + period
while True:
time_until_trigger = trigger_at - time.time()
if time_until_trigger > 0:
time.sleep(time_until_trigger)
else:
self._logger.error(
'Cannot read Carla data at frequency {}'.format(
self._flags.carla_step_frequency))
self.read_carla_data()
trigger_at += period
def update_control(self, msg):
"""Updates the control dict"""
self.control.update(msg.data)
def execute(self):
# Subscribe to control streams
self.control = {
'steer': 0.0,
'throttle': 0.0,
'break': 0.0,
'hand_break': False,
'reverse': False
}
self.client = CarlaClient(self._flags.carla_host,
self._flags.carla_port,
timeout=10)
self.client.connect()
scene = self.client.load_settings(self.settings)
# Choose one player start at random.
number_of_player_starts = len(scene.player_start_spots)
player_start = self._flags.carla_start_player_num
if self._flags.carla_random_player_start:
player_start = np.random.randint(
0, max(0, number_of_player_starts - 1))
self.client.start_episode(player_start)
self.read_data_at_frequency()
self.spin()
class CarlaEnvironmentWrapper(EnvironmentWrapper):
def __init__(self,
num_speedup_steps=30,
require_explicit_reset=True,
is_render_enabled=False,
early_termination_enabled=False,
run_offscreen=False,
save_screens=False,
port=2000,
gpu=0,
discrete_control=True,
kill_when_connection_lost=True,
city_name="Town01",
channel_last=True):
EnvironmentWrapper.__init__(self, is_render_enabled, save_screens)
print("port:", port)
self.episode_max_time = 1000000
self.allow_braking = True
self.log_path = os.path.join(DEFAULT_CARLA_LOG_DIR, "CarlaLogs.txt")
self.num_speedup_steps = num_speedup_steps
self.is_game_ready_for_input = False
self.run_offscreen = run_offscreen
self.kill_when_connection_lost = kill_when_connection_lost
# server configuration
self.port = port
self.gpu = gpu
self.host = 'localhost'
self.level = 'town1'
self.map = CarlaLevel().get(self.level)
# experiment = basic_experiment_suite.BasicExperimentSuite(city_name)
experiment = CoRL2017(city_name)
self.experiments = experiment.get_experiments()
self.experiment_type = 0
self.planner = Planner(city_name)
self.car_speed = 0
self.is_game_setup = False # Will be true only when setup_client_and_server() is called, either explicitly, or by reset()
# action space
self.discrete_controls = discrete_control
self.action_space_size = 3
self.action_space_high = np.array([1, 1, 1])
self.action_space_low = np.array([-1, -1, -1])
self.action_space_abs_range = np.maximum(
np.abs(self.action_space_low), np.abs(self.action_space_high))
self.steering_strength = 0.35
self.gas_strength = 1.0
self.brake_strength = 0.6
self.actions = {
0: [0., 0.],
1: [0., -self.steering_strength],
2: [0., self.steering_strength],
3: [self.gas_strength - 0.15, 0.],
4: [-self.brake_strength, 0],
5: [self.gas_strength - 0.3, -self.steering_strength],
6: [self.gas_strength - 0.3, self.steering_strength],
7: [-self.brake_strength, -self.steering_strength],
8: [-self.brake_strength, self.steering_strength]
}
self.actions_description = [
'NO-OP', 'TURN_LEFT', 'TURN_RIGHT', 'GAS', 'BRAKE',
'GAS_AND_TURN_LEFT', 'GAS_AND_TURN_RIGHT', 'BRAKE_AND_TURN_LEFT',
'BRAKE_AND_TURN_RIGHT'
]
if discrete_control:
self.action_space = Discrete(len(self.actions))
else:
self.action_space = Box(low=self.action_space_low,
high=self.action_space_high)
self.observation_space = Box(low=-np.inf,
high=np.inf,
shape=[88, 200, 3])
# measurements
self.measurements_size = (1, )
self.pre_image = None
self.first_debug = True
self.channel_last = channel_last
def setup_client_and_server(self, reconnect_client_only=False):
# open the server
if not reconnect_client_only:
self.server, self.out = self._open_server()
self.server_pid = self.server.pid # To kill incase child process gets lost
print("setup server, out:", self.server_pid, "dockerid", self.out)
while True:
try:
self.game = CarlaClient(self.host, self.port, timeout=99999999)
self.game.connect(
connection_attempts=100
) # It's taking a very long time for the server process to spawn, so the client needs to wait or try sufficient no. of times lol
self.game.load_settings(CarlaSettings())
self.game.start_episode(0)
self.is_game_setup = self.server and self.game
print("setup client")
# self.out = shell("docker ps -q")
# print("dockerid", self.out)
return
except TCPConnectionError as error:
print(error)
time.sleep(1)
def close_client_and_server(self):
self._close_server()
print("Disconnecting the client")
self.game.disconnect()
self.game = None
self.server = None
self.is_game_setup = False
return
def _open_server(self):
# Note: There is no way to disable rendering in CARLA as of now
# https://github.com/carla-simulator/carla/issues/286
# decrease the window resolution if you want to see if performance increases
# Command: $CARLA_ROOT/CarlaUE4.sh /Game/Maps/Town02 -benchmark -carla-server -fps=15 -world-port=9876 -windowed -ResX=480 -ResY=360 -carla-no-hud
# To run off_screen: SDL_VIDEODRIVER=offscreen SDL_HINT_CUDA_DEVICE=0 <command> #https://github.com/carla-simulator/carla/issues/225
my_env = None
if self.run_offscreen:
my_env = {
**os.environ,
'SDL_VIDEODRIVER': 'offscreen',
'SDL_HINT_CUDA_DEVICE': '0',
}
with open(self.log_path, "wb") as out:
#docker <19.03
# p = subprocess.Popen(['docker', 'run', '--rm', '-d', '-p',
# str(self.port) + '-' + str(self.port + 2) + ':' + str(self.port) + '-' + str(self.port + 2),
# '--runtime=nvidia', '-e', 'NVIDIA_VISIBLE_DEVICES='+str(self.gpu), "carlasim/carla:0.8.4",
# '/bin/bash', 'CarlaUE4.sh', self.map, '-windowed',
# '-benchmark', '-fps=10', '-world-port=' + str(self.port)], shell=False,
# stdout=subprocess.PIPE)
#docker=19.03
p = subprocess.Popen([
'docker', 'run', '--rm', '-d', '-p',
str(self.port) + '-' + str(self.port + 2) + ':' +
str(self.port) + '-' + str(self.port + 2),
'--gpus=' + str(self.gpu), "carlasim/carla:0.8.4", '/bin/bash',
'CarlaUE4.sh', self.map, '-windowed', '-benchmark', '-fps=10',
'-world-port=' + str(self.port)
],
shell=False,
stdout=subprocess.PIPE)
# docker_id = shell("docker ps -q")
docker_out, err = p.communicate()
# cmd = [path.join(environ.get('CARLA_ROOT'), 'CarlaUE4.sh'), self.map,
# "-benchmark", "-carla-server", "-fps=10", "-world-port={}".format(self.port),
# "-windowed -ResX={} -ResY={}".format(carla_config.server_width, carla_config.server_height),
# "-carla-no-hud"]
# p = subprocess.Popen(cmd, stdout=out, stderr=out, env=my_env, preexec_fn=os.setsid)
# p = subprocess.Popen(cmd, env=my_env, preexec_fn=os.setsid)
return p, docker_out
def _close_server(self):
if self.kill_when_connection_lost:
print("kill before")
# os.kill(os.getpgid(self.server.pid), signal.SIGKILL)
# self.server.kill()
print(self.out)
subprocess.call(['docker', 'stop', self.out[:-1]])
while True:
cmd = shell("docker ps -q")
if cmd == "":
break
print("kill after")
return
# no_of_attempts = 0
# while is_process_alive(self.server_pid):
# try:
# self.server.terminate()
# self.server.wait()
# os.killpg(self.server.pid, signal.SIGTERM)
# time.sleep(10)
# except Exception as error:
# print(error)
def _get_directions(self, current_point, end_point):
"""
Class that should return the directions to reach a certain goal
"""
directions = self.planner.get_next_command(
(current_point.location.x, current_point.location.y, 0.22),
(current_point.orientation.x, current_point.orientation.y,
current_point.orientation.z),
(end_point.location.x, end_point.location.y, 0.22),
(end_point.orientation.x, end_point.orientation.y,
end_point.orientation.z))
return directions
def _update_state(self):
# get measurements and observations
measurements = []
while type(measurements) == list:
try:
measurements, sensor_data = self.game.read_data()
except:
print(
"Connection to server lost while reading state. Reconnecting..........."
)
# self.game.disconnect()
# self.setup_client_and_server(reconnect_client_only=True)
import psutil
info = psutil.virtual_memory()
print("memory used", str(info.percent))
self.close_client_and_server()
self.setup_client_and_server(reconnect_client_only=False)
scene = self.game.load_settings(self.cur_experiment.conditions)
self.positions = scene.player_start_spots
self.start_point = self.positions[self.pose[0]]
self.end_point = self.positions[self.pose[1]]
self.game.start_episode(self.pose[0])
self.done = True
current_point = measurements.player_measurements.transform
direction = self._get_directions(current_point, self.end_point)
speed = measurements.player_measurements.forward_speed
self.reward = 0
dist = sldist([current_point.location.x, current_point.location.y],
[self.end_point.location.x, self.end_point.location.y])
if direction == 5: #go straight
if abs(self.control.steer) > 0.2:
self.reward -= 20
self.reward += min(35, speed * 3.6)
elif direction == 2: #follow lane
self.reward += min(35, speed * 3.6)
elif direction == 3: #turn left ,steer negtive
if self.control.steer > 0:
self.reward -= 15
if speed * 3.6 <= 20:
self.reward += speed * 3.6
else:
self.reward += 40 - speed * 3.6
elif direction == 4: #turn right
if self.control.steer < 0:
self.reward -= 15
if speed * 3.6 <= 20:
self.reward += speed * 3.6
else:
self.reward += 40 - speed * 3.6
elif direction == 0:
self.done = True
self.reward += 100
direction = 2
print("success", dist)
else:
print("error direction")
direction = 5
direction -= 2
direction = int(direction)
intersection_offroad = measurements.player_measurements.intersection_offroad
intersection_otherlane = measurements.player_measurements.intersection_otherlane
collision_veh = measurements.player_measurements.collision_vehicles
collision_ped = measurements.player_measurements.collision_pedestrians
collision_other = measurements.player_measurements.collision_other
if intersection_otherlane > 0 or intersection_offroad > 0 or collision_ped > 0 or collision_veh > 0:
self.reward -= 100
elif collision_other > 0:
self.reward -= 50
if collision_other > 0 or collision_veh > 0 or collision_ped > 0:
self.done = True
if intersection_offroad > 0.2 or intersection_otherlane > 0.2:
self.done = True
if speed * 3.6 <= 1:
self.nospeed_time += 1
if self.nospeed_time > 100:
self.done = True
self.reward -= 1
else:
self.nospeed_time = 0
# speed = min(1, speed/10.0)
speed = speed / 25
# print(measurements)
# print(sensor_data)
self.observation = {
"img": self.process_image(sensor_data['CameraRGB'].data),
"speed": speed,
"direction": direction
}
return self.observation, self.reward, self.done
def encode_obs(self, image):
if self.pre_image is None:
self.pre_image = image
img = np.concatenate([self.pre_image, image], axis=2)
self.pre_image = image
return img
def process_image(self, data):
rgb_img = data[115:510, :]
rgb_img = cv2.resize(rgb_img, (200, 88), interpolation=cv2.INTER_AREA)
if not self.channel_last:
rgb_img = np.transpose(rgb_img, (2, 0, 1))
# return cv2.resize(
# data, (80, 80),
# interpolation=cv2.INTER_AREA)
return rgb_img
def _take_action(self, action_idx):
if not self.is_game_setup:
print("Reset the environment duh by reset() before calling step()")
sys.exit(1)
if type(action_idx) == np.int64 or type(action_idx) == np.int:
action = self.actions[action_idx]
else:
action = action_idx
self.control = VehicleControl()
if len(action) == 3:
self.control.throttle = np.clip(action[1], 0, 1)
self.control.steer = np.clip(action[0], -1, 1)
self.control.brake = np.clip(action[2], 0, 1)
else:
self.control.throttle = np.clip(action[0], 0, 1)
self.control.steer = np.clip(action[1], -1, 1)
self.control.brake = np.abs(np.clip(action[0], -1, 0))
if not self.allow_braking:
self.control.brake = 0
self.control.hand_brake = False
self.control.reverse = False
controls_sent = False
while not controls_sent:
try:
self.game.send_control(self.control)
controls_sent = True
# print(self.control)
# #
# rand = random.randint(0, 4)
# if rand == 0 and self.first_debug:
# self.first_debug = False
# raise Exception
except:
print(
"Connection to server lost while sending controls. Reconnecting..........."
)
import psutil
info = psutil.virtual_memory()
print("memory used", str(info.percent))
self.close_client_and_server()
self.setup_client_and_server(reconnect_client_only=False)
scene = self.game.load_settings(self.cur_experiment.conditions)
self.positions = scene.player_start_spots
self.start_point = self.positions[self.pose[0]]
self.end_point = self.positions[self.pose[1]]
self.game.start_episode(self.pose[0])
self.done = True
controls_sent = False
return
def _restart_environment_episode(self, force_environment_reset=True):
if not force_environment_reset and not self.done and self.is_game_setup:
print("Can't reset dude, episode ain't over yet")
return None # User should handle this
self.is_game_ready_for_input = False
if not self.is_game_setup:
self.setup_client_and_server()
experiment_type = random.randint(0, 0)
self.cur_experiment = self.experiments[experiment_type]
self.pose = random.choice(self.cur_experiment.poses)
# self.cur_experiment = self.experiments[0]
# self.pose = self.cur_experiment.poses[0]
scene = self.game.load_settings(self.cur_experiment.conditions)
self.positions = scene.player_start_spots
self.start_point = self.positions[self.pose[0]]
self.end_point = self.positions[self.pose[1]]
# print("start episode")
while True:
try:
self.game.start_episode(self.pose[0])
# start the game with some initial speed
self.car_speed = 0
self.nospeed_time = 0
observation = None
for i in range(self.num_speedup_steps):
observation, reward, done, _ = self.step([0, 1.0, 0])
self.observation = observation
self.is_game_ready_for_input = True
break
except Exception as error:
print(error)
self.game.connect()
self.game.start_episode(self.pose[0])
return observation
class CarlaClientConsole(cmd.Cmd):
def __init__(self, args):
cmd.Cmd.__init__(self)
self.args = args
self.prompt = '\x1b[1;34m%s\x1b[0m ' % '(carla)'
self.client = CarlaClient(args.host, args.port)
self.settings = get_default_carla_settings(args)
self.print_measurements = False
self.control = _Control()
self.thread = threading.Thread(target=self._agent_thread_worker)
self.done = False
self.thread.start()
def cleanup(self):
self.do_disconnect()
self.done = True
if self.thread.is_alive():
self.thread.join()
def default(self, line):
logging.error('unknown command \'%s\'!', line)
def emptyline(self):
pass
def precmd(self, line):
return line.strip().lower()
def can_exit(self):
return True
def do_exit(self, line=None):
"""Exit the console."""
return True
def do_eof(self, line=None):
"""Exit the console."""
print('exit')
return self.do_exit(line)
def help_help(self):
print('usage: help [topic]')
def do_disconnect(self, line=None):
"""Disconnect from the server."""
self.client.disconnect()
def do_new_episode(self, line=None):
"""Request a new episode. Connect to the server if not connected."""
try:
self.control = _Control()
if not self.client.connected():
self.client.connect()
self.client.load_settings(self.settings)
self.client.start_episode(0)
logging.info('new episode started')
except Exception as exception:
logging.error(exception)
def do_control(self, line=None):
"""Set control message:\nusage: control [reset|<param> <value>]\n(e.g., control throttle 0.5)"""
try:
if line == 'reset':
self.control = _Control()
else:
self.control.set(line)
logging.debug('control: %s', self.control.kwargs())
except Exception as exception:
logging.error(exception)
def complete_control(self, text, *args, **kwargs):
options = self.control.action_list()
options.append('reset')
return [x + ' ' for x in options if x.startswith(text)]
def do_settings(self, line=None):
"""Open a text editor to edit CARLA settings."""
result = edit_text(self.settings)
if result is not None:
self.settings = result
def do_print_measurements(self, line):
"""Print received measurements to console.\nusage: print_measurements [t/f]"""
self.print_measurements = True if not line else _Control._parse(bool, line)
def _agent_thread_worker(self):
filename = '_images/console/camera_{:0>3d}/image_{:0>8d}.png'
while not self.done:
try:
measurements, sensor_data = self.client.read_data()
if self.print_measurements:
print(measurements)
if self.args.images_to_disk:
images = [x for x in sensor_data.values() if isinstance(x, Image)]
for n, image in enumerate(images):
path = filename.format(n, measurements.game_timestamp)
image.save_to_disk(path)
self.client.send_control(**self.control.kwargs())
except Exception as exception:
# logging.exception(exception)
time.sleep(1)
class CarlaEnvironment(EnvironmentInterface):
def __init__(self,
experiment_path=None,
frame_skip=1,
server_height=512,
server_width=720,
camera_height=88,
camera_width=200,
experiment_suite=None,
quality="low",
cameras=[CameraTypes.FRONT],
weather_id=[1],
episode_max_time=30000,
max_speed=35.0,
port=2000,
map_name="Town01",
verbose=True,
seed=None,
is_rendered=True,
num_speedup_steps=30,
separate_actions_for_throttle_and_brake=False,
rendred_image_type='forward_camera'):
self.frame_skip = frame_skip # the frame skip affects the fps of the server directly. fps = 30 / frameskip
self.server_height = server_height
self.server_width = server_width
self.camera_height = camera_height
self.camera_width = camera_width
self.experiment_suite = experiment_suite # an optional CARLA experiment suite to use
self.quality = quality
self.cameras = cameras
self.weather_id = weather_id
self.episode_max_time = episode_max_time # miliseconds for each episode
self.max_speed = max_speed # km/h
self.port = port
self.host = 'localhost'
self.map_name = map_name
self.map_path = map_path_mapper[self.map_name]
self.experiment_path = experiment_path
self.current_episode_steps_counter = 1
# client configuration
self.verbose = verbose
self.episode_idx = 0
self.num_speedup_steps = num_speedup_steps
self.max_speed = max_speed
self.is_rendered = is_rendered
# setup server settings
self.experiment_suite = experiment_suite
self.separate_actions_for_throttle_and_brake = separate_actions_for_throttle_and_brake
self.rendred_image_type = rendred_image_type
self.left_poses = []
self.right_poses = []
self.follow_poses = []
self.Straight_poses = []
self.settings = CarlaSettings()
self.settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=15,
NumberOfPedestrians=30,
WeatherId=self.weather_id,
QualityLevel=self.quality,
SeedVehicles=seed,
SeedPedestrians=seed)
if seed is None:
self.settings.randomize_seeds()
self.settings = self.add_cameras(self.settings, self.cameras,
self.camera_width, self.camera_height)
# open the server
self.server = self.open_server()
logging.disable(40)
print("Successfully opened the server")
# open the client
self.game = CarlaClient(self.host, self.port, timeout=99999999)
print("Successfully opened the client")
self.game.connect()
print("Successfull Connection")
if self.experiment_suite:
self.current_experiment_idx = 0
self.current_experiment = self.experiment_suite.get_experiments()[
self.current_experiment_idx]
self.scene = self.game.load_settings(
self.current_experiment.conditions)
else:
self.scene = self.game.load_settings(self.settings)
# get available start positions
self.positions = self.scene.player_start_spots
self.num_positions = len(self.positions)
self.current_start_position_idx = 0
self.current_pose = 0
self.action_space = ActionSpace(
shape=2,
low=np.array([-1, -1]),
high=np.array([1, 1]),
descriptions=["steer", "gas_and_brake"])
# state space
#define all measurments
self.state_space = {
"measurements": {
"forward_speed": np.array([1]),
"x": np.array([1]),
"y": np.array([1]),
"z": np.array([1])
}
}
#define all cameras
for camera in self.scene.sensors:
self.state_space[camera.name] = {
"data": np.array([self.camera_height, self.camera_width, 3])
}
print("Define ", camera.name, " Camera")
# measurements
self.autopilot = None
self.planner = Planner(self.map_name)
#rendering
if self.is_rendered:
pygame.init()
pygame.font.init()
self.display = pygame.display.set_mode(
(self.camera_width, self.camera_height))
# env initialization
self.reset(True)
def reset(self, force_environment_reset=False):
"""
Reset the environment and all the variable of the wrapper
:param force_environment_reset: forces environment reset even when the game did not end
:return: A dictionary containing the observation, reward, done flag, action and measurements
"""
self.reset_ep = True
self.restart_environment_episode(force_environment_reset)
self.last_episode_time = time.time()
if self.current_episode_steps_counter > 0:
self.episode_idx += 1
self.done = False
self.total_reward_in_current_episode = self.reward = 0.0
self.last_action = 0
self.current_episode_steps_counter = 0
self.last_episode_images = []
self.step([0, 1, 0])
self.last_env_response = {
"reward": self.reward,
"next_state": self.state,
"goal": self.current_goal,
"game_over": self.done
}
return self.last_env_response
def add_cameras(self, settings, cameras, camera_width, camera_height):
# add a front facing camera
print("Available Cameras are ", cameras)
if CameraTypes.FRONT in cameras:
camera = Camera(CameraTypes.FRONT.value)
camera.set(FOV=100)
camera.set_image_size(camera_width, camera_height)
camera.set_position(2.0, 0, 1.4)
camera.set_rotation(-15.0, 0, 0)
settings.add_sensor(camera)
# add a left facing camera
if CameraTypes.LEFT in cameras:
camera = Camera(CameraTypes.LEFT.value)
camera.set(FOV=100)
camera.set_image_size(camera_width, camera_height)
camera.set_position(2.0, 0, 1.4)
camera.set_rotation(-15.0, -30, 0)
settings.add_sensor(camera)
# add a right facing camera
if CameraTypes.RIGHT in cameras:
camera = Camera(CameraTypes.RIGHT.value)
camera.set(FOV=100)
camera.set_image_size(camera_width, camera_height)
camera.set_position(2.0, 0, 1.4)
camera.set_rotation(-15.0, 30, 0)
settings.add_sensor(camera)
# add a front facing depth camera
if CameraTypes.DEPTH in cameras:
camera = Camera(CameraTypes.DEPTH.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, 30, 0)
camera.PostProcessing = 'Depth'
settings.add_sensor(camera)
# add a front facing semantic segmentation camera
if CameraTypes.SEGMENTATION in cameras:
camera = Camera(CameraTypes.SEGMENTATION.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, 30, 0)
camera.PostProcessing = 'SemanticSegmentation'
settings.add_sensor(camera)
print("Successfully adding a SemanticSegmentation camera")
return settings
def get_directions(self, current_point, end_point):
"""
Class that should return the directions to reach a certain goal
"""
directions = self.planner.get_next_command(
(current_point.location.x, current_point.location.y, 0.22),
(current_point.orientation.x, current_point.orientation.y,
current_point.orientation.z),
(end_point.location.x, end_point.location.y, 0.22),
(end_point.orientation.x, end_point.orientation.y,
end_point.orientation.z))
return directions
def open_server(self):
log_path = path.join(
self.experiment_path if self.experiment_path is not None else '.',
'logs', "CARLA_LOG_{}.txt".format(self.port))
if not os.path.exists(os.path.dirname(log_path)):
os.makedirs(os.path.dirname(log_path))
with open(log_path, "wb") as out:
cmd = [
path.join(environ.get('CARLA_ROOT'),
'CarlaUE4.sh'), self.map_path, "-benchmark",
"-carla-server", "-fps={}".format(30 / self.frame_skip),
"-world-port={}".format(self.port),
"-windowed -ResX={} -ResY={}".format(self.server_width,
self.server_height),
"-carla-no-hud"
]
print("CMD is : ", cmd)
# if self.config:
# cmd.append("-carla-settings={}".format(self.config))
p = subprocess.Popen(cmd, stdout=out, stderr=out)
return p
def close_server(self):
os.killpg(os.getpgid(self.server.pid), signal.SIGKILL)
def step(self, action):
# get measurements and observations
measurements = []
while type(measurements) == list:
measurements, sensor_data = self.game.read_data()
self.state = {}
for camera in self.scene.sensors:
if camera.name == 'segmentation':
#labels_to_road_noroad taker Sensor.Image not numpy array
self.state[camera.name] = labels_to_road_noroad(
sensor_data[camera.name])
else:
self.state[camera.name] = sensor_data[camera.name].data
#self.state[camera.name] = sensor_data[camera.name].data
self.location = [
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
]
self.distance_from_goal = np.linalg.norm(
np.array(self.location[:2]) -
[self.current_goal.location.x, self.current_goal.location.y])
is_collision = measurements.player_measurements.collision_vehicles != 0 \
or measurements.player_measurements.collision_pedestrians != 0 \
or measurements.player_measurements.collision_other != 0
speed_reward = measurements.player_measurements.forward_speed - 1
if speed_reward > 30.:
speed_reward = 30.
self.reward = speed_reward \
- (measurements.player_measurements.intersection_otherlane * 5) \
- (measurements.player_measurements.intersection_offroad * 5) \
- is_collision * 100 \
- np.abs(self.control.steer) * 10
# update measurements
#self.measurements = [measurements.player_measurements.forward_speed] + self.location
#TODO, Add control signals to measurements
control_signals = [
np.clip(action[0], -1, 1),
np.clip(action[1], 0, 1),
np.clip(action[2], 0, 1)
] #steer, throttle, brake
self.measurements = [measurements.player_measurements.forward_speed
] + self.location + control_signals
self.autopilot = measurements.player_measurements.autopilot_control
# The directions to reach the goal (0 Follow lane, 1 Left, 2 Right, 3 Straight)
directions = int(
self.get_directions(measurements.player_measurements.transform,
self.current_goal) - 2)
# if directions == 0:
# if self.reset_ep:
# self.follow_poses.append((self.current_start,self.current_goal))
# self.reset_ep = False
# elif directions == 1:
# self.left_poses.append((self.current_start,self.current_goal))
# elif directions == 2:
# self.right_poses.append((self.current_start,self.current_goal))
# elif directions == 1:
# if self.reset_ep:
# self.left_poses.append((self.current_start,self.current_goal))
# self.reset_ep = False
# elif directions == 2:
# if self.reset_ep:
# self.right_poses.append((self.current_start,self.current_goal))
# self.reset_ep = False
# elif directions == 3:
# if self.reset_ep:
# self.Straight_poses.append((self.current_start,self.current_goal))
# self.reset_ep = False
# else:
# self.reset_ep = False
self.state['high_level_command'] = directions
if (measurements.game_timestamp >=
self.episode_max_time) or is_collision:
self.done = True
self.state['measurements'] = np.array(self.measurements)
#prepare rendered image and update display
if self.is_rendered:
#check if user wants to close rendering, else continue rendering
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
self.is_rendered = False
self.surface = pygame.surfarray.make_surface(
self.state[self.rendred_image_type].swapaxes(0, 1))
self.display.blit(self.surface, (0, 0))
pygame.display.flip()
self.take_action(action)
def take_action(self, action):
self.control = VehicleControl()
# transform the 2 value action (steer, throttle - brake) into a 3 value action (steer, throttle, brake)
self.control.steer = np.clip(action[0], -1, 1)
self.control.throttle = np.clip(action[1], 0, 1)
self.control.brake = np.abs(np.clip(action[2], 0, 1))
# prevent braking
# if not self.allow_braking or self.control.brake < 0.1 or self.control.throttle > self.control.brake:
# self.control.brake = 0
# prevent over speeding
if hasattr(self, 'measurements') and self.measurements[
0] * 3.6 > self.max_speed and self.control.brake == 0:
self.control.throttle = 0.0
self.control.hand_brake = False
self.control.reverse = False
self.game.send_control(self.control)
def load_experiment(self, experiment_idx):
print("Loading the experiment")
self.current_experiment = self.experiment_suite.get_experiments(
)[experiment_idx]
self.scene = self.game.load_settings(
self.current_experiment.conditions)
self.positions = self.scene.player_start_spots
self.num_positions = len(self.positions)
self.current_start_position_idx = 0
self.current_pose = 0
def restart_environment_episode(self, force_environment_reset=False):
# select start and end positions
print("restarting new Episode")
if self.experiment_suite:
# if an expeirent suite is available, follow its given poses
if self.current_pose >= len(self.current_experiment.poses):
# load a new experiment
self.current_experiment_idx = (
self.current_experiment_idx + 1) % len(
self.experiment_suite.get_experiments())
self.load_experiment(self.current_experiment_idx)
self.current_start_position_idx = self.current_experiment.poses[
self.current_pose][0]
self.current_start = self.positions[self.current_experiment.poses[
self.current_pose][0]]
self.current_goal = self.positions[self.current_experiment.poses[
self.current_pose][1]]
self.current_pose += 1
else:
# go over all the possible positions in a cyclic manner
self.current_start_position_idx = (
self.current_start_position_idx + 1) % self.num_positions
self.current_start = self.positions[
self.current_start_position_idx]
# choose a random goal destination
self.current_goal = random.choice(self.positions)
try:
self.game.start_episode(self.current_start_position_idx)
except:
self.game.connect()
self.game.start_episode(self.current_start_position_idx)
# start the game with some initial speed
for i in range(self.num_speedup_steps):
self.control = VehicleControl(throttle=1.0,
brake=0,
steer=0,
hand_brake=False,
reverse=False)
self.game.send_control(VehicleControl())
def get_rendered_image(self) -> np.ndarray:
"""
Return a numpy array containing the image that will be rendered to the screen.
This can be different from the observation. For example, mujoco's observation is a measurements vector.
:return: numpy array containing the image that will be rendered to the screen
"""
image = [self.state[camera.name] for camera in self.scene.sensors]
image = np.vstack(image)
return image
class CarlaClientConsole(cmd.Cmd):
def __init__(self, args):
cmd.Cmd.__init__(self)
self.args = args
self.prompt = '\x1b[1;34m%s\x1b[0m ' % '(carla)'
self.client = CarlaClient(args.host, args.port)
self.settings = get_default_carla_settings(args)
self.print_measurements = False
self.control = _Control()
self.thread = threading.Thread(target=self._agent_thread_worker)
self.done = False
self.thread.start()
def cleanup(self):
self.do_disconnect()
self.done = True
if self.thread.is_alive():
self.thread.join()
def default(self, line):
logging.error('unknown command \'%s\'!', line)
def emptyline(self):
pass
def precmd(self, line):
return line.strip().lower()
def can_exit(self):
return True
def do_exit(self, line=None):
"""Exit the console."""
return True
def do_eof(self, line=None):
"""Exit the console."""
print('exit')
return self.do_exit(line)
def help_help(self):
print('usage: help [topic]')
def do_disconnect(self, line=None):
"""Disconnect from the server."""
self.client.disconnect()
def do_new_episode(self, line=None):
"""Request a new episode. Connect to the server if not connected."""
try:
self.control = _Control()
if not self.client.connected():
self.client.connect()
self.client.load_settings(self.settings)
self.client.start_episode(0)
logging.info('new episode started')
except Exception as exception:
logging.error(exception)
def do_control(self, line=None):
"""Set control message:\nusage: control [reset|<param> <value>]\n(e.g., control throttle 0.5)"""
try:
if line == 'reset':
self.control = _Control()
else:
self.control.set(line)
logging.debug('control: %s', self.control.kwargs())
except Exception as exception:
logging.error(exception)
def complete_control(self, text, *args, **kwargs):
options = self.control.action_list()
options.append('reset')
return [x + ' ' for x in options if x.startswith(text)]
def do_settings(self, line=None):
"""Open a text editor to edit CARLA settings."""
result = edit_text(self.settings)
if result is not None:
self.settings = result
def do_print_measurements(self, line):
"""Print received measurements to console.\nusage: print_measurements [t/f]"""
self.print_measurements = True if not line else _Control._parse(
bool, line)
def _agent_thread_worker(self):
filename = '_images/console/camera_{:0>3d}/image_{:0>8d}.png'
while not self.done:
try:
measurements, sensor_data = self.client.read_data()
if self.print_measurements:
print(measurements)
if self.args.images_to_disk:
images = [
x for x in sensor_data.values()
if isinstance(x, Image)
]
for n, image in enumerate(images):
path = filename.format(n, measurements.game_timestamp)
image.save_to_disk(path)
self.client.send_control(**self.control.kwargs())
except Exception as exception:
# logging.exception(exception)
time.sleep(1)
class CarlaEnv(gym.Env):
def __init__(self,
host="localhost",
num_vehicles=1,
vehicles_seed=lambda: 200,
player_starts=2,
goals=0):
self.num_vehicles = num_vehicles
self.vehicles_seed = vehicles_seed
self.starts = list_from_scalar(player_starts)
self.goals = list_from_scalar(goals)
self.port = get_open_port()
self.host = host
self.metadata = {
'render.modes': ['rgb_array'],
#'video.frames_per_second': int(np.round(1.0 / self.dt))
}
self.server = self.open_server()
print(f"Connecting to CARLA Client on port {self.port}")
self.client = CarlaClient(self.host, self.port, timeout=99999999)
time.sleep(3)
self.client.connect(connection_attempts=1000)
print(f"Connected on port: {self.port}")
self.action_space = gym.spaces.Box(
-2,
2, (len(self._map_controls([1, 2, 3, 4, 5]).items()), ),
dtype=np.float32)
obs_size = len(self.reset())
self.observation_space = gym.spaces.Box(-float("inf"),
float("inf"), (obs_size, ),
dtype=np.float32)
def dist_from_goal(self, measurements):
x = array_from_loc(measurements.player_measurements.transform.location)
y = array_from_loc(self.scene.player_start_spots[self.goal].location)
return np.sqrt(np.sum((x - y)**2))
def open_server(self):
#os.devnull
log_file = f"./logs/carla_logs/carla_{self.port}.txt"
os.makedirs(os.path.dirname(log_file), exist_ok=True)
with open(log_file, "wb+") as out:
"""
true_script_name = os.path.join(CARLA_PATH, 'CarlaUE4.sh')
if os.path.islink(true_script_name):
true_script_name = os.readlink(execute_path)
project_root = os.path.dirname(true_script_name)
execute_path = f"{project_root}/CarlaUE4/Binaries/Linux/CarlaUE4"
# chmod +x
st = os.stat(execute_path)
os.chmod(execute_path, st.st_mode | stat.S_IEXEC)
"""
cmd = [
os.path.join(CARLA_PATH,
'CarlaUE4.sh'), #execute_path, "CarlaUE4",
"-carla-server",
"-fps=60",
f"-world-port={self.port}",
f"-windowed -ResX={500} -ResY={500}",
"-carla-no-hud"
]
# - benchmark
p = subprocess.Popen(cmd,
stdout=out,
stderr=out,
stdin=subprocess.PIPE,
preexec_fn=os.setpgrp)
return p
def close_server(self):
#self.server.terminate()
pid = self.server.pid
no_of_attempts = 0
def is_process_alive(pid):
## Source: https://stackoverflow.com/questions/568271/how-to-check-if-there-exists-a-process-with-a-given-pid-in-python
try:
os.kill(pid, 0)
except OSError:
return False
return True
while is_process_alive(pid):
pgroup = os.getpgid(self.server.pid)
self.server.terminate()
os.killpg(pgroup, signal.SIGTERM)
_, _ = os.waitpid(pid, os.WNOHANG)
time.sleep(5)
def _add_settings(self):
self.settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self.num_vehicles,
NumberOfPedestrians=0,
WeatherId=1,
QualityLevel="Low",
SeedVehicles=self.vehicles_seed())
#settings.randomize_seeds()
def _add_sensors(self):
camera0 = Camera('RenderCamera0')
# Set image resolution in pixels.
camera0.set_image_size(800, 600)
# Set its position relative to the car in meters.
camera0.set_position(-4.30, 0, 2.60)
camera0.set_rotation(pitch=-25, yaw=0, roll=0)
self.settings.add_sensor(camera0)
def _map_controls(self, a):
return dict(steer=a[0], throttle=a[1])
def _process_observation(self, measurements, sensor_data):
return array_from_measurements(measurements)
def _get_reward_and_termination(self):
measurements, sensor_data = self.current_state
collision_penalty = (
measurements.player_measurements.collision_vehicles +
measurements.player_measurements.collision_other)
is_collided = collision_penalty > 2
offroad_penalty = measurements.player_measurements.intersection_offroad
is_offroad = offroad_penalty > 0.5
dist_from_goal = self.dist_from_goal(measurements)
is_at_target = dist_from_goal < 5
is_done = is_collided or is_at_target or is_offroad
distance_reward = 300 - dist_from_goal
collision_cost = collision_penalty / 300
offroad_cost = (offroad_penalty * 10)
reward = distance_reward - collision_cost - offroad_cost
reward = reward / 20
return reward, is_done
def get_new_start_goal(self):
start = np.random.choice(self.starts)
goal = np.random.choice(self.goals)
while goal == start:
start = np.random.choice(self.starts)
goal = np.random.choice(self.goals)
return start, goal
def reset(self):
self.settings = CarlaSettings()
self._add_settings()
self._add_sensors()
self.scene = self.client.load_settings(self.settings)
start, goal = self.get_new_start_goal()
self.goal = goal
for i in range(100):
try:
self.client.start_episode(start)
self.current_state = self.client.read_data()
if i > 0:
print("Reconnected.")
break
except carla.tcp.TCPConnectionError:
if i % 10 == 0:
print(f"There was a TCP Error (Attempt {i}). Retrying. ")
time.sleep(3)
measurements, sensor_data = self.current_state
return self._process_observation(measurements, sensor_data)
def step(self, a):
control = self._map_controls(a)
for i in range(100):
try:
self.client.send_control(**control)
break
except carla.tcp.TCPConnectionError:
time.sleep(0.5)
self.current_state = self.client.read_data()
measurements, sensor_data = self.current_state
reward, is_done = self._get_reward_and_termination()
obs = self._process_observation(measurements, sensor_data)
return obs, reward, is_done, {}
def render(self, mode='human', **kwargs):
if mode == 'rgb_array':
return np.concatenate([
sensor.data for name, sensor in self.current_state[1].items()
if "Render" in name
],
axis=1)
super().render(mode=mode, **kwargs)
def _close(self):
self.close()
def close(self):
print(
f"Disconnecting from CARLA Client (port: {self.port}, pid: {self.server.pid})"
)
self.close_server()
time.sleep(3)
while self.client.connected():
self.client.disconnect()
print(
f"Disconnected from CARLA Client (port: {self.port}, pid: {self.server.pid})."
)
class CarlaEnvironmentWrapper(EnvironmentWrapper):
def __init__(self,
num_speedup_steps=10,
require_explicit_reset=True,
is_render_enabled=False,
automatic_render=False,
early_termination_enabled=False,
run_offscreen=False,
cameras=['SceneFinal'],
save_screens=False,
carla_settings=None,
carla_server_settings=None,
location_indices=(9, 16)):
EnvironmentWrapper.__init__(self, is_render_enabled, save_screens)
self.automatic_render = automatic_render
self.episode_max_time = 100000 # miliseconds for each episode
self.allow_braking = True
self.log_path = "carla_logs.txt"
self.verbose = True
self.observation = None
self.num_speedup_steps = num_speedup_steps
self.counter = 0
self.rgb_camera_name = 'CameraRGB'
self.rgb_camera = 'SceneFinal' in cameras
self.is_game_ready_for_input = False
self.run_offscreen = run_offscreen
self.kill_when_connection_lost = True
# server configuration
self.carla_server_settings = carla_server_settings
self.port = get_open_port()
self.host = 'localhost'
# Why town2: https://github.com/carla-simulator/carla/issues/10#issuecomment-342483829
self.level = 'town2'
self.map = CarlaLevel().get(self.level)
# client configuration
if type(carla_settings) == str:
# load settings from file
with open(carla_settings, 'r') as fp:
self.settings = fp.read()
elif type(carla_settings) == CarlaSettings:
self.settings = carla_settings
carla_settings = None
elif carla_settings == None:
raise Exception(
"Please load a CarlaSettings object or provide a path to a settings file.")
self.car_speed = 0.
self.max_speed = 10. #m/s
# Will be true only when setup_client_and_server() is called, either explicitly, or by reset()
self.is_game_setup = False
# measurements
self.autopilot = None
self.kill_if_unmoved_for_n_steps = 50*4
self.unmoved_steps = 0.0
self.early_termination_enabled = early_termination_enabled
if self.early_termination_enabled:
self.max_neg_steps = 100*4
self.cur_neg_steps = 0
self.early_termination_punishment = 20.0
# env initialization
if not require_explicit_reset:
self.reset(True)
# camera-view renders
if self.automatic_render:
self.init_renderer()
if self.save_screens:
create_dir(self.images_path)
self.rgb_img_path = self.images_path+"/rgb/"
# locations indices
self.start_loc_idx = location_indices[0]
self.end_loc_idx = location_indices[1]
self.end_loc = None
self.last_distance = 0
self.curr_distance = 0
self.end_loc_measurement = None
def setup_client_and_server(self, reconnect_client_only=False, settings=None):
# open the server
if not reconnect_client_only:
self.server = self._open_server()
self.server_pid = self.server.pid # To kill incase child process gets lost
# open the client
self.game = CarlaClient(self.host, self.port, timeout=99999999)
# It's taking a very long time for the server process to spawn, so the client needs to wait or try sufficient no. of times lol
self.game.connect(connection_attempts=100)
self.scene = self.game.load_settings(self.settings)
# get available start positions
positions = self.scene.player_start_spots
start_loc = positions[self.start_loc_idx].location
end_loc = positions[self.end_loc_idx].location
self.end_loc = end_loc
self.last_distance = self.cal_distance(start_loc, end_loc)
self.curr_distance = self.last_distance
self.end_loc_measurement = [end_loc.x, end_loc.y, end_loc.z]
# self.num_pos = len(positions)
# self.iterator_start_positions = 0
self.is_game_setup = self.server and self.game
return
def close_client_and_server(self):
self._close_server()
print("\t Disconnecting the client")
self.game.disconnect()
self.game = None
self.server = None
self.is_game_setup = False
return
def _open_server(self):
# Note: There is no way to disable rendering in CARLA as of now
# https://github.com/carla-simulator/carla/issues/286
# decrease the window resolution if you want to see if performance increases
# Command: $CARLA_ROOT/CarlaUE4.sh /Game/Maps/Town02 -benchmark -carla-server -fps=15 -world-port=9876 -windowed -ResX=480 -ResY=360 -carla-no-hud
# To run off_screen: SDL_VIDEODRIVER=offscreen SDL_HINT_CUDA_DEVICE=0 <command> #https://github.com/carla-simulator/carla/issues/225
my_env = None
if self.run_offscreen:
# my_env = {**os.environ, 'SDL_VIDEODRIVER': 'offscreen', 'SDL_HINT_CUDA_DEVICE':'0'}
pass
with open(self.log_path, "wb") as out:
cmd = [path.join(environ.get('CARLA_ROOT'), 'CarlaUE4.sh'),
self.map,
"-benchmark", "-carla-server", "-fps=10", "-world-port={}".format(
self.port),
"-windowed -ResX={} -ResY={}".format(
carla_config.server_width, carla_config.server_height),
"-carla-no-hud"]
if self.carla_server_settings:
cmd.append("-carla-settings={}".format(self.carla_server_settings))
p = subprocess.Popen(cmd, stdout=out, stderr=out, env=my_env)
return p
def _close_server(self):
if self.kill_when_connection_lost:
os.killpg(os.getpgid(self.server.pid), signal.SIGKILL)
return
no_of_attempts = 0
while is_process_alive(self.server_pid):
print("Trying to close Carla server with pid %d" % self.server_pid)
if no_of_attempts < 5:
self.server.terminate()
elif no_of_attempts < 10:
self.server.kill()
elif no_of_attempts < 15:
os.kill(self.server_pid, signal.SIGTERM)
else:
os.kill(self.server_pid, signal.SIGKILL)
time.sleep(10)
no_of_attempts += 1
def check_early_stop(self, player_measurements, immediate_reward):
# if player_measurements.intersection_offroad > 0.75 or \
if immediate_reward < 0 :
# or \
# (self.control.throttle == 0.0 and player_measurements.forward_speed < 0.1 and self.control.brake != 0.0):
self.cur_neg_steps += 1
# print("updated neg steps:{}".format(self.cur_neg_steps))
early_done = (self.cur_neg_steps > self.max_neg_steps)
if early_done:
print("\t Early terminate. neg steps:{}, unmoved:{}".format(self.cur_neg_steps, self.unmoved_steps))
return early_done, self.early_termination_punishment
else:
self.cur_neg_steps = self.cur_neg_steps - 1 if self.cur_neg_steps > 0 else 0 # decay
return False, 0.0
def _update_state(self):
# get measurements and observations
try:
measurements, sensor_data = self.game.read_data()
except:
# Connection between cli and server lost; reconnect
if self.kill_when_connection_lost:
raise
print("\t Connection to server lost while reading state. Reconnecting...")
self.close_client_and_server()
self.setup_client_and_server(reconnect_client_only=False)
self.done = True
self.location = [measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z]
self.last_distance = self.curr_distance
self.curr_distance = self.cal_distance(
measurements.player_measurements.transform.location, self.end_loc)
is_collision = measurements.player_measurements.collision_vehicles != 0 \
or measurements.player_measurements.collision_pedestrians != 0 \
or measurements.player_measurements.collision_other != 0
# CARLA doesn't recognize if collision occured and colliding speed is less than 5 km/h (Around 0.7 m/s)
# Ref: https://github.com/carla-simulator/carla/issues/13
# Recognize that as a collision
self.car_speed = measurements.player_measurements.forward_speed
if self.control.throttle > 0.25 and self.car_speed < 0.7 and self.is_game_ready_for_input:
self.unmoved_steps += 1
# print("updated unmoved(carla bug) steps:{}".format(self.unmoved_steps), self.control.throttle, self.car_speed, self.is_game_ready_for_input)
if self.unmoved_steps > self.kill_if_unmoved_for_n_steps:
is_collision = True
print("\t Car stuck somewhere. neg steps:{}, unmoved:{}".format(self.cur_neg_steps, self.unmoved_steps))
# elif self.unmoved_steps > 0:
# # decay slowly, since it may be stuck and not accelerate few times
# self.unmoved_steps -= 0.50
if is_collision:
print("\t Collision occured.")
# Reward Shaping:
speed_reward = self.car_speed
if speed_reward > 30.:
speed_reward = 30.
self.reward = speed_reward * 5 \
- (measurements.player_measurements.intersection_otherlane * 1.5) \
- (measurements.player_measurements.intersection_offroad * 1.5) \
- is_collision * 10 \
- np.abs(self.control.steer) * 1.5 \
+ (self.last_distance - self.curr_distance)*10
# Scale down the reward by a factor
# self.reward /= 10
# print("reward: {} = {} - ({} * 1.5) - ({} * 1.5) - {} * 10 - np.abs({}) * 10 + ({} - {})*10".format(self.reward,
# speed_reward,
# measurements.player_measurements.intersection_otherlane,
# measurements.player_measurements.intersection_offroad,
# is_collision,
# self.control.steer,
# self.last_distance,
# self.curr_distance))
if self.early_termination_enabled:
early_done, punishment = self.check_early_stop(
measurements.player_measurements, self.reward)
if early_done:
self.done = True
self.reward -= punishment
# update measurements
self.observation = np.hstack(self.location + self.end_loc_measurement + [measurements.player_measurements.acceleration.x,
measurements.player_measurements.acceleration.y,
measurements.player_measurements.acceleration.z,
measurements.player_measurements.forward_speed])
if self.rgb_camera:
img_data = sensor_data[self.rgb_camera_name].data/255.
self.observation = [self.observation, img_data]
# self.counter +=1
# self.observation = np.array(
# [np.array([self.counter - 1]), np.array([self.counter, self.counter + 1])])
# print(self.observation.shape)
self.autopilot = measurements.player_measurements.autopilot_control
if (measurements.game_timestamp >= self.episode_max_time) or is_collision:
# print('EPISODE IS DONE. GameTime: {}, Collision: {}'.format(str(measurements.game_timestamp),
# str(is_collision)))
self.done = True
def _take_action(self, action):
if not self.is_game_setup:
print("\t Reset the environment by reset() before calling step()")
sys.exit(1)
# assert len(actions) == 2, "Send actions in the format [steer, accelaration]"
self.control = VehicleControl()
self.control.steer = np.clip(action[0], -1, 1)
self.control.throttle = np.clip(action[1], 0, 1)
self.control.brake = np.abs(np.clip(action[1], -1, 0))
self.control.hand_brake = False
self.control.reverse = False
# prevent braking
if not self.allow_braking or self.control.brake < 0.1 or self.control.throttle > self.control.brake:
self.control.brake = 0
if self.car_speed > self.max_speed and self.control.brake == 0:
self.control.throttle = 0.0
controls_sent = False
while not controls_sent:
try:
self.game.send_control(self.control)
controls_sent = True
# print("controls sent!")
except Exception:
traceback.print_exc()
if self.kill_when_connection_lost:
print("\t Connection to server lost while sending controls. Reconnecting...")
self.close_client_and_server()
self.setup_client_and_server(reconnect_client_only=False)
self.done = True
def _restart_environment_episode(self, force_environment_reset=True, settings=None):
if not force_environment_reset and not self.done and self.is_game_setup:
print("Can't reset dude, episode ain't over yet")
return None # User should handle this
self.is_game_ready_for_input = False
if not self.is_game_setup:
self.setup_client_and_server()
# if self.is_render_enabled:
# self.init_renderer()
else:
# get settings. Only needed if we want random rollouts
if type(settings) == str:
# load settings from file
with open(settings, 'r') as fp:
self.settings = fp.read()
self.scene = self.game.load_settings(self.settings)
elif type(settings) == CarlaSettings:
self.settings = settings
self.scene = self.game.load_settings(self.settings)
elif settings == None:
pass # already set during initialization
# self.iterator_start_positions += 1
# if self.iterator_start_positions >= self.num_pos:
# self.iterator_start_positions = 0
try:
self.game.start_episode(self.start_loc_idx)
except:
self.game.connect()
self.game.start_episode(self.start_loc_idx)
self.unmoved_steps = 0.0
self.cur_neg_steps = 0
self.car_speed = 0
# start the game with some initial speed
observation = None
for i in range(self.num_speedup_steps):
observation, reward, done, _ = self.step([0, 0.25])
self.observation = observation
self.is_game_ready_for_input = True
return observation
def init_renderer(self):
self.num_cameras = 0
if self.rgb_camera:
self.num_cameras += 1
self.renderer.create_screen(
carla_config.render_width, carla_config.render_height*self.num_cameras)
def get_rendered_image(self):
temp = []
if self.rgb_camera:
temp.append(self.observation['rgb_image'])
return np.vstack((img for img in temp))
def save_screenshots(self):
if not self.save_screens:
print("save_screens is set False")
return
filename = str(int(time.time()*100))
if self.rgb_camera:
save_image(self.rgb_img_path+filename+".png",
self.observation['rgb_image'])
def cal_distance(self, start, end):
return ((end.x - start.x)**2 + (end.y - start.y)**2 + (end.z - start.z)**2)**0.5
class gym_carla_car_following:
def __init__(self, host="127.0.0.1", port=2000, timeout=15):
# Steer, Throttle/Brake
self.action_space = Box(low=np.array([-1.0, -1.0]),
high=np.array([1.0, 1.0]))
self.speed_scale = 50 # km/h
self.relative_x_scale = 4.0 # m
self.relative_y_scale = 40.0 # m
self.relative_angle_scale = 180 # degree
self._history_path_num = 80
low = np.zeros([self._history_path_num * 2 + 5])
high = np.ones([self._history_path_num * 2 + 5])
# Only shape of low/high array matters
self.observation_space = Box(low=low, high=high)
self._host = host
self._port = port
self._timeout = timeout
def reset(self):
# connect to server
self._client = CarlaClient(self._host, self._port, self._timeout)
self._client.connect()
# settings
index = random.randint(0, 4) # Last one is for testing
# print(index)
# index = 0
seed = npc_vehicle_seeds[index]
start_list_index = start_list_indices[index]
settings = CarlaSettings()
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=1,
NumberOfPedestrians=0,
SeedVehicles=seed,
SeedPedestrians=123456789,
WeatherId=weathers[index])
self._client.load_settings(settings)
self._client.start_episode(start_list_index)
# simulator init stage
for i in range(10):
measurements, _ = self._client.read_data()
self._client.send_control(steer=0,
throttle=0,
brake=0,
hand_brake=False,
reverse=False)
self._client.send_control(steer=0,
throttle=0,
brake=0,
hand_brake=False,
reverse=False)
# Reset Flags
self._reset = True
self._history_path = np.zeros([self._history_path_num, 4])
self._cur_steer = 0
self._his_steer = 0
observation = self._observe()
self._observation = observation
return observation
def step(self, action):
steering = action[0]
# filter small value
if action[1] >= 0.01:
acceleration = action[1]
brake = 0
elif action[1] <= -0.01:
acceleration = 0
brake = -action[1]
else:
acceleration = 0
brake = 0
# one command for every 2 frame
for i in range(2):
self._client.send_control(steer=steering,
throttle=acceleration,
brake=brake,
hand_brake=False,
reverse=False)
observation = self._observe()
info = {}
done = self._calculate_done(observation[:5])
reward = self._calculate_reward(observation[:5], done)
self._his_steer = self._cur_steer
self._cur_steer = steering
return observation, reward, done, info
def stop(self):
self._client.disconnect()
# Wait for server to be connectable again
import time
time.sleep(8)
def print_state(self, step, state, action, reward, epsilon=0):
content = "Step %d : epsilong=%f \n" \
"\tState : self_speed=%f(km/h), npc_speed=%f(km/h), rel_angle=%f(degree)\n," \
"\t\trel_x=%f(m), rel_y=%f(m), last_rel_x=%f(m), last_rel_y=%f(m)\n" \
"\tAction : steer=%f, throttle/brake=%f, reward=%f\n" \
% (step, epsilon, state[0] * self.speed_scale, state[1] * self.speed_scale, state[2] * self.relative_angle_scale,
state[3] * self.relative_x_scale, state[4] * self.relative_y_scale,
state[-2] * self.relative_x_scale, state[-1] * self.relative_y_scale,
action[0], action[1], reward)
print_over_same_line(content)
def _observe(self):
#################################### current state #######################################
measurements, _ = self._client.read_data()
self_car = measurements.player_measurements
self_speed = self_car.forward_speed
npc_car = measurements.non_player_agents[-1].vehicle
npc_speed = npc_car.forward_speed
# normalization
self_speed /= self.speed_scale
if self_speed < 0.0:
self_speed = 0.0
npc_speed /= self.speed_scale
if npc_speed < 0.0:
npc_speed = 0.0
# Coordination in this simulator is like this :
# ^ +
# I
# I
# <-------0--------
# + I -
# I -
# cm -> m
npc_pos_x = -npc_car.transform.location.x / 100.0
npc_pos_y = npc_car.transform.location.y / 100.0
npc_orientation_x = -npc_car.transform.orientation.x
npc_orientation_y = npc_car.transform.orientation.y
pos_x = -self_car.transform.location.x / 100.0
pos_y = self_car.transform.location.y / 100.0
orientation_x = -self_car.transform.orientation.x
orientation_y = self_car.transform.orientation.y
[npc_relative_x, npc_relative_y, npc_relative_angle] = \
self._transform_coordination(pos_x, pos_y, orientation_x, orientation_y,
npc_pos_x, npc_pos_y, npc_orientation_x, npc_orientation_y)
# normalization
npc_relative_x /= self.relative_x_scale
npc_relative_y /= self.relative_y_scale
npc_relative_angle /= self.relative_angle_scale
npc_relative_angle = np.clip(npc_relative_angle, -1.0, 1.0)
observation = np.zeros([self._history_path_num * 2 + 5])
observation[0] = self_speed
observation[1] = npc_speed
observation[2] = npc_relative_angle
observation[3] = npc_relative_x
observation[4] = npc_relative_y
#################################### history state #######################################
state = np.hstack(
(npc_pos_x, npc_pos_y, npc_orientation_x, npc_orientation_y))
if self._reset == True:
self._history_path[:] = state
self._reset = False
for i in range(self._history_path_num):
observation[5 + (self._history_path_num - 1 - i) *
2] = npc_relative_x
observation[5 + (self._history_path_num - 1 - i) * 2 +
1] = npc_relative_y
else:
last_his_num = 0
for i in range(self._history_path_num):
npc_pos_x = self._history_path[i, 0]
npc_pos_y = self._history_path[i, 1]
npc_orientation_x = self._history_path[i, 2]
npc_orientation_y = self._history_path[i, 3]
[npc_relative_x, npc_relative_y, npc_relative_angle] = \
self._transform_coordination(pos_x, pos_y, orientation_x, orientation_y,
npc_pos_x, npc_pos_y, npc_orientation_x, npc_orientation_y)
# normalization
npc_relative_x /= self.relative_x_scale
npc_relative_y /= self.relative_y_scale
npc_relative_angle /= self.relative_angle_scale
npc_relative_angle = np.clip(npc_relative_angle, -1.0, 1.0)
if npc_relative_y > 0.025:
observation[5 + i * 2] = npc_relative_x
observation[5 + i * 2 + 1] = npc_relative_y
else:
last_his_num = i
# print("********************************************************")
# print(observation[5 + i * 2 - 2], observation[5 + i * 2 + 1 - 2])
# print("********************************************************")
break
if last_his_num > 0:
# print("**********************************************")
# print(last_his_num)
# print("*************************************************")
for i in range(last_his_num, self._history_path_num):
observation[5 + i * 2] = observation[5 + (i - 1) * 2]
observation[5 + i * 2 + 1] = observation[5 + (i - 1) * 2 +
1]
self._history_path = np.roll(self._history_path, 1, axis=0)
self._history_path[0] = state
return observation
def _calculate_reward(self, observation, done):
speed = observation[0]
npc_speed = observation[1]
rel_angle = observation[2]
rel_x = observation[3]
rel_y = observation[4]
# reward = 0
if done == True:
reward = -1000.0
else:
# How to calculate reward ?
reward = ((rel_y - 0.5)**2) * 20.0 \
- abs(rel_x) * 20.0 + \
- abs(rel_angle - 1.0 / 2) * 20 \
- abs(speed - npc_speed) * 20.0 \
- abs(self._cur_steer - self._his_steer) * 20
return reward
def _calculate_done(self, observation):
speed = observation[0]
npc_speed = observation[1]
rel_angle = observation[2]
rel_x = observation[3]
rel_y = observation[4]
done = False
# if (abs(rel_x) >= 2.5) or \
# (rel_y >= 2.5) or (rel_y <= 0.15) or \
# (rel_angle < 1.0 / 8) or (rel_angle > 7.0 / 8):
# done = True
distance = math.sqrt((rel_x * self.relative_x_scale)**2 + \
(rel_y * self.relative_y_scale)**2)
if (distance > 80) or \
(distance < 8) or \
((rel_angle < -0.1) and (rel_angle > -0.9)):
done = True
elif self._calculate_stuck() == True:
# judge if stuck
done = True
return done
def _calculate_stuck(self):
return False
def _normalize_angle(self, angle):
while angle > 180:
angle -= 360
while angle < -180:
angle += 360
return angle
def _transform_coordination(self, pos_x, pos_y, orientation_x,
orientation_y, npc_pos_x, npc_pos_y,
npc_orientation_x, npc_orientation_y):
absolute_angle = math.atan2(orientation_y,
orientation_x) * 180 / 3.1415926
npc_absolute_angle = math.atan2(npc_orientation_y,
npc_orientation_x) * 180 / 3.1415926
# print("self %f %f with %f %f %f" % (pos_x, pos_y, orientation_x, orientation_y, absolute_angle))
# print("npc %f %f with %f %f %f" % (npc_pos_x, npc_pos_y, npc_orientation_x, npc_orientation_y, npc_absolute_angle))
delta_pos_x = npc_pos_x - pos_x
delta_pos_y = npc_pos_y - pos_y
npc_absolute_pos_angle = math.atan2(delta_pos_y,
delta_pos_x) * 180 / 3.1415926
# print("delta_x = %f, delta_y = %f " % (delta_pos_x, delta_pos_y))
npc_relative_angle = (90 - absolute_angle) + npc_absolute_angle
npc_relative_pos_angle = (90 - absolute_angle) + npc_absolute_pos_angle
distance = math.sqrt(delta_pos_x**2 + delta_pos_y**2)
npc_relative_pos_x = distance * math.cos(
npc_relative_pos_angle * 3.1415926 / 180)
npc_relative_pos_y = distance * math.sin(
npc_relative_pos_angle * 3.1415926 / 180)
npc_relative_angle = self._normalize_angle(npc_relative_angle)
# print("relative %f %f %f %f" % (npc_relative_pos_x, npc_relative_pos_y, npc_relative_angle, distance))
return [npc_relative_pos_x, npc_relative_pos_y, npc_relative_angle]
class CarlaEnv(gym.Env):
def __init__(self, config=ENV_CONFIG, enable_autopilot=False):
self.enable_autopilot = enable_autopilot
self.config = config
self.config["x_res"], self.config["y_res"] = IMG_SIZE
self.city = self.config["server_map"].split("/")[-1]
if self.config["enable_planner"]:
self.planner = Planner(self.city)
self.intersections_node = self.planner._city_track._map.get_intersection_nodes(
)
self.intersections_pos = np.array([
self.planner._city_track._map.convert_to_world(
intersection_node)
for intersection_node in self.intersections_node
])
self.pre_intersection = np.array([0.0, 0.0])
# # Cartesian coordinates
self.headings = np.array([[1, 0], [-1, 0], [0, 1], [0, -1]])
# self.lrs_matrix = {"GO_STRAIGHT": np.array([[1,0],[0,1]]),\
# "TURN_RIGHT": np.array([[0,-1],[1,0]]),\
# "TURN_LEFT": np.array([[0,1],[-1,0]])}
# self.goal_heading = np.array([0.0,0.0])
# self.current_heading = None
# self.pre_heading = None
# self.angular_speed = None
# Angular degree
self.headings_degree = np.array(
[0.0, 180.0, 90.0, -90.0]) # one-one mapping to self.headings
self.lrs_degree = {
"GO_STRAIGHT": 0.0,
"TURN_LEFT": -90.0,
"TURN_RIGHT": 90.0
}
self.goal_heading_degree = 0.0
self.current_heading_degree = None
self.pre_heading_degree = None
self.angular_speed_degree = np.array(0.0)
# The Action Space
if config["discrete_actions"]:
self.action_space = Discrete(
len(DISCRETE_ACTIONS
)) # It will be transformed to continuous 2D action.
else:
self.action_space = Box(-1.0, 1.0, shape=(2, ),
dtype=np.float32) # 2D action.
if config["use_sensor"] == 'use_semantic':
image_space = Box(0.0,
255.0,
shape=(config["y_res"], config["x_res"],
1 * config["framestack"]),
dtype=np.float32)
elif config["use_sensor"] in ['use_depth', 'use_logdepth']:
image_space = Box(0.0,
255.0,
shape=(config["y_res"], config["x_res"],
1 * config["framestack"]),
dtype=np.float32)
elif config["use_sensor"] == 'use_rgb':
image_space = Box(0,
255,
shape=(config["y_res"], config["x_res"],
3 * config["framestack"]),
dtype=np.uint8)
elif config["use_sensor"] == 'use_2rgb':
image_space = Box(0,
255,
shape=(config["y_res"], config["x_res"],
2 * 3 * config["framestack"]),
dtype=np.uint8)
# The Observation Space
self.observation_space = Tuple([
image_space,
Discrete(len(COMMANDS_ENUM)), # next_command
Box(-128.0, 128.0, shape=(2, ),
dtype=np.float32) # forward_speed, dist to goal
])
# TODO(ekl) this isn't really a proper gym spec
self._spec = lambda: None
self._spec.id = "Carla-v0"
self.server_port = None
self.server_process = None
self.client = None
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.prev_image = None
self.episode_id = None
self.measurements_file = None
self.weather = None
self.scenario = None
self.start_pos = None
self.end_pos = None
self.start_coord = None
self.end_coord = None
self.last_obs = None
self.cnt1 = None
self.displacement = None
self.next_command = "LANE_FOLLOW"
def init_server(self):
print("Initializing new Carla server...")
# Create a new server process and start the client.
self.server_port = random.randint(10000, 60000)
self.server_process = subprocess.Popen(
[
SERVER_BINARY,
self.config["server_map"],
"-windowed",
"-ResX=480",
"-ResY=360",
"-carla-server",
"-benchmark -fps=10", # "-benchmark -fps=10": to run the simulation at a fixed time-step of 0.1 seconds
"-carla-world-port={}".format(self.server_port)
],
preexec_fn=os.setsid,
stdout=open(os.devnull, "w")
) # ResourceWarning: unclosed file <_io.TextIOWrapper name='/dev/null' mode='w' encoding='UTF-8'>
live_carla_processes.add(os.getpgid(self.server_process.pid))
for i in range(RETRIES_ON_ERROR):
try:
self.client = CarlaClient("localhost", self.server_port)
return self.client.connect()
except Exception as e:
print("Error connecting: {}, attempt {}".format(e, i))
time.sleep(2)
def clear_server_state(self):
print("Clearing Carla server state")
try:
if self.client:
self.client.disconnect()
self.client = None
except Exception as e:
print("Error disconnecting client: {}".format(e))
pass
if self.server_process:
pgid = os.getpgid(self.server_process.pid)
os.killpg(pgid, signal.SIGKILL)
live_carla_processes.remove(pgid)
self.server_port = None
self.server_process = None
def __del__(
self
): # the __del__ method will be called when the instance of the class is deleted.(memory is freed.)
self.clear_server_state()
def reset(self):
error = None
for _ in range(RETRIES_ON_ERROR):
try:
if not self.server_process:
self.init_server()
return self._reset()
except Exception as e:
print("Error during reset: {}".format(traceback.format_exc()))
self.clear_server_state()
error = e
raise error
# @set_timeout(15)
def _reset(self):
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.prev_image = None
self.episode_id = datetime.today().strftime("%Y-%m-%d_%H-%M-%S_%f")
self.measurements_file = None
self.pre_intersection = np.array([0.0, 0.0])
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
self.scenario = random.choice(self.config["scenarios"])
assert self.scenario["city"] == self.city, (self.scenario, self.city)
self.weather = random.choice(self.scenario["weather_distribution"])
settings.set(
SynchronousMode=True,
# ServerTimeOut=10000, # CarlaSettings: no key named 'ServerTimeOut'
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self.scenario["num_vehicles"],
NumberOfPedestrians=self.scenario["num_pedestrians"],
WeatherId=self.weather)
settings.randomize_seeds()
if self.config["use_sensor"] == 'use_semantic':
camera0 = Camera("CameraSemantic",
PostProcessing="SemanticSegmentation")
camera0.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
# camera0.set_position(30, 0, 130)
camera0.set(FOV=120)
camera0.set_position(2.0, 0.0, 1.4)
camera0.set_rotation(0.0, 0.0, 0.0)
settings.add_sensor(camera0)
if self.config["use_sensor"] in ['use_depth', 'use_logdepth']:
camera1 = Camera("CameraDepth", PostProcessing="Depth")
camera1.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
# camera1.set_position(30, 0, 130)
camera1.set(FOV=120)
camera1.set_position(2.0, 0.0, 1.4)
camera1.set_rotation(0.0, 0.0, 0.0)
settings.add_sensor(camera1)
if self.config["use_sensor"] == 'use_rgb':
camera2 = Camera("CameraRGB")
camera2.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
# camera2.set_position(30, 0, 130)
# camera2.set_position(0.3, 0.0, 1.3)
camera2.set(FOV=120)
camera2.set_position(2.0, 0.0, 1.4)
camera2.set_rotation(0.0, 0.0, 0.0)
settings.add_sensor(camera2)
if self.config["use_sensor"] == 'use_2rgb':
camera_l = Camera("CameraRGB_L")
camera_l.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
camera_l.set(FOV=120)
camera_l.set_position(2.0, -0.1, 1.4)
camera_l.set_rotation(0.0, 0.0, 0.0)
settings.add_sensor(camera_l)
camera_r = Camera("CameraRGB_R")
camera_r.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
camera_r.set(FOV=120)
camera_r.set_position(2.0, 0.1, 1.4)
camera_r.set_rotation(0.0, 0.0, 0.0)
settings.add_sensor(camera_r)
# Setup start and end positions
scene = self.client.load_settings(settings)
self.positions = positions = scene.player_start_spots
self.start_pos = positions[self.scenario["start_pos_id"]]
self.pre_pos = self.start_pos.location
self.cnt1 = 0
self.displacement = 1000.0
self.end_pos = positions[self.scenario["end_pos_id"]]
self.start_coord = [
self.start_pos.location.x // 1, self.start_pos.location.y // 1
]
self.end_coord = [
self.end_pos.location.x // 1, self.end_pos.location.y // 1
]
print("Start pos {} ({}), end {} ({})".format(
self.scenario["start_pos_id"], self.start_coord,
self.scenario["end_pos_id"], self.end_coord))
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print("Starting new episode...")
self.client.start_episode(self.scenario["start_pos_id"])
# remove the vehicle dropping when starting a new episode.
cnt = 1
z1 = 0
zero_control = VehicleControl()
while (cnt < 3):
self.client.send_control(
zero_control
) # VehicleControl().steer = 0, VehicleControl().throttle = 0, VehicleControl().reverse = False
z0 = z1
z1 = self.client.read_data(
)[0].player_measurements.transform.location.z
print(z1)
if z0 - z1 == 0:
cnt += 1
print('Starting new episode done.\n')
# Process observations: self._read_observation() returns image and py_measurements.
image, py_measurements = self._read_observation()
self.prev_measurement = py_measurements
# self.current_heading = self.pre_heading = np.array([py_measurements["x_orient"], py_measurements["y_orient"]])
# self.angular_speed = 0.0
self.pre_heading_degree = self.current_heading_degree = py_measurements[
"current_heading_degree"]
self.angular_speed_degree = np.array(0.0)
return self.encode_obs(self.preprocess_image(image),
py_measurements), py_measurements
def encode_obs(self, image, py_measurements):
assert self.config["framestack"] in [1, 2]
prev_image = self.prev_image
self.prev_image = image
if prev_image is None:
prev_image = image
if self.config["framestack"] == 2:
image = np.concatenate([prev_image, image], axis=2)
# obs = (image, COMMAND_ORDINAL[py_measurements["next_command"]], [
# py_measurements["forward_speed"],
# py_measurements["distance_to_goal"]
# ])
obs = image
self.last_obs = obs
return obs
def step(self, action):
try:
obs = self._step(action)
return obs
except Exception:
print("Error during step, terminating episode early",
traceback.format_exc())
self.clear_server_state()
return (self.last_obs, 0.0, True, self.prev_measurement)
def delta_degree(self, deltaxy):
return deltaxy if np.abs(
deltaxy) < 180 else deltaxy - np.sign(deltaxy) * 360
# image, py_measurements = self._read_observation() ---> self.preprocess_image(image) ---> step observation output
# @set_timeout(10)
def _step(self, action):
if self.config["discrete_actions"]:
action = DISCRETE_ACTIONS[int(action)] # Carla action is 2D.
assert len(action) == 2, "Invalid action {}".format(action)
if self.enable_autopilot:
action[
0] = self.autopilot.brake if self.autopilot.brake < 0 else self.autopilot.throttle
action[1] = self.autopilot.steer
if self.config["squash_action_logits"]:
forward = 2 * float(sigmoid(action[0]) - 0.5)
throttle = float(np.clip(forward, 0, 1))
brake = float(np.abs(np.clip(forward, -1, 0)))
steer = 2 * float(sigmoid(action[1]) - 0.5)
else:
throttle = float(np.clip(action[0], 0, 1))
brake = float(np.abs(np.clip(action[0], -1, 0)))
steer = float(np.clip(action[1], -1, 1))
# reverse and hand_brake are disabled.
reverse = False
hand_brake = False
if self.config["verbose"]:
print("steer", steer, "throttle", throttle, "brake", brake,
"reverse", reverse)
# print(self.client)
self.client.send_control(steer=steer,
throttle=throttle,
brake=brake,
hand_brake=hand_brake,
reverse=reverse)
# Process observations: self._read_observation() returns image and py_measurements.
image, py_measurements = self._read_observation()
if self.config["verbose"]:
print("Next command", py_measurements["next_command"])
print("Next command", py_measurements["next_command"])
print("dist_to_intersection:", py_measurements["dist_to_intersection"])
if type(action) is np.ndarray:
py_measurements["action"] = [float(a) for a in action]
else:
py_measurements["action"] = action
py_measurements["control"] = {
"steer": steer,
"throttle": throttle,
"brake": brake,
"reverse": reverse,
"hand_brake": hand_brake,
}
# compute angular_speed
self.current_heading_degree = py_measurements["current_heading_degree"]
self.angular_speed_degree = np.array(
self.delta_degree(self.current_heading_degree -
self.pre_heading_degree))
self.pre_heading_degree = py_measurements["current_heading_degree"]
# compute reward
reward = compute_reward(self, self.prev_measurement, py_measurements)
self.total_reward += reward
py_measurements["reward"] = reward
py_measurements["total_reward"] = self.total_reward
# done or not
# done = False
# done = self.cnt1 > 50 and (py_measurements["collision_vehicles"] or py_measurements["collision_pedestrians"] or py_measurements["collision_other"] or self.displacement < 0.5)
done = self.cnt1 > 50 and self.displacement < 0.2
# done = (self.num_steps > self.scenario["max_steps"]
# or py_measurements["next_command"] == "REACH_GOAL" or py_measurements["intersection_offroad"] or py_measurements["intersection_otherlane"]
# or (self.config["early_terminate_on_collision"]
# and collided_done(py_measurements)))
py_measurements["done"] = done
self.prev_measurement = py_measurements
# Write out measurements to file
if self.config["verbose"] and CARLA_OUT_PATH:
if not self.measurements_file:
self.measurements_file = open(
os.path.join(
CARLA_OUT_PATH,
"measurements_{}.json".format(self.episode_id)), "w")
self.measurements_file.write(json.dumps(py_measurements))
self.measurements_file.write("\n")
if done:
self.measurements_file.close()
self.measurements_file = None
if self.config["convert_images_to_video"]:
self.images_to_video()
self.num_steps += 1
image = self.preprocess_image(image)
return (self.encode_obs(image, py_measurements), reward, done,
py_measurements)
def images_to_video(self):
videos_dir = os.path.join(CARLA_OUT_PATH, "Videos")
if not os.path.exists(videos_dir):
os.makedirs(videos_dir)
ffmpeg_cmd = (
"ffmpeg -loglevel -8 -r 60 -f image2 -s {x_res}x{y_res} "
"-start_number 0 -i "
"{img}_%04d.jpg -vcodec libx264 {vid}.mp4 && rm -f {img}_*.jpg "
).format(x_res=self.config["render_x_res"],
y_res=self.config["render_y_res"],
vid=os.path.join(videos_dir, self.episode_id),
img=os.path.join(CARLA_OUT_PATH, "CameraRGB",
self.episode_id))
print("Executing ffmpeg command", ffmpeg_cmd)
subprocess.call(ffmpeg_cmd, shell=True)
def preprocess_image(self, image):
if self.config[
"use_sensor"] == 'use_semantic': # image.data: uint8(0 ~ 12)
data = image.data * 21 # data: uint8(0 ~ 255)
data = data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 1)
data = cv2.resize(data,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = np.expand_dims(
data, 2) # data: uint8(0 ~ 255), shape(y_res, x_res, 1)
elif self.config["use_sensor"] == 'use_depth': # depth: float64(0 ~ 1)
# data = (image.data - 0.5) * 2
data = image.data * 255 # data: float64(0 ~ 255)
data = data.reshape(self.config["render_y_res"],
self.config["render_x_res"],
1) # shape(render_y_res,render_x_res,1)
data = cv2.resize(
data, (self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA) # shape(y_res, x_res)
data = np.expand_dims(
data, 2) # data: float64(0 ~ 255), shape(y_res, x_res, 1)
elif self.config[
"use_sensor"] == 'use_logdepth': # depth: float64(0 ~ 1)
data = (np.log(image.data) +
7.0) * 255.0 / 7.0 # data: float64(0 ~ 255)
data = data.reshape(self.config["render_y_res"],
self.config["render_x_res"],
1) # shape(render_y_res,render_x_res,1)
data = cv2.resize(
data, (self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA) # shape(y_res, x_res)
data = np.expand_dims(
data, 2) # data: float64(0 ~ 255), shape(y_res, x_res, 1)
elif self.config["use_sensor"] == 'use_rgb':
data = image.data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 3)
data = cv2.resize(
data,
(self.config["x_res"], self.config["y_res"]
), # data: uint8(0 ~ 255), shape(y_res, x_res, 3)
interpolation=cv2.INTER_AREA)
# data = (data.astype(np.float32) - 128) / 128
elif self.config["use_sensor"] == 'use_2rgb':
data_l, data_r = image[0].data, image[0].data
data_l = data_l.reshape(self.config["render_y_res"],
self.config["render_x_res"], 3)
data_r = data_r.reshape(self.config["render_y_res"],
self.config["render_x_res"], 3)
data_l = cv2.resize(data_l,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data_r = cv2.resize(data_r,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = np.concatenate(
(data_l, data_r),
axis=2) # data: uint8(0 ~ 255), shape(y_res, x_res, 6)
return data
def _read_observation(self):
# Read the data produced by the server this frame.
measurements, sensor_data = self.client.read_data()
if self.enable_autopilot:
self.autopilot = measurements.player_measurements.autopilot_control
# Print some of the measurements.
if self.config["verbose"]:
print_measurements(measurements)
observation = None
if self.config["use_sensor"] == 'use_semantic':
camera_name = "CameraSemantic"
elif self.config["use_sensor"] in ['use_depth', 'use_logdepth']:
camera_name = "CameraDepth"
elif self.config["use_sensor"] == 'use_rgb':
camera_name = "CameraRGB"
elif self.config["use_sensor"] == 'use_2rgb':
camera_name = ["CameraRGB_L", "CameraRGB_R"]
# for name, image in sensor_data.items():
# if name == camera_name:
# observation = image
if camera_name == ["CameraRGB_L", "CameraRGB_R"]:
observation = [
sensor_data["CameraRGB_L"], sensor_data["CameraRGB_R"]
]
else:
observation = sensor_data[camera_name]
cur = measurements.player_measurements
if self.config["enable_planner"]:
# modify next_command
current_pos = np.array([cur.transform.location.x, cur.transform.location.y])\
+ np.array([cur.transform.orientation.x, cur.transform.orientation.y]) * 5.0
dist_intersection_current_pos = np.linalg.norm(
self.intersections_pos[:, :2] - current_pos, axis=1)
is_near_intersection = np.min(dist_intersection_current_pos) < 18.0
if not is_near_intersection:
self.next_command = "LANE_FOLLOW"
# goal_heading
if is_near_intersection:
cur_intersection = self.intersections_pos[
dist_intersection_current_pos.argmin(), :2]
self.dist_to_intersection = np.linalg.norm(cur_intersection -
current_pos)
else:
self.goal_heading_degree = 0.0
self.dist_to_intersection = 1000.0
if is_near_intersection and np.linalg.norm(self.pre_intersection -
cur_intersection) > 0.1:
self.next_command = COMMANDS_ENUM[self.planner.get_next_command(
[cur.transform.location.x, cur.transform.location.y, GROUND_Z], \
[cur.transform.orientation.x, cur.transform.orientation.y, GROUND_Z], \
[self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], \
[self.end_pos.orientation.x, self.end_pos.orientation.y, GROUND_Z] \
)]
if self.next_command == "LANE_FOLLOW":
self.next_command = random.choice(
["GO_STRAIGHT", "TURN_LEFT", "TURN_RIGHT"])
cur_heading0 = cur_intersection - current_pos
cur_heading_1 = cur_heading0 / np.linalg.norm(cur_heading0)
cur_heading_degree = self.headings_degree[np.linalg.norm(
cur_heading_1 - self.headings, axis=1).argmin()]
self.goal_heading_degree = self.delta_degree(
cur_heading_degree + self.lrs_degree[self.next_command])
self.pre_intersection = cur_intersection
else:
self.next_command = "LANE_FOLLOW"
if self.next_command == "REACH_GOAL":
distance_to_goal = 0.0 # avoids crash in planner
self.end_pos = self.positions[random.choice(
self.config["scenarios"])['end_pos_id']]
# elif self.config["enable_planner"]:
# distance_to_goal = self.planner.get_shortest_path_distance([
# cur.transform.location.x, cur.transform.location.y, GROUND_Z
# ], [
# cur.transform.orientation.x, cur.transform.orientation.y,
# GROUND_Z
# ], [self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [
# self.end_pos.orientation.x, self.end_pos.orientation.y,
# GROUND_Z
# ])
else:
distance_to_goal = -1
distance_to_goal_euclidean = float(
np.linalg.norm([ # default norm: L2 norm
cur.transform.location.x - self.end_pos.location.x,
cur.transform.location.y - self.end_pos.location.y
]))
# displacement
if self.cnt1 > 70 and self.cnt1 % 30 == 0:
self.displacement = float(
np.linalg.norm([
cur.transform.location.x - self.pre_pos.x,
cur.transform.location.y - self.pre_pos.y
]))
self.pre_pos = cur.transform.location
self.cnt1 += 1
py_measurements = {
"episode_id": self.episode_id,
"step": self.num_steps,
"x": cur.transform.location.x,
"y": cur.transform.location.y,
"x_orient": cur.transform.orientation.x,
"y_orient": cur.transform.orientation.y,
"forward_speed": np.array(cur.forward_speed), ###
"distance_to_goal": distance_to_goal,
"distance_to_goal_euclidean": distance_to_goal_euclidean,
"collision_vehicles": cur.collision_vehicles,
"collision_pedestrians": cur.collision_pedestrians,
"collision_other": cur.collision_other,
"collided": collision_(cur), ###
"intersection_offroad": np.array(cur.intersection_offroad), ###
"intersection_otherlane":
np.array(cur.intersection_otherlane), ###
"weather": self.weather,
"map": self.config["server_map"],
"start_coord": self.start_coord,
"end_coord": self.end_coord,
"current_scenario": self.scenario,
"x_res": self.config["x_res"],
"y_res": self.config["y_res"],
"num_vehicles": self.scenario["num_vehicles"],
"num_pedestrians": self.scenario["num_pedestrians"],
"max_steps": self.scenario["max_steps"],
"next_command": self.next_command,
"next_command_id": COMMAND_ORDINAL[self.next_command],
"displacement": self.displacement,
"is_near_intersection": is_near_intersection,
"goal_heading_degree": self.goal_heading_degree,
"angular_speed_degree": self.angular_speed_degree, ###
"current_heading_degree":
cur.transform.rotation.yaw, # left-, right+, (-180 ~ 180) degrees
"dist_to_intersection": self.dist_to_intersection
}
if CARLA_OUT_PATH and self.config["log_images"]:
for name, image in sensor_data.items():
out_dir = os.path.join(CARLA_OUT_PATH, name)
if not os.path.exists(out_dir):
os.makedirs(out_dir)
out_file = os.path.join(
out_dir, "{}_{:>04}.jpg".format(self.episode_id,
self.num_steps))
scipy.misc.imsave(out_file,
image.data) # image.data without preprocess.
assert observation is not None, sensor_data
return observation, py_measurements
class CarlaEnvironment(Environment):
def __init__(self, level: LevelSelection, seed: int, frame_skip: int,
human_control: bool, custom_reward_threshold: Union[int,
float],
visualization_parameters: VisualizationParameters,
server_height: int, server_width: int, camera_height: int,
camera_width: int, verbose: bool, config: str,
episode_max_time: int, allow_braking: bool,
quality: CarlaEnvironmentParameters.Quality,
cameras: List[CameraTypes], weather_id: List[int],
experiment_path: str, **kwargs):
super().__init__(level, seed, frame_skip, human_control,
custom_reward_threshold, visualization_parameters)
# server configuration
self.server_height = server_height
self.server_width = server_width
self.port = get_open_port()
self.host = 'localhost'
self.map = self.env_id
self.experiment_path = experiment_path
# client configuration
self.verbose = verbose
self.quality = quality
self.cameras = cameras
self.weather_id = weather_id
self.episode_max_time = episode_max_time
self.allow_braking = allow_braking
self.camera_width = camera_width
self.camera_height = camera_height
# state space
self.state_space = StateSpace({
"measurements":
VectorObservationSpace(
4, measurements_names=["forward_speed", "x", "y", "z"])
})
for camera in self.cameras:
self.state_space[camera.value] = ImageObservationSpace(
shape=np.array([self.camera_height, self.camera_width, 3]),
high=255)
# setup server settings
self.config = config
if self.config:
# load settings from file
with open(self.config, 'r') as fp:
self.settings = fp.read()
else:
# hard coded settings
self.settings = CarlaSettings()
self.settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=15,
NumberOfPedestrians=30,
WeatherId=random.choice(
force_list(self.weather_id)),
QualityLevel=self.quality.value,
SeedVehicles=seed,
SeedPedestrians=seed)
if seed is None:
self.settings.randomize_seeds()
self.settings = self._add_cameras(self.settings, self.cameras,
self.camera_width,
self.camera_height)
# open the server
self.server = self._open_server()
logging.disable(40)
# open the client
self.game = CarlaClient(self.host, self.port, timeout=99999999)
self.game.connect()
scene = self.game.load_settings(self.settings)
# get available start positions
positions = scene.player_start_spots
self.num_pos = len(positions)
self.iterator_start_positions = 0
# action space
self.action_space = BoxActionSpace(shape=2,
low=np.array([-1, -1]),
high=np.array([1, 1]))
# human control
if self.human_control:
# convert continuous action space to discrete
self.steering_strength = 0.5
self.gas_strength = 1.0
self.brake_strength = 0.5
self.action_space = PartialDiscreteActionSpaceMap(
target_actions=[[0., 0.], [0., -self.steering_strength],
[0., self.steering_strength],
[self.gas_strength, 0.],
[-self.brake_strength, 0],
[self.gas_strength, -self.steering_strength],
[self.gas_strength, self.steering_strength],
[self.brake_strength, -self.steering_strength],
[self.brake_strength, self.steering_strength]],
descriptions=[
'NO-OP', 'TURN_LEFT', 'TURN_RIGHT', 'GAS', 'BRAKE',
'GAS_AND_TURN_LEFT', 'GAS_AND_TURN_RIGHT',
'BRAKE_AND_TURN_LEFT', 'BRAKE_AND_TURN_RIGHT'
])
# map keyboard keys to actions
for idx, action in enumerate(self.action_space.descriptions):
for key in key_map.keys():
if action == key:
self.key_to_action[key_map[key]] = idx
self.num_speedup_steps = 30
# measurements
self.autopilot = None
# env initialization
self.reset_internal_state(True)
# render
if self.is_rendered:
image = self.get_rendered_image()
self.renderer.create_screen(image.shape[1], image.shape[0])
def _add_cameras(self, settings, cameras, camera_width, camera_height):
# add a front facing camera
if CameraTypes.FRONT in cameras:
camera = Camera(CameraTypes.FRONT.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, 0, 0)
settings.add_sensor(camera)
# add a left facing camera
if CameraTypes.LEFT in cameras:
camera = Camera(CameraTypes.LEFT.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, -30, 0)
settings.add_sensor(camera)
# add a right facing camera
if CameraTypes.RIGHT in cameras:
camera = Camera(CameraTypes.RIGHT.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, 30, 0)
settings.add_sensor(camera)
# add a front facing depth camera
if CameraTypes.DEPTH in cameras:
camera = Camera(CameraTypes.DEPTH.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, 30, 0)
camera.PostProcessing = 'Depth'
settings.add_sensor(camera)
# add a front facing semantic segmentation camera
if CameraTypes.SEGMENTATION in cameras:
camera = Camera(CameraTypes.SEGMENTATION.value)
camera.set_image_size(camera_width, camera_height)
camera.set_position(0.2, 0, 1.3)
camera.set_rotation(8, 30, 0)
camera.PostProcessing = 'SemanticSegmentation'
settings.add_sensor(camera)
return settings
def _open_server(self):
log_path = path.join(
self.experiment_path if self.experiment_path is not None else '.',
'logs', "CARLA_LOG_{}.txt".format(self.port))
if not os.path.exists(os.path.dirname(log_path)):
os.makedirs(os.path.dirname(log_path))
with open(log_path, "wb") as out:
cmd = [
path.join(environ.get('CARLA_ROOT'),
'CarlaUE4.sh'), self.map, "-benchmark",
"-carla-server", "-fps={}".format(30 / self.frame_skip),
"-world-port={}".format(self.port),
"-windowed -ResX={} -ResY={}".format(self.server_width,
self.server_height),
"-carla-no-hud"
]
if self.config:
cmd.append("-carla-settings={}".format(self.config))
p = subprocess.Popen(cmd, stdout=out, stderr=out)
return p
def _close_server(self):
os.killpg(os.getpgid(self.server.pid), signal.SIGKILL)
def _update_state(self):
# get measurements and observations
measurements = []
while type(measurements) == list:
measurements, sensor_data = self.game.read_data()
self.state = {}
for camera in self.cameras:
self.state[camera.value] = sensor_data[camera.value].data
self.location = [
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
]
is_collision = measurements.player_measurements.collision_vehicles != 0 \
or measurements.player_measurements.collision_pedestrians != 0 \
or measurements.player_measurements.collision_other != 0
speed_reward = measurements.player_measurements.forward_speed - 1
if speed_reward > 30.:
speed_reward = 30.
self.reward = speed_reward \
- (measurements.player_measurements.intersection_otherlane * 5) \
- (measurements.player_measurements.intersection_offroad * 5) \
- is_collision * 100 \
- np.abs(self.control.steer) * 10
# update measurements
self.measurements = [measurements.player_measurements.forward_speed
] + self.location
self.autopilot = measurements.player_measurements.autopilot_control
# action_p = ['%.2f' % member for member in [self.control.throttle, self.control.steer]]
# screen.success('REWARD: %.2f, ACTIONS: %s' % (self.reward, action_p))
if (measurements.game_timestamp >=
self.episode_max_time) or is_collision:
# screen.success('EPISODE IS DONE. GameTime: {}, Collision: {}'.format(str(measurements.game_timestamp),
# str(is_collision)))
self.done = True
self.state['measurements'] = self.measurements
def _take_action(self, action):
self.control = VehicleControl()
self.control.throttle = np.clip(action[0], 0, 1)
self.control.steer = np.clip(action[1], -1, 1)
self.control.brake = np.abs(np.clip(action[0], -1, 0))
if not self.allow_braking:
self.control.brake = 0
self.control.hand_brake = False
self.control.reverse = False
self.game.send_control(self.control)
def _restart_environment_episode(self, force_environment_reset=False):
self.iterator_start_positions += 1
if self.iterator_start_positions >= self.num_pos:
self.iterator_start_positions = 0
try:
self.game.start_episode(self.iterator_start_positions)
except:
self.game.connect()
self.game.start_episode(self.iterator_start_positions)
# start the game with some initial speed
for i in range(self.num_speedup_steps):
self._take_action([1.0, 0])
def get_rendered_image(self) -> np.ndarray:
"""
Return a numpy array containing the image that will be rendered to the screen.
This can be different from the observation. For example, mujoco's observation is a measurements vector.
:return: numpy array containing the image that will be rendered to the screen
"""
image = [self.state[camera.value] for camera in self.cameras]
image = np.vstack(image)
return image
class CarlaEnv(gym.Env):
def __init__(self, config=ENV_CONFIG):
self.config = config
self.city = self.config["server_map"].split("/")[-1]
if self.config["enable_planner"]:
self.planner = Planner(self.city)
if config["discrete_actions"]:
self.action_space = Discrete(len(DISCRETE_ACTIONS))
else:
self.action_space = Box(-1.0, 1.0, shape=(2, ), dtype=np.float32)
if config["use_depth_camera"]:
image_space = Box(
-1.0,
1.0,
shape=(config["y_res"], config["x_res"],
1 * config["framestack"]),
dtype=np.float32)
else:
image_space = Box(
0,
255,
shape=(config["y_res"], config["x_res"],
3 * config["framestack"]),
dtype=np.uint8)
self.observation_space = Tuple( # forward_speed, dist to goal
[
image_space,
Discrete(len(COMMANDS_ENUM)), # next_command
Box(-128.0, 128.0, shape=(2, ), dtype=np.float32)
])
# TODO(ekl) this isn't really a proper gym spec
self._spec = lambda: None
self._spec.id = "Carla-v0"
self.server_port = None
self.server_process = None
self.client = None
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.prev_image = None
self.episode_id = None
self.measurements_file = None
self.weather = None
self.scenario = None
self.start_pos = None
self.end_pos = None
self.start_coord = None
self.end_coord = None
self.last_obs = None
def init_server(self):
print("Initializing new Carla server...")
# Create a new server process and start the client.
self.server_port = random.randint(10000, 60000)
self.server_process = subprocess.Popen(
[
SERVER_BINARY, self.config["server_map"], "-windowed",
"-ResX=400", "-ResY=300", "-carla-server",
"-carla-world-port={}".format(self.server_port)
],
preexec_fn=os.setsid,
stdout=open(os.devnull, "w"))
live_carla_processes.add(os.getpgid(self.server_process.pid))
for i in range(RETRIES_ON_ERROR):
try:
self.client = CarlaClient("localhost", self.server_port)
return self.client.connect()
except Exception as e:
print("Error connecting: {}, attempt {}".format(e, i))
time.sleep(2)
def clear_server_state(self):
print("Clearing Carla server state")
try:
if self.client:
self.client.disconnect()
self.client = None
except Exception as e:
print("Error disconnecting client: {}".format(e))
pass
if self.server_process:
pgid = os.getpgid(self.server_process.pid)
os.killpg(pgid, signal.SIGKILL)
live_carla_processes.remove(pgid)
self.server_port = None
self.server_process = None
def __del__(self):
self.clear_server_state()
def reset(self):
error = None
for _ in range(RETRIES_ON_ERROR):
try:
if not self.server_process:
self.init_server()
return self._reset()
except Exception as e:
print("Error during reset: {}".format(traceback.format_exc()))
self.clear_server_state()
error = e
raise error
def _reset(self):
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.prev_image = None
self.episode_id = datetime.today().strftime("%Y-%m-%d_%H-%M-%S_%f")
self.measurements_file = None
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
self.scenario = random.choice(self.config["scenarios"])
assert self.scenario["city"] == self.city, (self.scenario, self.city)
self.weather = random.choice(self.scenario["weather_distribution"])
settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self.scenario["num_vehicles"],
NumberOfPedestrians=self.scenario["num_pedestrians"],
WeatherId=self.weather)
settings.randomize_seeds()
if self.config["use_depth_camera"]:
camera1 = Camera("CameraDepth", PostProcessing="Depth")
camera1.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
camera1.set_position(30, 0, 130)
settings.add_sensor(camera1)
camera2 = Camera("CameraRGB")
camera2.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
camera2.set_position(30, 0, 130)
settings.add_sensor(camera2)
# Setup start and end positions
scene = self.client.load_settings(settings)
positions = scene.player_start_spots
self.start_pos = positions[self.scenario["start_pos_id"]]
self.end_pos = positions[self.scenario["end_pos_id"]]
self.start_coord = [
self.start_pos.location.x // 100, self.start_pos.location.y // 100
]
self.end_coord = [
self.end_pos.location.x // 100, self.end_pos.location.y // 100
]
print("Start pos {} ({}), end {} ({})".format(
self.scenario["start_pos_id"], self.start_coord,
self.scenario["end_pos_id"], self.end_coord))
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print("Starting new episode...")
self.client.start_episode(self.scenario["start_pos_id"])
image, py_measurements = self._read_observation()
self.prev_measurement = py_measurements
return self.encode_obs(self.preprocess_image(image), py_measurements)
def encode_obs(self, image, py_measurements):
assert self.config["framestack"] in [1, 2]
prev_image = self.prev_image
self.prev_image = image
if prev_image is None:
prev_image = image
if self.config["framestack"] == 2:
image = np.concatenate([prev_image, image], axis=2)
obs = (image, COMMAND_ORDINAL[py_measurements["next_command"]], [
py_measurements["forward_speed"],
py_measurements["distance_to_goal"]
])
self.last_obs = obs
return obs
def step(self, action):
try:
obs = self._step(action)
return obs
except Exception:
print("Error during step, terminating episode early",
traceback.format_exc())
self.clear_server_state()
return (self.last_obs, 0.0, True, {})
def _step(self, action):
if self.config["discrete_actions"]:
action = DISCRETE_ACTIONS[int(action)]
assert len(action) == 2, "Invalid action {}".format(action)
if self.config["squash_action_logits"]:
forward = 2 * float(sigmoid(action[0]) - 0.5)
throttle = float(np.clip(forward, 0, 1))
brake = float(np.abs(np.clip(forward, -1, 0)))
steer = 2 * float(sigmoid(action[1]) - 0.5)
else:
throttle = float(np.clip(action[0], 0, 1))
brake = float(np.abs(np.clip(action[0], -1, 0)))
steer = float(np.clip(action[1], -1, 1))
reverse = False
hand_brake = False
if self.config["verbose"]:
print("steer", steer, "throttle", throttle, "brake", brake,
"reverse", reverse)
self.client.send_control(
steer=steer,
throttle=throttle,
brake=brake,
hand_brake=hand_brake,
reverse=reverse)
# Process observations
image, py_measurements = self._read_observation()
if self.config["verbose"]:
print("Next command", py_measurements["next_command"])
if type(action) is np.ndarray:
py_measurements["action"] = [float(a) for a in action]
else:
py_measurements["action"] = action
py_measurements["control"] = {
"steer": steer,
"throttle": throttle,
"brake": brake,
"reverse": reverse,
"hand_brake": hand_brake,
}
reward = compute_reward(self, self.prev_measurement, py_measurements)
self.total_reward += reward
py_measurements["reward"] = reward
py_measurements["total_reward"] = self.total_reward
done = (self.num_steps > self.scenario["max_steps"]
or py_measurements["next_command"] == "REACH_GOAL"
or (self.config["early_terminate_on_collision"]
and collided_done(py_measurements)))
py_measurements["done"] = done
self.prev_measurement = py_measurements
# Write out measurements to file
if CARLA_OUT_PATH:
if not self.measurements_file:
self.measurements_file = open(
os.path.join(
CARLA_OUT_PATH,
"measurements_{}.json".format(self.episode_id)), "w")
self.measurements_file.write(json.dumps(py_measurements))
self.measurements_file.write("\n")
if done:
self.measurements_file.close()
self.measurements_file = None
if self.config["convert_images_to_video"]:
self.images_to_video()
self.num_steps += 1
image = self.preprocess_image(image)
return (self.encode_obs(image, py_measurements), reward, done,
py_measurements)
def images_to_video(self):
videos_dir = os.path.join(CARLA_OUT_PATH, "Videos")
if not os.path.exists(videos_dir):
os.makedirs(videos_dir)
ffmpeg_cmd = (
"ffmpeg -loglevel -8 -r 60 -f image2 -s {x_res}x{y_res} "
"-start_number 0 -i "
"{img}_%04d.jpg -vcodec libx264 {vid}.mp4 && rm -f {img}_*.jpg "
).format(
x_res=self.config["render_x_res"],
y_res=self.config["render_y_res"],
vid=os.path.join(videos_dir, self.episode_id),
img=os.path.join(CARLA_OUT_PATH, "CameraRGB", self.episode_id))
print("Executing ffmpeg command", ffmpeg_cmd)
subprocess.call(ffmpeg_cmd, shell=True)
def preprocess_image(self, image):
if self.config["use_depth_camera"]:
assert self.config["use_depth_camera"]
data = (image.data - 0.5) * 2
data = data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 1)
data = cv2.resize(
data, (self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = np.expand_dims(data, 2)
else:
data = image.data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 3)
data = cv2.resize(
data, (self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = (data.astype(np.float32) - 128) / 128
return data
def _read_observation(self):
# Read the data produced by the server this frame.
measurements, sensor_data = self.client.read_data()
# Print some of the measurements.
if self.config["verbose"]:
print_measurements(measurements)
observation = None
if self.config["use_depth_camera"]:
camera_name = "CameraDepth"
else:
camera_name = "CameraRGB"
for name, image in sensor_data.items():
if name == camera_name:
observation = image
cur = measurements.player_measurements
if self.config["enable_planner"]:
next_command = COMMANDS_ENUM[self.planner.get_next_command(
[cur.transform.location.x, cur.transform.location.y, GROUND_Z],
[
cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z
],
[self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [
self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z
])]
else:
next_command = "LANE_FOLLOW"
if next_command == "REACH_GOAL":
distance_to_goal = 0.0 # avoids crash in planner
elif self.config["enable_planner"]:
distance_to_goal = self.planner.get_shortest_path_distance([
cur.transform.location.x, cur.transform.location.y, GROUND_Z
], [
cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z
], [self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [
self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z
]) / 100
else:
distance_to_goal = -1
distance_to_goal_euclidean = float(
np.linalg.norm([
cur.transform.location.x - self.end_pos.location.x,
cur.transform.location.y - self.end_pos.location.y
]) / 100)
py_measurements = {
"episode_id": self.episode_id,
"step": self.num_steps,
"x": cur.transform.location.x,
"y": cur.transform.location.y,
"x_orient": cur.transform.orientation.x,
"y_orient": cur.transform.orientation.y,
"forward_speed": cur.forward_speed,
"distance_to_goal": distance_to_goal,
"distance_to_goal_euclidean": distance_to_goal_euclidean,
"collision_vehicles": cur.collision_vehicles,
"collision_pedestrians": cur.collision_pedestrians,
"collision_other": cur.collision_other,
"intersection_offroad": cur.intersection_offroad,
"intersection_otherlane": cur.intersection_otherlane,
"weather": self.weather,
"map": self.config["server_map"],
"start_coord": self.start_coord,
"end_coord": self.end_coord,
"current_scenario": self.scenario,
"x_res": self.config["x_res"],
"y_res": self.config["y_res"],
"num_vehicles": self.scenario["num_vehicles"],
"num_pedestrians": self.scenario["num_pedestrians"],
"max_steps": self.scenario["max_steps"],
"next_command": next_command,
}
if CARLA_OUT_PATH and self.config["log_images"]:
for name, image in sensor_data.items():
out_dir = os.path.join(CARLA_OUT_PATH, name)
if not os.path.exists(out_dir):
os.makedirs(out_dir)
out_file = os.path.join(
out_dir, "{}_{:>04}.jpg".format(self.episode_id,
self.num_steps))
scipy.misc.imsave(out_file, image.data)
assert observation is not None, sensor_data
return observation, py_measurements
class CarlaEnvironmentWrapper(EnvironmentWrapper):
def __init__(self, tuning_parameters):
EnvironmentWrapper.__init__(self, tuning_parameters)
self.tp = tuning_parameters
# server configuration
self.server_height = self.tp.env.server_height
self.server_width = self.tp.env.server_width
self.port = get_open_port()
self.host = 'localhost'
self.map = CarlaLevel().get(self.tp.env.level)
# client configuration
self.verbose = self.tp.env.verbose
self.depth = self.tp.env.depth
self.stereo = self.tp.env.stereo
self.semantic_segmentation = self.tp.env.semantic_segmentation
self.height = self.server_height * (1 + int(self.depth) +
int(self.semantic_segmentation))
self.width = self.server_width * (1 + int(self.stereo))
self.size = (self.width, self.height)
self.config = self.tp.env.config
if self.config:
# load settings from file
with open(self.config, 'r') as fp:
self.settings = fp.read()
else:
# hard coded settings
self.settings = CarlaSettings()
self.settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=False,
NumberOfVehicles=15,
NumberOfPedestrians=30,
WeatherId=1)
self.settings.randomize_seeds()
# add cameras
camera = Camera('CameraRGB')
camera.set_image_size(self.width, self.height)
camera.set_position(200, 0, 140)
camera.set_rotation(0, 0, 0)
self.settings.add_sensor(camera)
# open the server
self.server = self._open_server()
logging.disable(40)
# open the client
self.game = CarlaClient(self.host, self.port, timeout=99999999)
self.game.connect()
scene = self.game.load_settings(self.settings)
# get available start positions
positions = scene.player_start_spots
self.num_pos = len(positions)
self.iterator_start_positions = 0
# action space
self.discrete_controls = False
self.action_space_size = 2
self.action_space_high = [1, 1]
self.action_space_low = [-1, -1]
self.action_space_abs_range = np.maximum(
np.abs(self.action_space_low), np.abs(self.action_space_high))
self.steering_strength = 0.5
self.gas_strength = 1.0
self.brake_strength = 0.5
self.actions = {
0: [0., 0.],
1: [0., -self.steering_strength],
2: [0., self.steering_strength],
3: [self.gas_strength, 0.],
4: [-self.brake_strength, 0],
5: [self.gas_strength, -self.steering_strength],
6: [self.gas_strength, self.steering_strength],
7: [self.brake_strength, -self.steering_strength],
8: [self.brake_strength, self.steering_strength]
}
self.actions_description = [
'NO-OP', 'TURN_LEFT', 'TURN_RIGHT', 'GAS', 'BRAKE',
'GAS_AND_TURN_LEFT', 'GAS_AND_TURN_RIGHT', 'BRAKE_AND_TURN_LEFT',
'BRAKE_AND_TURN_RIGHT'
]
for idx, action in enumerate(self.actions_description):
for key in key_map.keys():
if action == key:
self.key_to_action[key_map[key]] = idx
self.num_speedup_steps = 30
# measurements
self.measurements_size = (1, )
self.autopilot = None
# env initialization
self.reset(True)
# render
if self.is_rendered:
image = self.get_rendered_image()
self.renderer.create_screen(image.shape[1], image.shape[0])
def _open_server(self):
log_path = path.join(logger.experiments_path,
"CARLA_LOG_{}.txt".format(self.port))
with open(log_path, "wb") as out:
cmd = [
path.join(environ.get('CARLA_ROOT'),
'CarlaUE4.sh'), self.map, "-benchmark",
"-carla-server", "-fps=10", "-world-port={}".format(self.port),
"-windowed -ResX={} -ResY={}".format(self.server_width,
self.server_height),
"-carla-no-hud"
]
if self.config:
cmd.append("-carla-settings={}".format(self.config))
p = subprocess.Popen(cmd, stdout=out, stderr=out)
return p
def _close_server(self):
os.killpg(os.getpgid(self.server.pid), signal.SIGKILL)
def _update_state(self):
# get measurements and observations
measurements = []
while type(measurements) == list:
measurements, sensor_data = self.game.read_data()
self.location = (measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z)
is_collision = measurements.player_measurements.collision_vehicles != 0 \
or measurements.player_measurements.collision_pedestrians != 0 \
or measurements.player_measurements.collision_other != 0
speed_reward = measurements.player_measurements.forward_speed - 1
if speed_reward > 30.:
speed_reward = 30.
self.reward = speed_reward \
- (measurements.player_measurements.intersection_otherlane * 5) \
- (measurements.player_measurements.intersection_offroad * 5) \
- is_collision * 100 \
- np.abs(self.control.steer) * 10
# update measurements
self.state = {
'observation': sensor_data['CameraRGB'].data,
'measurements': [measurements.player_measurements.forward_speed],
}
self.autopilot = measurements.player_measurements.autopilot_control
# action_p = ['%.2f' % member for member in [self.control.throttle, self.control.steer]]
# screen.success('REWARD: %.2f, ACTIONS: %s' % (self.reward, action_p))
if (measurements.game_timestamp >=
self.tp.env.episode_max_time) or is_collision:
# screen.success('EPISODE IS DONE. GameTime: {}, Collision: {}'.format(str(measurements.game_timestamp),
# str(is_collision)))
self.done = True
def _take_action(self, action_idx):
if type(action_idx) == int:
action = self.actions[action_idx]
else:
action = action_idx
self.last_action_idx = action
self.control = VehicleControl()
self.control.throttle = np.clip(action[0], 0, 1)
self.control.steer = np.clip(action[1], -1, 1)
self.control.brake = np.abs(np.clip(action[0], -1, 0))
if not self.tp.env.allow_braking:
self.control.brake = 0
self.control.hand_brake = False
self.control.reverse = False
self.game.send_control(self.control)
def _restart_environment_episode(self, force_environment_reset=False):
self.iterator_start_positions += 1
if self.iterator_start_positions >= self.num_pos:
self.iterator_start_positions = 0
try:
self.game.start_episode(self.iterator_start_positions)
except:
self.game.connect()
self.game.start_episode(self.iterator_start_positions)
# start the game with some initial speed
state = None
for i in range(self.num_speedup_steps):
state = self.step([1.0, 0])['state']
self.state = state
return state
class CarlaEnv(object):
'''
An OpenAI Gym Environment for CARLA.
'''
def __init__(self,
obs_converter,
action_converter,
env_id,
random_seed=0,
exp_suite_name='TrainingSuite',
reward_class_name='RewardCarla',
host='127.0.0.1',
port=2000,
city_name='Town01',
subset=None,
video_every=100,
video_dir='./video/',
distance_for_success=2.0,
benchmark=False):
self.logger = get_carla_logger()
self.logger.info('Environment {} running in port {}'.format(
env_id, port))
self.host, self.port = host, port
self.id = env_id
self._obs_converter = obs_converter
self.observation_space = obs_converter.get_observation_space()
self._action_converter = action_converter
self.action_space = self._action_converter.get_action_space()
if benchmark:
self._experiment_suite = getattr(experiment_suites_benchmark,
exp_suite_name)(city_name)
else:
self._experiment_suite = getattr(experiment_suites,
exp_suite_name)(city_name, subset)
self._reward = getattr(rewards, reward_class_name)()
self._experiments = self._experiment_suite.get_experiments()
self.subset = subset
self._make_carla_client(host, port)
self._distance_for_success = distance_for_success
self._planner = Planner(city_name)
self.done = False
self.last_obs = None
self.last_distance_to_goal = None
self.last_direction = None
self.last_measurements = None
np.random.seed(random_seed)
self.video_every = video_every
self.video_dir = video_dir
self.video_writer = None
self._success = False
self._failure_timeout = False
self._failure_collision = False
self.benchmark = benchmark
self.benchmark_index = [0, 0, 0]
try:
if not os.path.isdir(self.video_dir):
os.makedirs(self.video_dir)
except OSError:
pass
self.steps = 0
self.num_episodes = 0
def step(self, action):
if self.done:
raise ValueError(
'self.done should always be False when calling step')
while True:
try:
# Send control
control = self._action_converter.action_to_control(
action, self.last_measurements)
self._client.send_control(control)
# Gather the observations (including measurements, sensor and directions)
measurements, sensor_data = self._client.read_data()
self.last_measurements = measurements
current_timestamp = measurements.game_timestamp
distance_to_goal = self._get_distance_to_goal(
measurements, self._target)
self.last_distance_to_goal = distance_to_goal
directions = self._get_directions(
measurements.player_measurements.transform, self._target)
self.last_direction = directions
obs = self._obs_converter.convert(measurements, sensor_data,
directions, self._target,
self.id)
if self.video_writer is not None and self.steps % 2 == 0:
self._raster_frame(sensor_data, measurements, directions,
obs)
self.last_obs = obs
except CameraException:
self.logger.debug('Camera Exception in step()')
obs = self.last_obs
distance_to_goal = self.last_distance_to_goal
current_timestamp = self.last_measurements.game_timestamp
except TCPConnectionError as e:
self.logger.debug(
'TCPConnectionError inside step(): {}'.format(e))
self.done = True
return self.last_obs, 0.0, True, {'carla-reward': 0.0}
break
# Check if terminal state
timeout = (current_timestamp -
self._initial_timestamp) > (self._time_out * 1000)
collision, _ = self._is_collision(measurements)
success = distance_to_goal < self._distance_for_success
if timeout:
self.logger.debug('Timeout')
self._failure_timeout = True
if collision:
self.logger.debug('Collision')
self._failure_collision = True
if success:
self.logger.debug('Success')
self.done = timeout or collision or success
# Get the reward
env_state = {
'timeout': timeout,
'collision': collision,
'success': success
}
reward = self._reward.get_reward(measurements, self._target,
self.last_direction, control,
env_state)
# Additional information
info = {'carla-reward': reward}
self.steps += 1
return obs, reward, self.done, info
def reset(self):
# Loop forever due to TCPConnectionErrors
while True:
try:
self._reward.reset_reward()
self.done = False
if self.video_writer is not None:
try:
self.video_writer.close()
except Exception as e:
self.logger.debug(
'Error when closing video writer in reset')
self.logger.error(e)
self.video_writer = None
if self.benchmark:
end_indicator = self._new_episode_benchmark()
if end_indicator is False:
return False
else:
self._new_episode()
# Hack: Try sleeping so that the server is ready. Reduces the number of TCPErrors
time.sleep(4)
# measurements, sensor_data = self._client.read_data()
self._client.send_control(VehicleControl())
measurements, sensor_data = self._client.read_data()
self._initial_timestamp = measurements.game_timestamp
self.last_measurements = measurements
self.last_distance_to_goal = self._get_distance_to_goal(
measurements, self._target)
directions = self._get_directions(
measurements.player_measurements.transform, self._target)
self.last_direction = directions
obs = self._obs_converter.convert(measurements, sensor_data,
directions, self._target,
self.id)
self.last_obs = obs
self.done = False
self._success = False
self._failure_timeout = False
self._failure_collision = False
return obs
except CameraException:
self.logger.debug('Camera Exception in reset()')
continue
except TCPConnectionError as e:
self.logger.debug('TCPConnectionError in reset()')
self.logger.error(e)
# Disconnect and reconnect
self.disconnect()
time.sleep(5)
self._make_carla_client(self.host, self.port)
def disconnect(self):
if self.video_writer is not None:
try:
self.video_writer.close()
except Exception as e:
self.logger.debug(
'Error when closing video writer in disconnect')
self.logger.error(e)
self.video_writer = None
self._client.disconnect()
def _raster_frame(self, sensor_data, measurements, directions, obs):
frame = sensor_data['CameraRGB'].data.copy()
cv2.putText(frame,
text='Episode number: {:,}'.format(self.num_episodes - 1),
org=(50, 50),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1.0,
color=[0, 0, 0],
thickness=2)
cv2.putText(frame,
text='Environment steps: {:,}'.format(self.steps),
org=(50, 80),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1.0,
color=[0, 0, 0],
thickness=2)
REACH_GOAL = 0.0
GO_STRAIGHT = 5.0
TURN_RIGHT = 4.0
TURN_LEFT = 3.0
LANE_FOLLOW = 2.0
if np.isclose(directions, REACH_GOAL):
dir_str = 'REACH GOAL'
elif np.isclose(directions, GO_STRAIGHT):
dir_str = 'GO STRAIGHT'
elif np.isclose(directions, TURN_RIGHT):
dir_str = 'TURN RIGHT'
elif np.isclose(directions, TURN_LEFT):
dir_str = 'TURN LEFT'
elif np.isclose(directions, LANE_FOLLOW):
dir_str = 'LANE FOLLOW'
else:
raise ValueError(directions)
cv2.putText(frame,
text='Direction: {}'.format(dir_str),
org=(50, 110),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1.0,
color=[0, 0, 0],
thickness=2)
cv2.putText(frame,
text='Speed: {:.02f}'.format(
measurements.player_measurements.forward_speed * 3.6),
org=(50, 140),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1.0,
color=[0, 0, 0],
thickness=2)
cv2.putText(frame,
text='rel_x: {:.02f}, rel_y: {:.02f}'.format(
obs['v'][-2].item(), obs['v'][-1].item()),
org=(50, 170),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1.0,
color=[0, 0, 0],
thickness=2)
self.video_writer.writeFrame(frame)
def _get_distance_to_goal(self, measurements, target):
current_x = measurements.player_measurements.transform.location.x
current_y = measurements.player_measurements.transform.location.y
distance_to_goal = np.linalg.norm(np.array([current_x, current_y]) - \
np.array([target.location.x, target.location.y]))
return distance_to_goal
def _new_episode(self):
experiment_idx = np.random.randint(0, len(self._experiments))
experiment = self._experiments[experiment_idx]
exp_settings = experiment.conditions
exp_settings.set(QualityLevel='Low')
positions = self._client.load_settings(exp_settings).player_start_spots
idx_pose = np.random.randint(0, len(experiment.poses))
pose = experiment.poses[idx_pose]
self.logger.info('Env {} gets experiment {} with pose {}'.format(
self.id, experiment_idx, idx_pose))
start_index = pose[0]
end_index = pose[1]
self._client.start_episode(start_index)
self._time_out = self._experiment_suite.calculate_time_out(
self._get_shortest_path(positions[start_index],
positions[end_index]))
self._target = positions[end_index]
self._episode_name = str(experiment.Conditions.WeatherId) + '_' \
+ str(experiment.task) + '_' + str(start_index) \
+ '_' + str(end_index)
if ((self.num_episodes % self.video_every) == 0) and (self.id == 0):
video_path = os.path.join(
self.video_dir, '{:08d}_'.format(self.num_episodes) +
self._episode_name + '.mp4')
self.logger.info('Writing video at {}'.format(video_path))
self.video_writer = skvideo.io.FFmpegWriter(
video_path, inputdict={'-r': '30'}, outputdict={'-r': '30'})
else:
self.video_writer = None
self.num_episodes += 1
def _new_episode_benchmark(self):
experiment_idx_past = self.benchmark_index[0]
pose_idx_past = self.benchmark_index[1]
repetition_idx_past = self.benchmark_index[2]
experiment_past = self._experiments[experiment_idx_past]
poses_past = experiment_past.poses[0:]
repetition_past = experiment_past.repetitions
if repetition_idx_past == repetition_past:
if pose_idx_past == len(poses_past) - 1:
if experiment_idx_past == len(self._experiments) - 1:
return False
else:
experiment = self._experiments[experiment_idx_past + 1]
pose = experiment.poses[0:][0]
self.benchmark_index = [experiment_idx_past + 1, 0, 1]
else:
experiment = experiment_past
pose = poses_past[pose_idx_past + 1]
self.benchmark_index = [
experiment_idx_past, pose_idx_past + 1, 1
]
else:
experiment = experiment_past
pose = poses_past[pose_idx_past]
self.benchmark_index = [
experiment_idx_past, pose_idx_past, repetition_idx_past + 1
]
exp_settings = experiment.Conditions
exp_settings.set(QualityLevel='Low')
positions = self._client.load_settings(exp_settings).player_start_spots
start_index = pose[0]
end_index = pose[1]
self._client.start_episode(start_index)
self._time_out = self._experiment_suite.calculate_time_out(
self._get_shortest_path(positions[start_index],
positions[end_index]))
self._target = positions[end_index]
self._episode_name = str(experiment.Conditions.WeatherId) + '_' \
+ str(experiment.task) + '_' + str(start_index) \
+ '_' + str(end_index)
if ((self.num_episodes % self.video_every) == 0) and (self.id == 0):
video_path = os.path.join(
self.video_dir, '{:08d}_'.format(self.num_episodes) +
self._episode_name + '.mp4')
self.logger.info('Writing video at {}'.format(video_path))
self.video_writer = skvideo.io.FFmpegWriter(
video_path, inputdict={'-r': '30'}, outputdict={'-r': '30'})
else:
self.video_writer = None
self.num_episodes += 1
def _get_directions(self, current_point, end_point):
directions = self._planner.get_next_command(
(current_point.location.x, current_point.location.y, 0.22),
(current_point.orientation.x, current_point.orientation.y,
current_point.orientation.z),
(end_point.location.x, end_point.location.y, 0.22),
(end_point.orientation.x, end_point.orientation.y,
end_point.orientation.z))
return directions
def _get_shortest_path(self, start_point, end_point):
return self._planner.get_shortest_path_distance(
[start_point.location.x, start_point.location.y, 0.22],
[start_point.orientation.x, start_point.orientation.y, 0.22],
[end_point.location.x, end_point.location.y, end_point.location.z],
[
end_point.orientation.x, end_point.orientation.y,
end_point.orientation.z
])
@staticmethod
def _is_collision(measurements):
c = 0
c += measurements.player_measurements.collision_vehicles
c += measurements.player_measurements.collision_pedestrians
c += measurements.player_measurements.collision_other
sidewalk_intersection = measurements.player_measurements.intersection_offroad
otherlane_intersection = measurements.player_measurements.intersection_otherlane
return (c > 1e-9) or (sidewalk_intersection >
0.01) or (otherlane_intersection > 0.9), c
def _make_carla_client(self, host, port):
while True:
try:
self.logger.info(
"Trying to make client on port {}".format(port))
self._client = CarlaClient(host, port, timeout=100)
self._client.connect()
self._client.load_settings(CarlaSettings(QualityLevel='Low'))
self._client.start_episode(0)
self.logger.info(
"Successfully made client on port {}".format(port))
break
except TCPConnectionError as error:
self.logger.debug('Got TCPConnectionError..sleeping for 1')
self.logger.error(error)
time.sleep(1)
class CarlaEnv(gym.Env):
def __init__(self, config=ENV_CONFIG):
self.config = config
self.city = self.config["server_map"].split("/")[-1]
if self.config["enable_planner"]:
self.planner = Planner(self.city)
# The Action Space
if config["discrete_actions"]:
self.action_space = Discrete(
len(DISCRETE_ACTIONS
)) # It will be transformed to continuous 2D action.
else:
self.action_space = Box(-1.0, 1.0, shape=(2, ),
dtype=np.float32) # 2D action.
if config["use_depth_camera"]:
image_space = Box(-1.0,
1.0,
shape=(config["y_res"], config["x_res"],
1 * config["framestack"]),
dtype=np.float32)
else:
image_space = Box(0,
255,
shape=(config["y_res"], config["x_res"],
3 * config["framestack"]),
dtype=np.uint8)
# encode_measure ---> rgb + depth + pre_rgb + measurement_encode
# 3 + 1 + 3 + 1 = 8
if config["encode_measurement"]:
image_space = Box(0,
255,
shape=(config["y_res"], config["x_res"], 8),
dtype=np.float32)
# The Observation Space
self.observation_space = Tuple([
image_space,
Discrete(len(COMMANDS_ENUM)), # next_command
Box(-128.0, 128.0, shape=(2, ),
dtype=np.float32) # forward_speed, dist to goal
])
# TODO(ekl) this isn't really a proper gym spec
self._spec = lambda: None
self._spec.id = "Carla-v0"
self.server_port = None
self.server_process = None
self.client = None
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.prev_image = None
self.episode_id = None
self.measurements_file = None
self.weather = None
self.scenario = None
self.start_pos = None
self.end_pos = None
self.start_coord = None
self.end_coord = None
self.last_obs = None
def init_server(self):
print("Initializing new Carla server...")
# Create a new server process and start the client.
self.server_port = random.randint(1000, 60000)
self.server_process = subprocess.Popen(
[
SERVER_BINARY,
self.config["server_map"],
"-windowed",
"-ResX=800",
"-ResY=600",
"-carla-server",
"-benchmark -fps=10", #: to run the simulation at a fixed time-step of 0.1 seconds
"-carla-world-port={}".format(self.server_port)
],
preexec_fn=os.setsid,
stdout=open(os.devnull, "w"))
live_carla_processes.add(os.getpgid(self.server_process.pid))
for i in range(RETRIES_ON_ERROR):
try:
self.client = CarlaClient("localhost", self.server_port)
return self.client.connect()
except Exception as e:
print("Error connecting: {}, attempt {}".format(e, i))
time.sleep(2)
def clear_server_state(self):
print("Clearing Carla server state")
try:
if self.client:
self.client.disconnect()
self.client = None
except Exception as e:
print("Error disconnecting client: {}".format(e))
pass
if self.server_process:
pgid = os.getpgid(self.server_process.pid)
os.killpg(pgid, signal.SIGKILL)
live_carla_processes.remove(pgid)
self.server_port = None
self.server_process = None
def __del__(
self
): # the __del__ method will be called when the instance of the class is deleted.(memory is freed.)
self.clear_server_state()
def reset(self):
error = None
for _ in range(RETRIES_ON_ERROR):
try:
if not self.server_process:
self.init_server()
return self._reset()
except Exception as e:
print("Error during reset: {}".format(traceback.format_exc()))
self.clear_server_state()
error = e
raise error
def _reset(self):
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.prev_image = None
self.episode_id = datetime.today().strftime("%Y-%m-%d_%H-%M-%S_%f")
self.measurements_file = None
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
self.scenario = random.choice(self.config["scenarios"])
assert self.scenario["city"] == self.city, (self.scenario, self.city)
self.weather = random.choice(self.scenario["weather_distribution"])
settings.set(
SynchronousMode=True,
# ServerTimeOut=10000, # CarlaSettings: no key named 'ServerTimeOut'
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self.scenario["num_vehicles"],
NumberOfPedestrians=self.scenario["num_pedestrians"],
WeatherId=self.weather)
settings.randomize_seeds()
if self.config["use_depth_camera"]:
camera1 = Camera("CameraDepth", PostProcessing="Depth")
camera1.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
# camera1.set_position(30, 0, 170)
camera1.set_position(0.5, 0.0, 1.6)
# camera1.set_rotation(0.0, 0.0, 0.0)
settings.add_sensor(camera1)
if self.config["encode_measurement"]:
camera1 = Camera("CameraDepth", PostProcessing="Depth")
camera1.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
# camera1.set_position(30, 0, 170)
camera1.set_position(0.5, 0.0, 1.6)
camera1.set(FOV=120)
settings.add_sensor(camera1)
camera2 = Camera("CameraRGB")
camera2.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
camera2.set(FOV=120)
camera2.set_position(0.5, 0.0, 1.6)
settings.add_sensor(camera2)
# Setup start and end positions
scene = self.client.load_settings(settings)
positions = scene.player_start_spots
self.start_pos = positions[self.scenario["start_pos_id"]]
self.end_pos = positions[self.scenario["end_pos_id"]]
self.start_coord = [
self.start_pos.location.x, self.start_pos.location.y
]
self.end_coord = [self.end_pos.location.x, self.end_pos.location.y]
print("Start pos {} ({}), end {} ({})".format(
self.scenario["start_pos_id"], [int(x) for x in self.start_coord],
self.scenario["end_pos_id"], [int(x) for x in self.end_coord]))
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print("Starting new episode...")
self.client.start_episode(self.scenario["start_pos_id"])
# Process observations: self._read_observation() returns image and py_measurements.
image, py_measurements = self._read_observation()
self.prev_measurement = py_measurements
return self.encode_obs(self.preprocess_image(image), py_measurements)
# rgb depth forward_speed next_comment
def encode_obs(self, image, py_measurements):
assert self.config["framestack"] in [1, 2]
prev_image = self.prev_image
self.prev_image = image
if prev_image is None:
prev_image = image
if self.config["framestack"] == 2:
image = np.concatenate([prev_image, image], axis=2)
if self.config["encode_measurement"]:
feature_map = np.zeros([4, 4])
feature_map[1, :] = (py_measurements["forward_speed"] - 15) / 15
feature_map[2, :] = (
COMMAND_ORDINAL[py_measurements["next_command"]] - 2) / 2
feature_map[0, 0] = py_measurements["x_orient"]
feature_map[0, 1] = py_measurements["y_orient"]
feature_map[0,
2] = (py_measurements["distance_to_goal"] - 170) / 170
feature_map[0,
1] = (py_measurements["distance_to_goal_euclidean"] -
170) / 170
feature_map[3, 0] = (py_measurements["x"] - 50) / 150
feature_map[3, 1] = (py_measurements["y"] - 50) / 150
feature_map[3, 2] = (py_measurements["end_coord"][0] - 150) / 150
feature_map[3, 3] = (py_measurements["end_coord"][1] - 150) / 150
feature_map = np.tile(feature_map, (24, 24))
image = np.concatenate(
[prev_image[:, :, 0:3], image, feature_map[:, :, np.newaxis]],
axis=2)
# print("image shape after encoding", image.shape)
obs = (image, COMMAND_ORDINAL[py_measurements["next_command"]], [
py_measurements["forward_speed"],
py_measurements["distance_to_goal"]
])
self.last_obs = obs
# print('distance to goal', py_measurements["distance_to_goal"])
# print("speed", py_measurements["forward_speed"])
return obs
# TODO:example of py_measurement
# {'episode_id': '2019-02-22_11-26-36_990083',
# 'step': 0,
# 'x': 71.53003692626953,
# 'y': 302.57000732421875,
# 'x_orient': -1.0,
# 'y_orient': -6.4373016357421875e-06,
# 'forward_speed': -3.7578740032934155e-13,
# 'distance_to_goal': 0.6572,
# 'distance_to_goal_euclidean': 0.6200001239776611,
# 'collision_vehicles': 0.0,
# 'collision_pedestrians': 0.0,
# 'collision_other': 0.0,
# 'intersection_offroad': 0.0,
# 'intersection_otherlane': 0.0,
# 'weather': 0,
# 'map': '/Game/Maps/Town02',
# 'start_coord': [0.0, 3.0],
# 'end_coord': [0.0, 3.0],
# 'current_scenario': {'city': 'Town02',
# 'num_vehicles': 0,
# 'num_pedestrians': 20,
# 'weather_distribution': [0],
# 'start_pos_id': 36,
# 'end_pos_id': 40,
# 'max_steps': 600},
# 'x_res': 10,
# 'y_res': 10,
# 'num_vehicles': 0,
# 'num_pedestrians': 20,
# 'max_steps': 600,
# 'next_command': 'GO_STRAIGHT',
# 'action': (0, 1),
# 'control': {'steer': 1.0,
# 'throttle': 0.0,
# 'brake': 0.0,
# 'reverse': False,
# 'hand_brake': False},
# 'reward': 0.0,
# 'total_reward': 0.0,
# 'done': False}
def step(self, action):
try:
obs = self._step(action)
return obs
except Exception:
print("Error during step, terminating episode early",
traceback.format_exc())
self.clear_server_state()
return (self.last_obs, 0.0, True, {})
def _step(self, action):
if self.config["discrete_actions"]:
action = DISCRETE_ACTIONS[int(action)] # Carla action is 2D.
assert len(action) == 2, "Invalid action {}".format(action)
if self.config["squash_action_logits"]:
forward = 2 * float(sigmoid(action[0]) - 0.5)
throttle = float(np.clip(forward, 0, 1))
brake = float(np.abs(np.clip(forward, -1, 0)))
steer = 2 * float(sigmoid(action[1]) - 0.5)
else:
throttle = float(np.clip(action[0], 0, 1))
brake = float(np.abs(np.clip(action[0], -1, 0)))
steer = float(np.clip(action[1], -1, 1))
# reverse and hand_brake are disabled.
reverse = False
hand_brake = False
if self.config["verbose"]:
print("steer", steer, "throttle", throttle, "brake", brake,
"reverse", reverse)
self.client.send_control(steer=steer,
throttle=throttle,
brake=brake,
hand_brake=hand_brake,
reverse=reverse)
# Process observations: self._read_observation() returns image and py_measurements.
image, py_measurements = self._read_observation()
if self.config["verbose"]:
print("Next command", py_measurements["next_command"])
if type(action) is np.ndarray:
py_measurements["action"] = [float(a) for a in action]
else:
py_measurements["action"] = action
py_measurements["control"] = {
"steer": steer,
"throttle": throttle,
"brake": brake,
"reverse": reverse,
"hand_brake": hand_brake,
}
# compute reward
reward = compute_reward(self, self.prev_measurement, py_measurements)
self.total_reward += reward
py_measurements["reward"] = reward
py_measurements["total_reward"] = self.total_reward
# done or not
done = (self.num_steps > self.scenario["max_steps"]
or py_measurements["next_command"] == "REACH_GOAL"
or (self.config["early_terminate_on_collision"]
and collided_done(py_measurements)))
py_measurements["done"] = done
self.prev_measurement = py_measurements
# Write out measurements to file
if self.config["verbose"] and CARLA_OUT_PATH:
if not self.measurements_file:
self.measurements_file = open(
os.path.join(
CARLA_OUT_PATH,
"measurements_{}.json".format(self.episode_id)), "w")
self.measurements_file.write(json.dumps(py_measurements))
self.measurements_file.write("\n")
if done:
self.measurements_file.close()
self.measurements_file = None
if self.config["convert_images_to_video"]:
self.images_to_video()
self.num_steps += 1
image = self.preprocess_image(image)
return (self.encode_obs(image, py_measurements), reward, done,
py_measurements)
def images_to_video(self):
videos_dir = os.path.join(CARLA_OUT_PATH, "Videos")
if not os.path.exists(videos_dir):
os.makedirs(videos_dir)
ffmpeg_cmd = (
"ffmpeg -loglevel -8 -r 60 -f image2 -s {x_res}x{y_res} "
"-start_number 0 -i "
"{img}_%04d.jpg -vcodec libx264 {vid}.mp4 && rm -f {img}_*.jpg "
).format(x_res=self.config["render_x_res"],
y_res=self.config["render_y_res"],
vid=os.path.join(videos_dir, self.episode_id),
img=os.path.join(CARLA_OUT_PATH, "CameraRGB",
self.episode_id))
print("Executing ffmpeg command", ffmpeg_cmd)
subprocess.call(ffmpeg_cmd, shell=True)
def preprocess_image(self, image):
if self.config["use_depth_camera"]:
assert self.config["use_depth_camera"]
data = (image.data - 0.5) * 2
data = data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 1)
data = cv2.resize(data,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = np.expand_dims(data, 2)
elif self.config["encode_measurement"]:
# data = (image - 0.5) * 2
data = image.reshape(self.config["render_y_res"],
self.config["render_x_res"], -1)
#data[:, :, 4] = (data[:, :, 0] - 0.5) * 2
#data[:, :, 0:3] = (data[:, :, 0:2].astype(np.float32) - 128) / 128
data = cv2.resize(data,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
else:
data = image.data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 3)
data = cv2.resize(data,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = (data.astype(np.float32) - 128) / 128
return data
def _read_observation(self):
# Read the data produced by the server this frame.
measurements, sensor_data = self.client.read_data()
# Print some of the measurements.
if self.config["verbose"]:
print_measurements(measurements)
observation = None
if self.config["encode_measurement"]:
# print(sensor_data["CameraRGB"].data.shape, sensor_data["CameraDepth"].data.shape)
observation = np.concatenate(
((sensor_data["CameraRGB"].data.astype(np.float32) - 128) /
128,
(sensor_data["CameraDepth"].data[:, :, np.newaxis] - 0.5) *
0.5),
axis=2)
# print("observation_shape", observation.shape)
else:
if self.config["use_depth_camera"]:
camera_name = "CameraDepth"
else:
camera_name = "CameraRGB"
for name, image in sensor_data.items():
if name == camera_name:
observation = image
cur = measurements.player_measurements
if self.config["enable_planner"]:
next_command = COMMANDS_ENUM[self.planner.get_next_command(
[cur.transform.location.x, cur.transform.location.y, GROUND_Z],
[
cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z
],
[self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [
self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z
])]
else:
next_command = "LANE_FOLLOW"
if next_command == "REACH_GOAL":
distance_to_goal = 0.0 # avoids crash in planner
elif self.config["enable_planner"]:
distance_to_goal = self.planner.get_shortest_path_distance(
[cur.transform.location.x, cur.transform.location.y, GROUND_Z],
[
cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z
],
[self.end_pos.location.x, self.end_pos.location.y, GROUND_Z], [
self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z
])
# now the metrix should be meter carla8.0
# TODO run experience to verify the scale of distance in order to determine reward function
else:
distance_to_goal = -1
distance_to_goal_euclidean = float(
np.linalg.norm([
cur.transform.location.x - self.end_pos.location.x,
cur.transform.location.y - self.end_pos.location.y
]))
py_measurements = {
"episode_id": self.episode_id,
"step": self.num_steps,
"x": cur.transform.location.x,
"y": cur.transform.location.y,
"x_orient": cur.transform.orientation.x,
"y_orient": cur.transform.orientation.y,
"forward_speed": cur.forward_speed,
"distance_to_goal": distance_to_goal,
"distance_to_goal_euclidean": distance_to_goal_euclidean,
"collision_vehicles": cur.collision_vehicles,
"collision_pedestrians": cur.collision_pedestrians,
"collision_other": cur.collision_other,
"intersection_offroad": cur.intersection_offroad,
"intersection_otherlane": cur.intersection_otherlane,
"weather": self.weather,
"map": self.config["server_map"],
"start_coord": self.start_coord,
"end_coord": self.end_coord,
"current_scenario": self.scenario,
"x_res": self.config["x_res"],
"y_res": self.config["y_res"],
"num_vehicles": self.scenario["num_vehicles"],
"num_pedestrians": self.scenario["num_pedestrians"],
"max_steps": self.scenario["max_steps"],
"next_command": next_command,
}
if CARLA_OUT_PATH and self.config["log_images"]:
for name, image in sensor_data.items():
out_dir = os.path.join(CARLA_OUT_PATH, name)
if not os.path.exists(out_dir):
os.makedirs(out_dir)
out_file = os.path.join(
out_dir, "{}_{:>04}.jpg".format(self.episode_id,
self.num_steps))
scipy.misc.imsave(out_file, image.data)
assert observation is not None, sensor_data
return observation, py_measurements
class CarlaEnv(gym.Env):
def __init__(self, config=ENV_CONFIG):
"""
Carla Gym Environment class implementation. Creates an OpenAI Gym compatible driving environment based on
Carla driving simulator.
:param config: A dictionary with environment configuration keys and values
"""
self.config = config
self.city = self.config["server_map"].split("/")[-1]
if self.config["enable_planner"]:
self.planner = Planner(self.city)
if config["discrete_actions"]:
self.action_space = Discrete(len(DISCRETE_ACTIONS))
else:
self.action_space = Box(-1.0, 1.0, shape=(2,), dtype=np.uint8)
if config["use_depth_camera"]:
image_space = Box(
-1.0, 1.0, shape=(
config["y_res"], config["x_res"],
1 * config["framestack"]), dtype=np.float32)
else:
image_space = Box(
0.0, 255.0, shape=(
config["y_res"], config["x_res"],
3 * config["framestack"]), dtype=np.float32)
if self.config["use_image_only_observations"]:
self.observation_space = image_space
else:
self.observation_space = Tuple(
[image_space,
Discrete(len(COMMANDS_ENUM)), # next_command
Box(-128.0, 128.0, shape=(2,), dtype=np.float32)]) # forward_speed, dist to goal
self._spec = lambda: None
self._spec.id = "Carla-v0"
self._seed = ENV_CONFIG["seed"]
self.server_port = None
self.server_process = None
self.client = None
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.prev_image = None
self.episode_id = None
self.measurements_file = None
self.weather = None
self.scenario = None
self.start_pos = None
self.end_pos = None
self.start_coord = None
self.end_coord = None
self.last_obs = None
self.viewer = None
def render(self, mode=None):
filename = f'images/id_{self._spec.id}_ep_{self.episode_id}_step_{self.num_steps}'
self.frame_image.save_to_disk(filename)
# from gym.envs.classic_control import rendering
# if self.viewer is None:
# self.viewer = rq
# endering.SimpleImageViewer()
# self.viewer.imshow(self.frame_image)
# return self.viewer.isopen
def init_server(self):
print("Initializing new Carla server...")
# Create a new server process and start the client.
self.server_port = random.randint(10000, 60000)
if self.config["render"]:
self.server_process = subprocess.Popen(
[SERVER_BINARY, self.config["server_map"],
"-windowed", "-ResX=400", "-ResY=300",
"-carla-server",
"-carla-world-port={}".format(self.server_port)],
preexec_fn=os.setsid, stdout=open(os.devnull, "w"))
else:
self.server_process = subprocess.Popen(
("SDL_VIDEODRIVER=offscreen SDL_HINT_CUDA_DEVICE={} {} " +
self.config["server_map"] + " -windowed -ResX=400 -ResY=300"
" -carla-server -carla-world-port={}").format(0, SERVER_BINARY, self.server_port),
shell=True, preexec_fn=os.setsid, stdout=open(os.devnull, "w"))
live_carla_processes.add(os.getpgid(self.server_process.pid))
for i in range(RETRIES_ON_ERROR):
try:
self.client = CarlaClient("localhost", self.server_port)
return self.client.connect()
except Exception as e:
print("Error connecting: {}, attempt {}".format(e, i))
time.sleep(2)
def clear_server_state(self):
print("Clearing Carla server state")
try:
if self.client:
self.client.disconnect()
self.client = None
except Exception as e:
print("Error disconnecting client: {}".format(e))
pass
if self.server_process:
pgid = os.getpgid(self.server_process.pid)
os.killpg(pgid, signal.SIGKILL)
live_carla_processes.remove(pgid)
self.server_port = None
self.server_process = None
def __del__(self):
self.clear_server_state()
def reset(self):
error = None
for _ in range(RETRIES_ON_ERROR):
try:
if not self.server_process:
self.init_server()
return self.reset_env()
except Exception as e:
print("Error during reset: {}".format(traceback.format_exc()))
self.clear_server_state()
error = e
raise error
def reset_env(self):
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.prev_image = None
self.episode_id = datetime.today().strftime("%Y-%m-%d_%H-%M-%S_%f")
self.measurements_file = None
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
# If config["scenarios"] is a single scenario, then use it if it's an array of scenarios, randomly choose one and init
if isinstance(self.config["scenarios"],dict):
self.scenario = self.config["scenarios"]
else: #isinstance array of dict
self.scenario = random.choice(self.config["scenarios"])
assert self.scenario["city"] == self.city, (self.scenario, self.city)
self.weather = random.choice(self.scenario["weather_distribution"])
settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self.scenario["num_vehicles"],
NumberOfPedestrians=self.scenario["num_pedestrians"],
WeatherId=self.weather)
settings.randomize_seeds()
if self.config["use_depth_camera"]:
camera1 = Camera("CameraDepth", PostProcessing="Depth")
camera1.set_image_size(
self.config["render_x_res"], self.config["render_y_res"])
camera1.set_position(0.30, 0, 1.30)
settings.add_sensor(camera1)
camera2 = Camera("CameraRGB")
camera2.set_image_size(
self.config["render_x_res"], self.config["render_y_res"])
camera2.set_position(0.30, 0, 1.30)
settings.add_sensor(camera2)
# Setup start and end positions
scene = self.client.load_settings(settings)
positions = scene.player_start_spots
self.start_pos = positions[self.scenario["start_pos_id"]]
self.end_pos = positions[self.scenario["end_pos_id"]]
self.start_coord = [
self.start_pos.location.x // 100, self.start_pos.location.y // 100]
self.end_coord = [
self.end_pos.location.x // 100, self.end_pos.location.y // 100]
print(
"Start pos {} ({}), end {} ({})".format(
self.scenario["start_pos_id"], self.start_coord,
self.scenario["end_pos_id"], self.end_coord))
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print("Starting new episode...")
self.client.start_episode(self.scenario["start_pos_id"])
image, py_measurements = self._read_observation()
self.prev_measurement = py_measurements
return self.encode_obs(self.preprocess_image(image), py_measurements)
def encode_obs(self, image, py_measurements):
assert self.config["framestack"] in [1, 2]
prev_image = self.prev_image
self.prev_image = image
if prev_image is None:
prev_image = image
if self.config["framestack"] == 2:
image = np.concatenate([prev_image, image], axis=2)
if self.config["use_image_only_observations"]:
obs = image
else:
obs = (
image,
COMMAND_ORDINAL[py_measurements["next_command"]],
[py_measurements["forward_speed"],
py_measurements["distance_to_goal"]])
self.last_obs = obs
return obs
def step(self, action):
try:
obs = self.step_env(action)
return obs
except Exception:
print(
"Error during step, terminating episode early",
traceback.format_exc())
self.clear_server_state()
return (self.last_obs, 0.0, True, {})
def step_env(self, action):
if self.config["discrete_actions"]:
action = DISCRETE_ACTIONS[int(action)]
assert len(action) == 2, "Invalid action {}".format(action)
throttle = float(np.clip(action[0], 0, 1))
brake = float(np.abs(np.clip(action[0], -1, 0)))
steer = float(np.clip(action[1], -1, 1))
reverse = False
hand_brake = False
if self.config["verbose"]:
print(
"steer", steer, "throttle", throttle, "brake", brake,
"reverse", reverse)
self.client.send_control(
steer=steer, throttle=throttle, brake=brake, hand_brake=hand_brake,
reverse=reverse)
# Process observations
image, py_measurements = self._read_observation()
if self.config["verbose"]:
print("Next command", py_measurements["next_command"])
if type(action) is np.ndarray:
py_measurements["action"] = [float(a) for a in action]
else:
py_measurements["action"] = action
py_measurements["control"] = {
"steer": steer,
"throttle": throttle,
"brake": brake,
"reverse": reverse,
"hand_brake": hand_brake,
}
reward = self.calculate_reward(py_measurements)
self.total_reward += reward
py_measurements["reward"] = reward
py_measurements["total_reward"] = self.total_reward
done = (self.num_steps > self.scenario["max_steps"] or
py_measurements["next_command"] == "REACH_GOAL" or
(self.config["early_terminate_on_collision"] and
check_collision(py_measurements)))
py_measurements["done"] = done
self.prev_measurement = py_measurements
self.num_steps += 1
image = self.preprocess_image(image)
return (
self.encode_obs(image, py_measurements), reward, done,
py_measurements)
def preprocess_image(self, image):
if self.config["use_depth_camera"]:
assert self.config["use_depth_camera"]
data = (image.data - 0.5) * 2
data = data.reshape(
self.config["render_y_res"], self.config["render_x_res"], 1)
data = cv2.resize(
data, (self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = np.expand_dims(data, 2)
else:
data = image.data.reshape(
self.config["render_y_res"], self.config["render_x_res"], 3)
data = cv2.resize(
data, (self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = (data.astype(np.float32) - 128) / 128
return data
def _read_observation(self):
# Read the data produced by the server this frame.
measurements, sensor_data = self.client.read_data()
# Print some of the measurements.
if self.config["verbose"]:
print_measurements(measurements)
observation = None
if self.config["use_depth_camera"]:
camera_name = "CameraDepth"
else:
camera_name = "CameraRGB"
for name, image in sensor_data.items():
if name == camera_name:
observation = image
self.frame_image = observation
cur = measurements.player_measurements
if self.config["enable_planner"]:
next_command = COMMANDS_ENUM[
self.planner.get_next_command(
[cur.transform.location.x, cur.transform.location.y,
GROUND_Z],
[cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z],
[self.end_pos.location.x, self.end_pos.location.y,
GROUND_Z],
[self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z])
]
else:
next_command = "LANE_FOLLOW"
if next_command == "REACH_GOAL":
distance_to_goal = 0.0 # avoids crash in planner
elif self.config["enable_planner"]:
distance_to_goal = self.planner.get_shortest_path_distance(
[cur.transform.location.x, cur.transform.location.y, GROUND_Z],
[cur.transform.orientation.x, cur.transform.orientation.y,
GROUND_Z],
[self.end_pos.location.x, self.end_pos.location.y, GROUND_Z],
[self.end_pos.orientation.x, self.end_pos.orientation.y,
GROUND_Z]) / 100
else:
distance_to_goal = -1
distance_to_goal_euclidean = float(np.linalg.norm(
[cur.transform.location.x - self.end_pos.location.x,
cur.transform.location.y - self.end_pos.location.y]) / 100)
py_measurements = {
"episode_id": self.episode_id,
"step": self.num_steps,
"x": cur.transform.location.x,
"y": cur.transform.location.y,
"x_orient": cur.transform.orientation.x,
"y_orient": cur.transform.orientation.y,
"forward_speed": cur.forward_speed,
"distance_to_goal": distance_to_goal,
"distance_to_goal_euclidean": distance_to_goal_euclidean,
"collision_vehicles": cur.collision_vehicles,
"collision_pedestrians": cur.collision_pedestrians,
"collision_other": cur.collision_other,
"intersection_offroad": cur.intersection_offroad,
"intersection_otherlane": cur.intersection_otherlane,
"weather": self.weather,
"map": self.config["server_map"],
"start_coord": self.start_coord,
"end_coord": self.end_coord,
"current_scenario": self.scenario,
"x_res": self.config["x_res"],
"y_res": self.config["y_res"],
"num_vehicles": self.scenario["num_vehicles"],
"num_pedestrians": self.scenario["num_pedestrians"],
"max_steps": self.scenario["max_steps"],
"next_command": next_command,
}
assert observation is not None, sensor_data
return observation, py_measurements
def calculate_reward(self, current_measurement):
"""
Calculate the reward based on the effect of the action taken using the previous and the current measurements
:param current_measurement: The measurement obtained from the Carla engine after executing the current action
:return: The scalar reward
"""
reward = 0.0
cur_dist = current_measurement["distance_to_goal"]
prev_dist = self.prev_measurement["distance_to_goal"]
if self.config["verbose"]:
print("Cur dist {}, prev dist {}".format(cur_dist, prev_dist))
# Distance travelled toward the goal in m
reward += np.clip(prev_dist - cur_dist, -10.0, 10.0)
# Change in speed (km/hr)
reward += 0.05 * (current_measurement["forward_speed"] - self.prev_measurement["forward_speed"])
# New collision damage
reward -= .00002 * (
current_measurement["collision_vehicles"] + current_measurement["collision_pedestrians"] +
current_measurement["collision_other"] - self.prev_measurement["collision_vehicles"] -
self.prev_measurement["collision_pedestrians"] - self.prev_measurement["collision_other"])
# New sidewalk intersection
reward -= 2 * (
current_measurement["intersection_offroad"] - self.prev_measurement["intersection_offroad"])
# New opposite lane intersection
reward -= 2 * (
current_measurement["intersection_otherlane"] - self.prev_measurement["intersection_otherlane"])
return reward
class CarlaEnv(gym.Env):
def __init__(self, config=ENV_CONFIG):
self.config = config
if config["discrete_actions"]:
self.action_space = Discrete(10)
else:
self.action_space = Box(-1.0, 1.0, shape=(3, ))
if config["use_depth_camera"]:
self.observation_space = Box(-1.0,
1.0,
shape=(config["y_res"],
config["x_res"], 1))
else:
self.observation_space = Box(0.0,
255.0,
shape=(config["y_res"],
config["x_res"], 3))
self._spec = lambda: None
self._spec.id = "Carla-v0"
self.server_port = None
self.server_process = None
self.client = None
self.num_steps = 0
self.total_reward = 0
self.prev_measurement = None
self.episode_id = None
self.measurements_file = None
self.weather = None
self.player_start = None
def init_server(self):
print("Initializing new Carla server...")
# Create a new server process and start the client.
self.server_port = random.randint(10000, 60000)
self.server_process = subprocess.Popen([
SERVER_BINARY, self.config["map"], "-windowed", "-ResX=400",
"-ResY=300", "-carla-server", "-carla-world-port={}".format(
self.server_port)
],
preexec_fn=os.setsid,
stdout=open(os.devnull, "w"))
self.client = CarlaClient("localhost", self.server_port)
self.client.connect()
def clear_server_state(self):
print("Clearing Carla server state")
try:
if self.client:
self.client.disconnect()
self.client = None
except Exception as e:
print("Error disconnecting client: {}".format(e))
pass
if self.server_process:
os.killpg(os.getpgid(self.server_process.pid), signal.SIGKILL)
self.server_port = None
self.server_process = None
def __del__(self):
self.clear_server_state()
def reset(self):
error = None
for _ in range(RETRIES_ON_ERROR):
try:
if not self.server_process:
self.init_server()
# reset twice since the first time a server is initialized,
# the starting location is different
self._reset()
return self._reset()
except Exception as e:
print("Error during reset: {}".format(traceback.format_exc()))
self.clear_server_state()
error = e
raise error
def _reset(self):
self.num_steps = 0
self.prev_measurement = None
self.episode_id = datetime.today().strftime("%Y-%m-%d_%H-%M-%S_%f")
self.measurements_file = None
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
self.weather = random.choice(self.config["weather"])
settings.set(SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self.config["num_vehicles"],
NumberOfPedestrians=self.config["num_pedestrians"],
WeatherId=self.weather)
settings.randomize_seeds()
camera1 = Camera("CameraDepth", PostProcessing="Depth")
camera1.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
camera1.set_position(30, 0, 130)
settings.add_sensor(camera1)
camera2 = Camera("CameraRGB")
camera2.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
camera2.set_position(30, 0, 130)
settings.add_sensor(camera2)
scene = self.client.load_settings(settings)
# Choose one player start at random.
number_of_player_starts = len(scene.player_start_spots)
if self.config["random_starting_location"]:
self.player_start = random.randint(
0, max(0, number_of_player_starts - 1))
else:
self.player_start = 0
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print("Starting new episode...")
self.client.start_episode(self.player_start)
image, py_measurements = self._read_observation()
self.prev_measurement = py_measurements
return self.preprocess_image(image)
def step(self, action):
try:
obs = self._step(action)
return obs
except Exception:
print("Error during step, terminating episode early",
traceback.format_exc())
self.clear_server_state()
return np.zeros(self.observation_space.shape), 0.0, True, {}
def _step(self, action):
if self.config["discrete_actions"]:
action = int(action)
assert action in range(10)
if action == 9:
brake = 1.0
steer = 0.0
throttle = 0.0
reverse = False
else:
brake = 0.0
if action >= 6:
steer = -1.0
elif action >= 3:
steer = 1.0
else:
steer = 0.0
action %= 3
if action == 0:
throttle = 0.0
reverse = False
elif action == 1:
throttle = 1.0
reverse = False
elif action == 2:
throttle = 1.0
reverse = True
else:
assert len(action) == 3, "Invalid action {}".format(action)
steer = action[0]
throttle = min(1.0, abs(action[1]))
brake = max(0.0, min(1.0, action[2]))
reverse = action[1] < 0.0
hand_brake = False
if self.config["verbose"]:
print("steer", steer, "throttle", throttle, "brake", brake,
"reverse", reverse)
self.client.send_control(steer=steer,
throttle=throttle,
brake=brake,
hand_brake=hand_brake,
reverse=reverse)
# Process observations
image, py_measurements = self._read_observation()
reward, done = compute_reward(self.config, self.prev_measurement,
py_measurements)
if self.num_steps > self.config["max_steps"]:
done = True
self.total_reward += reward
py_measurements["reward"] = reward
py_measurements["total_reward"] = self.total_reward
py_measurements["done"] = done
py_measurements["action"] = action
py_measurements["control"] = {
"steer": steer,
"throttle": throttle,
"brake": brake,
"reverse": reverse,
"hand_brake": hand_brake,
}
self.prev_measurement = py_measurements
# Write out measurements to file
if CARLA_OUT_PATH:
if not self.measurements_file:
self.measurements_file = open(
os.path.join(
CARLA_OUT_PATH,
"measurements_{}.json".format(self.episode_id)), "w")
self.measurements_file.write(json.dumps(py_measurements))
self.measurements_file.write("\n")
if done:
self.measurements_file.close()
self.measurements_file = None
self.num_steps += 1
image = self.preprocess_image(image)
return image, reward, done, py_measurements
def preprocess_image(self, image):
if self.config["use_depth_camera"]:
data = (image.data - 0.5) * 2
data = data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 1)
data = cv2.resize(data,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
else:
data = image.data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 3)
data = cv2.resize(data,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = (data.astype(np.float32) - 128) / 128
return data
def _read_observation(self):
# Read the data produced by the server this frame.
measurements, sensor_data = self.client.read_data()
# Print some of the measurements.
if self.config["verbose"]:
print_measurements(measurements)
observation = None
if self.config["use_depth_camera"]:
camera_name = "CameraDepth"
else:
camera_name = "CameraRGB"
for name, image in sensor_data.items():
if name == camera_name:
observation = image
cur = measurements.player_measurements
py_measurements = {
"episode_id": self.episode_id,
"step": self.num_steps,
"x": cur.transform.location.x,
"y": cur.transform.location.y,
"forward_speed": cur.forward_speed,
"collision_vehicles": cur.collision_vehicles,
"collision_pedestrians": cur.collision_pedestrians,
"collision_other": cur.collision_other,
"intersection_offroad": cur.intersection_offroad,
"intersection_otherlane": cur.intersection_otherlane,
"weather": self.weather,
"map": self.config["map"],
"target_x": self.config["target_x"],
"target_y": self.config["target_y"],
"x_res": self.config["x_res"],
"y_res": self.config["y_res"],
"num_vehicles": self.config["num_vehicles"],
"num_pedestrians": self.config["num_pedestrians"],
"max_steps": self.config["max_steps"],
}
if CARLA_OUT_PATH:
for name, image in sensor_data.items():
out_dir = os.path.join(CARLA_OUT_PATH, name)
if not os.path.exists(out_dir):
os.makedirs(out_dir)
out_file = os.path.join(
out_dir, "{}_{:>04}.jpg".format(self.episode_id,
self.num_steps))
scipy.misc.imsave(out_file, image.data)
assert observation is not None, sensor_data
return observation, py_measurements
