class Agent(object):
def __init__(self, city_name):
self.__metaclass__ = abc.ABCMeta
self._planner = Planner(city_name)
def get_distance(self, start_point, end_point):
path_distance = self._planner.get_shortest_path_distance(
[start_point.location.x, start_point.location.y, 22]
, [start_point.orientation.x, start_point.orientation.y, 22]
, [end_point.location.x, end_point.location.y, 22]
, [end_point.orientation.x, end_point.orientation.y, 22])
# We calculate the timout based on the distance
return path_distance
@abc.abstractmethod
def run_step(self, measurements, sensor_data, target):
"""
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(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)
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 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 DrivingBenchmark(object):
"""
The Benchmark class, controls the execution of the benchmark interfacing
an Agent class with a set Suite.
The benchmark class must be inherited with a class that defines the
all the experiments to be run by the agent
"""
def __init__(self,
city_name='Town01',
name_to_save='Test',
continue_experiment=False,
save_images=False,
distance_for_success=2.0):
self.__metaclass__ = abc.ABCMeta
self._city_name = city_name
self._base_name = name_to_save
# The minimum distance for arriving into the goal point in
# order to consider ir a success
self._distance_for_success = distance_for_success
# The object used to record the benchmark and to able to continue after
self._recording = Recording(dir_to_save=city_name,
name_to_save=name_to_save,
continue_experiment=continue_experiment,
save_images=save_images)
# We have a default planner instantiated that produces high level commands
self._planner = Planner(city_name)
self._episode_number = 0
def benchmark_agent(self, experiment_suite, agent, client):
"""
Function to benchmark the agent.
It first check the log file for this benchmark.
if it exist it continues from the experiment where it stopped.
Args:
experiment_suite
agent: an agent object with the run step class implemented.
client:
Return:
A dictionary with all the metrics computed from the
agent running the set of experiments.
"""
# Instantiate a metric object that will be used to compute the metrics for
# the benchmark afterwards.
metrics_object = Metrics(experiment_suite.metrics_parameters,
experiment_suite.dynamic_tasks)
# Function return the current pose and task for this benchmark.
start_pose, start_experiment = self._recording.get_pose_and_experiment(
experiment_suite.get_number_of_poses_task())
logging.info('START')
for experiment in experiment_suite.get_experiments(
)[int(start_experiment):]:
positions = client.load_settings(
experiment.conditions).player_start_spots
self._recording.log_start(experiment.task)
for pose in experiment.poses[start_pose:]:
for rep in range(experiment.repetitions):
start_index = pose[0]
end_index = pose[1]
client.start_episode(start_index)
self._episode_number += 1
# Print information on
logging.info('======== !!!! ==========')
logging.info('Episode Number: %d', self._episode_number)
logging.info(' Start Position %d End Position %d ',
start_index, end_index)
self._recording.log_poses(start_index, end_index,
experiment.Conditions.WeatherId)
# Calculate the initial distance for this episode
initial_distance = \
sldist(
[positions[start_index].location.x, positions[start_index].location.y],
[positions[end_index].location.x, positions[end_index].location.y])
time_out = experiment_suite.calculate_time_out(
self._get_shortest_path(positions[start_index],
positions[end_index]))
# running the agent
(result, reward_vec, control_vec, final_time, remaining_distance) = \
self._run_navigation_episode(
agent, client, time_out, positions[end_index],
str(experiment.Conditions.WeatherId) + '_'
+ str(experiment.task) + '_' + str(start_index)
+ '.' + str(end_index))
# Write the general status of the just ran episode
self._recording.write_summary_results(
experiment, pose, rep, initial_distance,
remaining_distance, final_time, time_out, result)
# Write the details of this episode.
self._recording.write_measurements_results(
experiment, rep, pose, reward_vec, control_vec)
if result > 0:
logging.info(
'+++++ Target achieved in %f seconds! +++++',
final_time)
else:
logging.info('----- Timeout! -----')
start_pose = 0
self._recording.log_end()
return metrics_object.compute(self._recording.path)
def get_path(self):
"""
Returns the path were the log was saved.
"""
return self._recording.path
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 _get_shortest_path(self, start_point, end_point):
"""
Calculates the shortest path between two points considering the road netowrk
"""
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
])
def _run_navigation_episode(self, agent, client, time_out, target,
episode_name):
"""
Run one episode of the benchmark (Pose) for a certain agent.
Args:
agent: the agent object
client: an object of the carla client to communicate
with the CARLA simulator
time_out: the time limit to complete this episode
target: the target to reach
episode_name: The name for saving images of this episode
"""
# Send an initial command.
measurements, sensor_data = client.read_data()
client.send_control(VehicleControl())
initial_timestamp = measurements.game_timestamp
current_timestamp = initial_timestamp
# The vector containing all measurements produced on this episode
measurement_vec = []
# The vector containing all controls produced on this episode
control_vec = []
frame = 0
distance = 10000
success = False
while (current_timestamp - initial_timestamp) < (time_out *
1000) and not success:
# Read data from server with the client
measurements, sensor_data = client.read_data()
# The directions to reach the goal are calculated.
directions = self._get_directions(
measurements.player_measurements.transform, target)
# Agent process the data.
# control = agent.run_step(measurements, sensor_data, directions, target)
control = agent.run_step(measurements, sensor_data, directions,
target)
# Send the control commands to the vehicle
client.send_control(control)
# save images if the flag is activated
self._recording.save_images(sensor_data, episode_name, frame)
current_x = measurements.player_measurements.transform.location.x
current_y = measurements.player_measurements.transform.location.y
logging.info("Controller is Inputting:")
logging.info('Steer = %f Throttle = %f Brake = %f ', control.steer,
control.throttle, control.brake)
current_timestamp = measurements.game_timestamp
# Get the distance travelled until now
distance = sldist([current_x, current_y],
[target.location.x, target.location.y])
# Write status of the run on verbose mode
logging.info('Status:')
logging.info('[d=%f] c_x = %f, c_y = %f ---> t_x = %f, t_y = %f',
float(distance), current_x, current_y,
target.location.x, target.location.y)
# Check if reach the target
if distance < self._distance_for_success:
success = True
# Increment the vectors and append the measurements and controls.
frame += 1
measurement_vec.append(measurements.player_measurements)
control_vec.append(control)
if success:
return 1, measurement_vec, control_vec, float(
current_timestamp - initial_timestamp) / 1000.0, distance
return 0, measurement_vec, control_vec, time_out, distance
class DrivingBenchmark(object):
"""
The Benchmark class, controls the execution of the benchmark interfacing
an Agent class with a set Suite.
The benchmark class must be inherited with a class that defines the
all the experiments to be run by the agent
"""
def __init__(self,
city_name='Town01',
name_to_save='Test',
continue_experiment=False,
save_images=False,
distance_for_success=2.0):
self.__metaclass__ = abc.ABCMeta
self._city_name = city_name
self._base_name = name_to_save
# The minimum distance for arriving into the goal point in
# order to consider ir a success
self._distance_for_success = distance_for_success
# The object used to record the benchmark and to able to continue after
self._recording = Recording(name_to_save=name_to_save,
continue_experiment=continue_experiment,
save_images=save_images)
# We have a default planner instantiated that produces high level commands
self._planner = Planner(city_name)
def benchmark_agent(self, experiment_suite, agent, client):
"""
Function to benchmark the agent.
It first check the log file for this benchmark.
if it exist it continues from the experiment where it stopped.
Args:
experiment_suite
agent: an agent object with the run step class implemented.
client:
Return:
A dictionary with all the metrics computed from the
agent running the set of experiments.
"""
# Instantiate a metric object that will be used to compute the metrics for
# the benchmark afterwards.
data = OrderedDict()
metrics_object = Metrics(experiment_suite.metrics_parameters,
experiment_suite.dynamic_tasks)
# Function return the current pose and task for this benchmark.
start_pose, start_experiment = self._recording.get_pose_and_experiment(
experiment_suite.get_number_of_poses_task())
logging.info('START')
cols = [
'Start_position', 'End_position', 'Total-Distance', 'Time',
"Distance-Travelled", "Follow_lane", "Straight", "Left", "Right",
"Avg-Speed", "Collision-Pedestrian", "Collision-Vehicle",
"Collision-Other", "Intersection-Lane", "Intersection-Offroad",
"Traffic_Light_Infraction", "Success"
]
df = pd.DataFrame(columns=cols)
ts = str(int(time.time()))
for experiment in experiment_suite.get_experiments(
)[int(start_experiment):]:
positions = client.load_settings(
experiment.conditions).player_start_spots
self._recording.log_start(experiment.task)
for pose in experiment.poses[start_pose:]:
data['Start_position'] = pose[0]
data['End_position'] = pose[1]
#print(df)
for rep in range(experiment.repetitions):
start_index = pose[0]
end_index = pose[1]
client.start_episode(start_index)
# Print information on
logging.info('======== !!!! ==========')
logging.info(' Start Position %d End Position %d ',
start_index, end_index)
self._recording.log_poses(start_index, end_index,
experiment.Conditions.WeatherId)
# Calculate the initial distance for this episode
initial_distance = \
sldist(
[positions[start_index].location.x, positions[start_index].location.y],
[positions[end_index].location.x, positions[end_index].location.y])
data["Total-Distance"] = initial_distance
time_out = experiment_suite.calculate_time_out(
self._get_shortest_path(positions[start_index],
positions[end_index]))
# running the agent
(result, reward_vec, control_vec, final_time, remaining_distance, data) = \
self._run_navigation_episode(
agent, client, time_out, positions[end_index],
str(experiment.Conditions.WeatherId) + '_'
+ str(experiment.task) + '_' + str(start_index)
+ '.' + str(end_index) , data)
data["Time"] = final_time
# Write the general status of the just ran episode
self._recording.write_summary_results(
experiment, pose, rep, initial_distance,
remaining_distance, final_time, time_out, result)
# Write the details of this episode.
self._recording.write_measurements_results(
experiment, rep, pose, reward_vec, control_vec)
data['Success'] = result
if result > 0:
logging.info(
'+++++ Target achieved in %f seconds! +++++',
final_time)
else:
logging.info('----- Timeout! -----')
#data['Start_position'] = start_index
#data['End_position'] = end_index
#df = df.append(data , ignore_index=True)
#print(df)
data["Avg-Speed"] = 3.6 * (data['Total-Distance'] /
data['Time'])
df = df.append(data, ignore_index=True)
df = df[cols]
try:
df.to_csv("Test_result_" + ts + '.csv',
columns=cols,
index=True)
except:
print("File being used will save later")
start_pose = 0
self._recording.log_end()
return metrics_object.compute(self._recording.path)
def get_path(self):
"""
Returns the path were the log was saved.
"""
return self._recording.path
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 _get_shortest_path(self, start_point, end_point):
"""
Calculates the shortest path between two points considering the road netowrk
"""
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
])
def _run_navigation_episode(self, agent, client, time_out, target,
episode_name, data):
"""
Run one episode of the benchmark (Pose) for a certain agent.
Args:
agent: the agent object
client: an object of the carla client to communicate
with the CARLA simulator
time_out: the time limit to complete this episode
target: the target to reach
episode_name: The name for saving images of this episode
"""
# Send an initial command.
direction2text = {
0: 'Follow_lane',
1: 'Follow_lane',
2: 'Follow_lane',
3: 'Left',
4: 'Right',
5: 'Straight'
}
measurements, sensor_data = client.read_data()
client.send_control(VehicleControl())
initial_timestamp = measurements.game_timestamp
current_timestamp = initial_timestamp
# The vector containing all measurements produced on this episode
measurement_vec = []
# The vector containing all controls produced on this episode
control_vec = []
frame = 0
distance = 10000
success = False
# Initialize lane and Offroad fields
data["Intersection-Lane"] = 0
data["Intersection-Offroad"] = 0
data["Directions"] = []
agent.command_follower.controller.params[
'target_speed'] = agent.command_follower.controller.params[
'default_speed_limit']
agent.traffic_light_infraction = False
for x in range(6):
data[direction2text[x]] = 0
data["Collision-Pedestrian"] = 0
data["Collision-Vehicle"] = 0
data["Collision-Other"] = 0
while True: #
# Read data from server with the client
measurements, sensor_data = client.read_data()
# The directions to reach the goal are calculated.
directions = self._get_directions(
measurements.player_measurements.transform, target)
# Agent process the data.
if data[direction2text[directions]] == 0:
data[direction2text[directions]] = 1
control, controller_state = agent.run_step(measurements,
sensor_data, directions,
target)
# Send the control commands to the vehicle
client.send_control(control)
if measurements.player_measurements.intersection_otherlane > data[
"Intersection-Lane"]:
data[
"Intersection-Lane"] = measurements.player_measurements.intersection_otherlane
if measurements.player_measurements.intersection_offroad > data[
"Intersection-Offroad"]:
data[
"Intersection-Offroad"] = measurements.player_measurements.intersection_offroad
# save images if the flag is activated
self._recording.save_images(sensor_data, episode_name, frame)
current_x = measurements.player_measurements.transform.location.x
current_y = measurements.player_measurements.transform.location.y
# If vehicle has stopped for pedestiran, vehicle or traffic light increase timeout by 1 unit
if min(controller_state['stop_pedestrian'], controller_state['stop_vehicle'],\
controller_state['stop_traffic_lights']) <= 0.3 :
time_out += (measurements.game_timestamp -
current_timestamp) / 1000
#logging.info("Controller sis Inputting:")
#logging.info('Steer = %f Throttle = %f Brake = %f ',
# control.steer, control.throttle, control.brake)
current_timestamp = measurements.game_timestamp
# Get the distance travelled until now
distance = sldist([current_x, current_y],
[target.location.x, target.location.y])
# Write status of the run on verbose mode
#logging.info('Status:')
print(
f"Distance to target: {float(distance):.2f} \t Time_taken: {(current_timestamp -initial_timestamp)/1000 :.2f}\t Timeout : {time_out :.2f}"
)
# Check if reach the target
data["Distance-Travelled"] = data["Total-Distance"] - distance
if distance < self._distance_for_success:
success = True
break
if (current_timestamp - initial_timestamp) / 1000 > time_out:
data["Intersection-Offroad"] = 0
data["Intersection-Lane"] = 0
data["Collision-Pedestrian"] = 0
data["Collision-Vehicle"] = 0
data["Collision-Other"] = 0
success = False
print("Timeout")
break
# Increment the vectors and append the measurements and controls.
frame += 1
measurement_vec.append(measurements.player_measurements)
control_vec.append(control)
#Stop in case of collision or excessive lane infraction
if measurements.player_measurements.collision_pedestrians >300 or \
measurements.player_measurements.collision_vehicles > 300 or \
measurements.player_measurements.collision_other>300:
if measurements.player_measurements.collision_pedestrians > 300:
data["Collision-Pedestrian"] = 1
elif measurements.player_measurements.collision_vehicles > 300:
data["Collision-Vehicle"] = 1
elif measurements.player_measurements.collision_other > 300:
data["Collision-Other"] = 1
data["Intersection-Offroad"] = 0
data["Intersection-Lane"] = 0
print("Collison detected")
success = False
break
if data["Intersection-Lane"] > 0.5:
data["Intersection-Lane"] = 1
data["Intersection-Offroad"] = 0
data["Collision-Pedestrian"] = 0
data["Collision-Vehicle"] = 0
data["Collision-Other"] = 0
print("Lane Infraction")
success = False
break
elif data["Intersection-Offroad"] > 0.5:
data["Intersection-Offroad"] = 1
data["Intersection-Lane"] = 0
data["Collision-Pedestrian"] = 0
data["Collision-Vehicle"] = 0
data["Collision-Other"] = 0
print("Lane Infraction")
success = False
break
if success:
data["Intersection-Offroad"] = 0
data["Intersection-Lane"] = 0
data["Collision-Pedestrian"] = 0
data["Collision-Vehicle"] = 0
data["Collision-Other"] = 0
return 1, measurement_vec, control_vec, float(
current_timestamp - initial_timestamp) / 1000.0, distance, data
return 0, measurement_vec, control_vec, time_out, distance, data
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):
self.config = config
self.config["x_res"], self.config["y_res"] = IMG_SIZE
self.city = self.config["server_map"].split("/")[-1]
if self.config["enable_planner"]:
self.planner = Planner(self.city)
# The Action Space
if config["discrete_actions"]:
self.action_space = Discrete(
len(DISCRETE_ACTIONS
)) # It will be transformed to continuous 2D action.
else:
self.action_space = Box(-1.0, 1.0, shape=(2, ),
dtype=np.float32) # 2D action.
if config["use_sensor"] == 'use_semantic':
image_space = Box(0.0,
255.0,
shape=(config["y_res"], config["x_res"],
1 * config["framestack"]),
dtype=np.float32)
elif config["use_sensor"] in ['use_depth', 'use_logdepth']:
image_space = Box(0.0,
255.0,
shape=(config["y_res"], config["x_res"],
1 * config["framestack"]),
dtype=np.float32)
elif config["use_sensor"] == 'use_rgb':
image_space = Box(0,
255,
shape=(config["y_res"], config["x_res"],
NUM_CHANNELS),
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._global_step = 0
def init_server(self):
print("Initializing new Carla server...")
self._global_step = 0
# 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._global_step = 0
self.prev_measurement = None
self.prev_image = None
self.episode_id = datetime.today().strftime("%Y-%m-%d_%H-%M-%S_%f")
self.measurements_file = None
# Create a CarlaSettings object. This object is a wrapper around
# the CarlaSettings.ini file. Here we set the configuration we
# want for the new episode.
settings = CarlaSettings()
self.scenario = random.choice(self.config["scenarios"])
assert self.scenario["city"] == self.city, (self.scenario, self.city)
self.weather = random.choice(self.scenario["weather_distribution"])
settings.set(
SynchronousMode=True,
# ServerTimeOut=10000, # CarlaSettings: no key named 'ServerTimeOut'
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=self.scenario["num_vehicles"],
NumberOfPedestrians=self.scenario["num_pedestrians"],
WeatherId=self.weather)
settings.randomize_seeds()
if self.config["use_sensor"] == 'use_semantic':
camera0 = Camera("CameraSemantic",
PostProcessing="SemanticSegmentation")
camera0.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
# camera0.set_position(30, 0, 130)
camera0.set(FOV=120)
camera0.set_position(2.0, 0.0, 1.4)
camera0.set_rotation(0.0, 0.0, 0.0)
settings.add_sensor(camera0)
if self.config["use_sensor"] in ['use_depth', 'use_logdepth']:
camera1 = Camera("CameraDepth", PostProcessing="Depth")
camera1.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
# camera1.set_position(30, 0, 130)
camera1.set(FOV=120)
camera1.set_position(2.0, 0.0, 1.4)
camera1.set_rotation(0.0, 0.0, 0.0)
settings.add_sensor(camera1)
if self.config["use_sensor"] == 'use_rgb':
camera2 = Camera("CameraRGB")
camera2.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
# camera2.set_position(30, 0, 130)
# camera2.set_position(0.3, 0.0, 1.3)
camera2.set(FOV=120)
camera2.set_position(2.0, 0.0, 1.4)
camera2.set_rotation(0.0, 0.0, 0.0)
settings.add_sensor(camera2)
if self.config["use_sensor"] == 'use_2rgb':
camera_l = Camera("CameraRGB_L")
camera_l.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
camera_l.set(FOV=120)
camera_l.set_position(2.0, -0.1, 1.4)
camera_l.set_rotation(0.0, 0.0, 0.0)
settings.add_sensor(camera_l)
camera_r = Camera("CameraRGB_R")
camera_r.set_image_size(self.config["render_x_res"],
self.config["render_y_res"])
camera_r.set(FOV=120)
camera_r.set_position(2.0, 0.1, 1.4)
camera_r.set_rotation(0.0, 0.0, 0.0)
settings.add_sensor(camera_r)
# Setup start and end positions
scene = self.client.load_settings(settings)
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"], self.start_coord,
self.scenario["end_pos_id"], self.end_coord))
# Notify the server that we want to start the episode at the
# player_start index. This function blocks until the server is ready
# to start the episode.
print("Starting new episode...")
self.client.start_episode(self.scenario["start_pos_id"])
# remove the vehicle dropping when starting a new episode.
cnt = 1
z1 = 0
zero_control = VehicleControl()
while (cnt < 3):
self.client.send_control(
zero_control
) # VehicleControl().steer = 0, VehicleControl().throttle = 0, VehicleControl().reverse = False
z0 = z1
z1 = self.client.read_data(
)[0].player_measurements.transform.location.z
print(z1)
if z0 - z1 == 0:
cnt += 1
print('Starting new episode done.\n')
# Process observations: self._read_observation() returns image and py_measurements.
image, py_measurements = self._read_observation()
self.prev_measurement = py_measurements
return self.encode_obs(self.preprocess_image(image), py_measurements)
def encode_obs(self, image, py_measurements):
assert self.config["framestack"] in [1, 2]
prev_image = self.prev_image
self.prev_image = image
if prev_image is None:
prev_image = image
if self.config["framestack"] == 2:
image = np.concatenate([prev_image, image], axis=2)
if ENCODE:
image = np.concatenate([
image, COMMAND_ORDINAL[py_measurements["next_command"]] * 50 *
np.ones(list(image.shape[:2]) + [1])
],
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,
np.array(1, np.float32).astype(np.float32))
# image, py_measurements = self._read_observation() ---> self.preprocess_image(image) ---> step observation output
# @set_timeout(10)
def _step(self, action):
self._global_step += 1
# print(self._global_step)
if self.config["discrete_actions"]:
action = DISCRETE_ACTIONS[int(action)] # Carla action is 2D.
assert len(action) == 2, "Invalid action {}".format(action)
if self.config["squash_action_logits"]:
forward = 2 * float(sigmoid(action[0]) - 0.5)
throttle = float(np.clip(forward, 0, 1))
brake = float(np.abs(np.clip(forward, -1, 0)))
steer = 2 * float(sigmoid(action[1]) - 0.5)
else:
throttle = float(np.clip(action[0], 0, 1))
brake = float(np.abs(np.clip(action[0], -1, 0)))
steer = float(np.clip(action[1], -1, 1))
# reverse and hand_brake are disabled.
reverse = False
hand_brake = False
if self.config["verbose"]:
print("steer", steer, "throttle", throttle, "brake", brake,
"reverse", reverse)
self.client.send_control(steer=steer,
throttle=throttle,
brake=brake,
hand_brake=hand_brake,
reverse=reverse)
# Process observations: self._read_observation() returns image and py_measurements.
image, py_measurements = self._read_observation()
if self.config["verbose"]:
print("Next command", py_measurements["next_command"])
if type(action) is np.ndarray:
py_measurements["action"] = [float(a) for a in action]
else:
py_measurements["action"] = action
py_measurements["control"] = {
"steer": steer,
"throttle": throttle,
"brake": brake,
"reverse": reverse,
"hand_brake": hand_brake,
}
# compute reward
reward = compute_reward(self, self.prev_measurement, py_measurements)
self.total_reward += reward
py_measurements["reward"] = reward
py_measurements["total_reward"] = self.total_reward
# done or not
# done = False
done = ((self.num_steps > self.scenario["max_steps"]
or py_measurements["next_command"] == "REACH_GOAL" or
(self.config["early_terminate_on_collision"]
and collided_done(py_measurements)))
and self._global_step > 51)
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)
info = np.array(COMMAND_ORDINAL[py_measurements["next_command"]],
np.float32).astype(np.float32)
# print(self.encode_obs(image, py_measurements)[0].shape)
return (self.encode_obs(image, py_measurements), reward, done, info)
def images_to_video(self):
videos_dir = os.path.join(CARLA_OUT_PATH, "Videos")
if not os.path.exists(videos_dir):
os.makedirs(videos_dir)
ffmpeg_cmd = (
"ffmpeg -loglevel -8 -r 60 -f image2 -s {x_res}x{y_res} "
"-start_number 0 -i "
"{img}_%04d.jpg -vcodec libx264 {vid}.mp4 && rm -f {img}_*.jpg "
).format(x_res=self.config["render_x_res"],
y_res=self.config["render_y_res"],
vid=os.path.join(videos_dir, self.episode_id),
img=os.path.join(CARLA_OUT_PATH, "CameraRGB",
self.episode_id))
print("Executing ffmpeg command", ffmpeg_cmd)
subprocess.call(ffmpeg_cmd, shell=True)
def preprocess_image(self, image):
if self.config[
"use_sensor"] == 'use_semantic': # image.data: uint8(0 ~ 12)
data = image.data * 21 # data: uint8(0 ~ 255)
data = data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 1)
data = cv2.resize(data,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = np.expand_dims(
data, 2) # data: uint8(0 ~ 255), shape(y_res, x_res, 1)
elif self.config["use_sensor"] == 'use_depth': # depth: float64(0 ~ 1)
# data = (image.data - 0.5) * 2
data = image.data * 255 # data: float64(0 ~ 255)
data = data.reshape(self.config["render_y_res"],
self.config["render_x_res"],
1) # shape(render_y_res,render_x_res,1)
data = cv2.resize(
data, (self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA) # shape(y_res, x_res)
data = np.expand_dims(
data, 2) # data: float64(0 ~ 255), shape(y_res, x_res, 1)
elif self.config[
"use_sensor"] == 'use_logdepth': # depth: float64(0 ~ 1)
data = (np.log(image.data) +
7.0) * 255.0 / 7.0 # data: float64(0 ~ 255)
data = data.reshape(self.config["render_y_res"],
self.config["render_x_res"],
1) # shape(render_y_res,render_x_res,1)
data = cv2.resize(
data, (self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA) # shape(y_res, x_res)
data = np.expand_dims(
data, 2) # data: float64(0 ~ 255), shape(y_res, x_res, 1)
elif self.config["use_sensor"] == 'use_rgb':
data = image.data.reshape(self.config["render_y_res"],
self.config["render_x_res"], 3)
data = cv2.resize(
data,
(self.config["x_res"], self.config["y_res"]
), # data: uint8(0 ~ 255), shape(y_res, x_res, 3)
interpolation=cv2.INTER_AREA)
# data = (data.astype(np.float32) - 128) / 128
elif self.config["use_sensor"] == 'use_2rgb':
data_l, data_r = image[0].data, image[0].data
data_l = data_l.reshape(self.config["render_y_res"],
self.config["render_x_res"], 3)
data_r = data_r.reshape(self.config["render_y_res"],
self.config["render_x_res"], 3)
data_l = cv2.resize(data_l,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data_r = cv2.resize(data_r,
(self.config["x_res"], self.config["y_res"]),
interpolation=cv2.INTER_AREA)
data = np.concatenate(
(data_l, data_r),
axis=2) # data: uint8(0 ~ 255), shape(y_res, x_res, 6)
return data
def _read_observation(self):
# Read the data produced by the server this frame.
measurements, sensor_data = self.client.read_data()
# Print some of the measurements.
if self.config["verbose"]:
print_measurements(measurements)
observation = None
if self.config["use_sensor"] == 'use_semantic':
camera_name = "CameraSemantic"
elif self.config["use_sensor"] in ['use_depth', 'use_logdepth']:
camera_name = "CameraDepth"
elif self.config["use_sensor"] == 'use_rgb':
camera_name = "CameraRGB"
elif self.config["use_sensor"] == 'use_2rgb':
camera_name = ["CameraRGB_L", "CameraRGB_R"]
# for name, image in sensor_data.items():
# if name == camera_name:
# observation = image
if camera_name == ["CameraRGB_L", "CameraRGB_R"]:
observation = [
sensor_data["CameraRGB_L"], sensor_data["CameraRGB_R"]
]
else:
observation = sensor_data[camera_name]
cur = measurements.player_measurements
if self.config["enable_planner"]:
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
])
else:
distance_to_goal = -1
distance_to_goal_euclidean = float(
np.linalg.norm([
cur.transform.location.x - self.end_pos.location.x,
cur.transform.location.y - self.end_pos.location.y
]))
py_measurements = {
"episode_id": self.episode_id,
"step": self.num_steps,
"x": cur.transform.location.x,
"y": cur.transform.location.y,
"x_orient": cur.transform.orientation.x,
"y_orient": cur.transform.orientation.y,
"forward_speed": cur.forward_speed,
"distance_to_goal": distance_to_goal,
"distance_to_goal_euclidean": distance_to_goal_euclidean,
"collision_vehicles": cur.collision_vehicles,
"collision_pedestrians": cur.collision_pedestrians,
"collision_other": cur.collision_other,
"intersection_offroad": cur.intersection_offroad,
"intersection_otherlane": cur.intersection_otherlane,
"weather": self.weather,
"map": self.config["server_map"],
"start_coord": self.start_coord,
"end_coord": self.end_coord,
"current_scenario": self.scenario,
"x_res": self.config["x_res"],
"y_res": self.config["y_res"],
"num_vehicles": self.scenario["num_vehicles"],
"num_pedestrians": self.scenario["num_pedestrians"],
"max_steps": self.scenario["max_steps"],
"next_command": next_command,
}
if CARLA_OUT_PATH and self.config["log_images"]:
for name, image in sensor_data.items():
out_dir = os.path.join(CARLA_OUT_PATH, name)
if not os.path.exists(out_dir):
os.makedirs(out_dir)
out_file = os.path.join(
out_dir, "{}_{:>04}.jpg".format(self.episode_id,
self.num_steps))
scipy.misc.imsave(out_file,
image.data) # image.data without preprocess.
assert observation is not None, sensor_data
return observation, py_measurements
class DrivingBenchmark(object):
"""
The Benchmark class, controls the execution of the benchmark interfacing
an Agent class with a set Suite.
The benchmark class must be inherited with a class that defines the
all the experiments to be run by the agent
"""
def __init__(self,
city_name='Town01',
name_to_save='Test',
continue_experiment=False,
save_images=False,
distance_for_success=2.0):
"""
Args
city_name:
name_to_save:
continue_experiment:
save_images:
distance_for_success:
collisions_as_failure: if this flag is set to true, episodes will terminate as failure, when the car collides.
"""
self.__metaclass__ = abc.ABCMeta
self._city_name = city_name
self._base_name = name_to_save
# The minimum distance for arriving into the goal point in
# order to consider ir a success
self._distance_for_success = distance_for_success
# The object used to record the benchmark and to able to continue after
self._recording = Recording(name_to_save=name_to_save,
continue_experiment=continue_experiment,
save_images=save_images)
# We have a default planner instantiated that produces high level commands
self._planner = Planner(city_name)
self._map = self._planner._city_track.get_map()
# TO keep track of the previous collisions
self._previous_pedestrian_collision = 0
self._previous_vehicle_collision = 0
self._previous_other_collision = 0
def benchmark_agent(self, experiment_suite, agent, client):
"""
Function to benchmark the agent.
It first check the log file for this benchmark.
if it exist it continues from the experiment where it stopped.
Args:
experiment_suite
agent: an agent object with the run step class implemented.
client:
Return:
A dictionary with all the metrics computed from the
agent running the set of experiments.
"""
# Instantiate a metric object that will be used to compute the metrics for
# the benchmark afterwards.
metrics_object = Metrics(experiment_suite.metrics_parameters,
experiment_suite.dynamic_tasks)
# Function return the current pose and task for this benchmark.
start_pose, start_experiment = self._recording.get_pose_and_experiment(
experiment_suite.get_number_of_poses_task())
logging.info('START')
for experiment in experiment_suite.get_experiments(
)[int(start_experiment):]:
positions = client.load_settings(
experiment.conditions).player_start_spots
self._recording.log_start(experiment.task)
for pose in experiment.poses[start_pose:]:
for rep in range(experiment.repetitions):
start_index = pose[0]
end_index = pose[1]
client.start_episode(start_index)
# Print information on
logging.info('======== !!!! ==========')
logging.info(' Start Position %d End Position %d ',
start_index, end_index)
self._recording.log_poses(start_index, end_index,
experiment.Conditions.WeatherId)
# Calculate the initial distance for this episode
initial_distance = \
sldist(
[positions[start_index].location.x, positions[start_index].location.y],
[positions[end_index].location.x, positions[end_index].location.y])
time_out = experiment_suite.calculate_time_out(
self._get_shortest_path(positions[start_index],
positions[end_index]))
logging.info('Timeout for Episode: %f', time_out)
# running the agent
(result, reward_vec, control_vec, final_time, remaining_distance, col_ped,
col_veh, col_oth, number_of_red_lights, number_of_green_lights) = \
self._run_navigation_episode(
agent, client, time_out, positions[end_index],
str(experiment.Conditions.WeatherId) + '_'
+ str(experiment.task) + '_' + str(start_index)
+ '.' + str(end_index), experiment_suite.metrics_parameters,
experiment_suite.collision_as_failure,
experiment_suite.traffic_light_as_failure)
# Write the general status of the just ran episode
self._recording.write_summary_results(
experiment, pose, rep, initial_distance,
remaining_distance, final_time, time_out, result,
col_ped, col_veh, col_oth, number_of_red_lights,
number_of_green_lights)
# Write the details of this episode.
self._recording.write_measurements_results(
experiment, rep, pose, reward_vec, control_vec)
if result > 0:
logging.info(
'+++++ Target achieved in %f seconds! +++++',
final_time)
else:
logging.info('----- Timeout! -----')
start_pose = 0
self._recording.log_end()
return metrics_object.compute(self._recording.path)
def get_path(self):
"""
Returns the path were the log was saved.
"""
return self._recording.path
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 _get_shortest_path(self, start_point, end_point):
"""
Calculates the shortest path between two points considering the road netowrk
"""
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
])
def _has_agent_collided(self, measurement, metrics_parameters):
"""
This function must have a certain state and only look to one measurement.
"""
collided_veh = 0
collided_ped = 0
collided_oth = 0
if (measurement.collision_vehicles - self._previous_vehicle_collision) \
> metrics_parameters['collision_vehicles']['threshold']/2.0:
collided_veh = 1
if (measurement.collision_pedestrians - self._previous_pedestrian_collision) \
> metrics_parameters['collision_pedestrians']['threshold']/2.0:
collided_ped = 1
if (measurement.collision_other - self._previous_other_collision) \
> metrics_parameters['collision_other']['threshold']/2.0:
collided_oth = 1
self._previous_pedestrian_collision = measurement.collision_pedestrians
self._previous_vehicle_collision = measurement.collision_vehicles
self._previous_other_collision = measurement.collision_other
return collided_ped, collided_veh, collided_oth
def _is_traffic_light_active(self, agent, orientation):
x_agent = agent.traffic_light.transform.location.x
y_agent = agent.traffic_light.transform.location.y
def search_closest_lane_point(x_agent, y_agent, depth):
step_size = 4
if depth > 1:
return None
try:
degrees = self._map.get_lane_orientation_degrees(
[x_agent, y_agent, 38])
#print (degrees)
except:
return None
if not self._map.is_point_on_lane([x_agent, y_agent, 38]):
#print (" Not on lane ")
result = search_closest_lane_point(x_agent + step_size,
y_agent, depth + 1)
if result is not None:
return result
result = search_closest_lane_point(x_agent,
y_agent + step_size,
depth + 1)
if result is not None:
return result
result = search_closest_lane_point(x_agent + step_size,
y_agent + step_size,
depth + 1)
if result is not None:
return result
result = search_closest_lane_point(x_agent + step_size,
y_agent - step_size,
depth + 1)
if result is not None:
return result
result = search_closest_lane_point(x_agent - step_size,
y_agent + step_size,
depth + 1)
if result is not None:
return result
result = search_closest_lane_point(x_agent - step_size,
y_agent, depth + 1)
if result is not None:
return result
result = search_closest_lane_point(x_agent,
y_agent - step_size,
depth + 1)
if result is not None:
return result
result = search_closest_lane_point(x_agent - step_size,
y_agent - step_size,
depth + 1)
if result is not None:
return result
else:
#print(" ON Lane ")
if degrees < 6:
return [x_agent, y_agent]
else:
return None
closest_lane_point = search_closest_lane_point(x_agent, y_agent, 0)
car_direction = math.atan2(orientation.y, orientation.x) + 3.1415
if car_direction > 6.0:
car_direction -= 6.0
return math.fabs(
car_direction - self._map.get_lane_orientation_degrees(
[closest_lane_point[0], closest_lane_point[1], 38])) < 1
def _test_for_traffic_lights(self, measurement):
"""
This function tests if the car passed into a traffic light, returning 'red'
if it crossed a red light , 'green' if it crossed a green light or none otherwise
Args:
measurement: all the measurements collected by carla 0.8.4
Returns:
"""
def is_on_burning_point(_map, location):
# We get the current lane orientation
ori_x, ori_y = _map.get_lane_orientation(
[location.x, location.y, 38])
# We test to walk in direction of the lane
future_location_x = location.x
future_location_y = location.y
for i in range(3):
future_location_x += ori_x
future_location_y += ori_y
# Take a point on a intersection in the future
location_on_intersection_x = future_location_x + 2 * ori_x
location_on_intersection_y = future_location_y + 2 * ori_y
if not _map.is_point_on_intersection([future_location_x,
future_location_y,
38]) and \
_map.is_point_on_intersection([location_on_intersection_x,
location_on_intersection_y,
38]):
return True
return False
# Check nearest traffic light with the correct orientation state.
player_x = measurement.player_measurements.transform.location.x
player_y = measurement.player_measurements.transform.location.y
# The vehicle is on an intersection
# THIS IS THE PLACE TO VERIFY FOR A TL BURN
for agent in measurement.non_player_agents:
if agent.HasField('traffic_light'):
if not self._map.is_point_on_intersection(
[player_x, player_y, 38]):
x_agent = agent.traffic_light.transform.location.x
y_agent = agent.traffic_light.transform.location.y
tl_vector, tl_dist = get_vec_dist(x_agent, y_agent,
player_x, player_y)
if self._is_traffic_light_active(
agent, measurement.player_measurements.transform.
orientation):
if is_on_burning_point(self._map,
measurement.player_measurements.transform.location)\
and tl_dist < 6.0:
if agent.traffic_light.state != 0: # Not green
return 'red'
else:
return 'green'
return None
def _run_navigation_episode(self, agent, client, time_out, target,
episode_name, metrics_parameters,
collision_as_failure,
traffic_light_as_failure):
"""
Run one episode of the benchmark (Pose) for a certain agent.
Args:
agent: the agent object
client: an object of the carla client to communicate
with the CARLA simulator
time_out: the time limit to complete this episode
target: the target to reach
episode_name: The name for saving images of this episode
metrics_object: The metrics object to check for collisions
"""
# Send an initial command.
measurements, sensor_data = client.read_data()
client.send_control(VehicleControl())
initial_timestamp = measurements.game_timestamp
current_timestamp = initial_timestamp
# The vector containing all measurements produced on this episode
measurement_vec = []
# The vector containing all controls produced on this episode
control_vec = []
frame = 0
distance = 10000
col_ped, col_veh, col_oth = 0, 0, 0
traffic_light_state, number_red_lights, number_green_lights = None, 0, 0
fail = False
success = False
not_count = 0
while not fail and not success:
# Read data from server with the client
measurements, sensor_data = client.read_data()
# The directions to reach the goal are calculated.
directions = self._get_directions(
measurements.player_measurements.transform, target)
# Agent process the data.
control = agent.run_step(measurements, sensor_data, directions,
target)
# Send the control commands to the vehicle
client.send_control(control)
# save images if the flag is activated
self._recording.save_images(sensor_data, episode_name, frame)
current_x = measurements.player_measurements.transform.location.x
current_y = measurements.player_measurements.transform.location.y
logging.info("Controller is Inputting:")
logging.info('Steer = %f Throttle = %f Brake = %f ', control.steer,
control.throttle, control.brake)
current_timestamp = measurements.game_timestamp
logging.info('Timestamp %f', current_timestamp)
# Get the distance travelled until now
distance = sldist([current_x, current_y],
[target.location.x, target.location.y])
# Write status of the run on verbose mode
logging.info('Status:')
logging.info('[d=%f] c_x = %f, c_y = %f ---> t_x = %f, t_y = %f',
float(distance), current_x, current_y,
target.location.x, target.location.y)
# Check if reach the target
col_ped, col_veh, col_oth = self._has_agent_collided(
measurements.player_measurements, metrics_parameters)
# test if car crossed the traffic light
traffic_light_state = self._test_for_traffic_lights(measurements)
if traffic_light_state == 'red' and not_count == 0:
number_red_lights += 1
not_count = 20
elif traffic_light_state == 'green' and not_count == 0:
number_green_lights += 1
not_count = 20
else:
not_count -= 1
not_count = max(0, not_count)
if distance < self._distance_for_success:
success = True
elif (current_timestamp - initial_timestamp) > (time_out * 1000):
fail = True
elif collision_as_failure and (col_ped or col_veh or col_oth):
fail = True
elif traffic_light_as_failure and traffic_light_state == 'red':
fail = True
logging.info('Traffic Lights:')
logging.info('red %f green %f, total %f', number_red_lights,
number_green_lights,
number_red_lights + number_green_lights)
# Increment the vectors and append the measurements and controls.
frame += 1
measurement_vec.append(measurements.player_measurements)
control_vec.append(control)
if success:
return 1, measurement_vec, control_vec, float(
current_timestamp - initial_timestamp) / 1000.0, distance, col_ped, col_veh, col_oth, \
number_red_lights, number_green_lights
return 0, measurement_vec, control_vec, time_out, distance, col_ped, col_veh, col_oth, \
number_red_lights, number_green_lights
class DrivingBenchmark(object):
"""
The Benchmark class, controls the execution of the benchmark interfacing
an Agent class with a set Suite.
The benchmark class must be inherited with a class that defines the
all the experiments to be run by the agent
"""
def __init__(
self,
city_name='Town01',
name_to_save='Test',
continue_experiment=False,
save_images=False,
distance_for_success=2.0
):
self.__metaclass__ = abc.ABCMeta
self._city_name = city_name
self._base_name = name_to_save
# The minimum distance for arriving into the goal point in
# order to consider ir a success
self._distance_for_success = distance_for_success
# The object used to record the benchmark and to able to continue after
self._recording = Recording(name_to_save=name_to_save,
continue_experiment=continue_experiment,
save_images=save_images
)
# We have a default planner instantiated that produces high level commands
self._planner = Planner(city_name)
self.carla_map = CarlaMap(city_name, 0.1653, 50)
def benchmark_agent(self, experiment_suite, agent, client):
"""
Function to benchmark the agent.
It first check the log file for this benchmark.
if it exist it continues from the experiment where it stopped.
Args:
experiment_suite
agent: an agent object with the run step class implemented.
client:
Return:
A dictionary with all the metrics computed from the
agent running the set of experiments.
"""
# Instantiate a metric object that will be used to compute the metrics for
# the benchmark afterwards.
metrics_object = Metrics(experiment_suite.metrics_parameters,
experiment_suite.dynamic_tasks)
# Function return the current pose and task for this benchmark.
start_pose, start_experiment = self._recording.get_pose_and_experiment(
experiment_suite.get_number_of_poses_task())
logging.info('START')
for experiment in experiment_suite.get_experiments()[int(start_experiment):]:
positions = client.load_settings(
experiment.conditions).player_start_spots
self._recording.log_start(experiment.task)
for pose in experiment.poses[start_pose:]:
for rep in range(experiment.repetitions):
start_index = pose[0]
end_index = pose[1]
client.start_episode(start_index)
# Print information on
logging.info('======== !!!! ==========')
logging.info(' Start Position %d End Position %d ',
start_index, end_index)
self._recording.log_poses(start_index, end_index,
experiment.Conditions.WeatherId)
# Calculate the initial distance for this episode
initial_distance = \
sldist(
[positions[start_index].location.x, positions[start_index].location.y],
[positions[end_index].location.x, positions[end_index].location.y])
time_out = experiment_suite.calculate_time_out(
self._get_shortest_path(positions[start_index], positions[end_index]))
# running the agent
(result, reward_vec, control_vec, final_time, remaining_distance) = \
self._run_navigation_episode(
agent, client, time_out, positions[end_index],
str(experiment.Conditions.WeatherId) + '_'
+ str(experiment.task) + '_' + str(start_index)
+ '.' + str(end_index))
# Write the general status of the just ran episode
self._recording.write_summary_results(
experiment, pose, rep, initial_distance,
remaining_distance, final_time, time_out, result)
# Write the details of this episode.
self._recording.write_measurements_results(experiment, rep, pose, reward_vec,
control_vec)
if result > 0:
logging.info('+++++ Target achieved in %f seconds! +++++',
final_time)
else:
logging.info('----- Timeout! -----')
start_pose = 0
self._recording.log_end()
return metrics_object.compute(self._recording.path)
def get_path(self):
"""
Returns the path were the log was saved.
"""
return self._recording.path
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 _get_shortest_path(self, start_point, end_point):
"""
Calculates the shortest path between two points considering the road netowrk
"""
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])
def load_empty_trajectory(self):
original_path = "./drive_interfaces/carla/carla_client/carla/planner/" + self._city_name + ".png"
self.trajectory_img = cv2.imread(original_path)
def update_trajectory(self, curr_x, curr_y, tar_x, tar_y, is_first, directions):
cur = self.carla_map.convert_to_pixel([curr_x, curr_y, .22])
cur = [int(cur[1]), int(cur[0])]
tar = self.carla_map.convert_to_pixel([tar_x, tar_y, .22])
tar = [int(tar[1]), int(tar[0])]
print("current location", cur, "final location", tar)
if is_first:
trj_sz = 10
else:
trj_sz = 3
directions_to_color={0.0: [255, 0, 0], 2.0: [255, 0, 0], 5.0: [255, 0, 0],
3.0: [0, 255, 0], 4.0: [0, 0, 255]}
color = directions_to_color[directions]
self.trajectory_img[cur[0] - trj_sz:cur[0] + trj_sz, cur[1] - trj_sz:cur[1] + trj_sz, 0] = color[0]
self.trajectory_img[cur[0] - trj_sz:cur[0] + trj_sz, cur[1] - trj_sz:cur[1] + trj_sz, 1] = color[1]
self.trajectory_img[cur[0] - trj_sz:cur[0] + trj_sz, cur[1] - trj_sz:cur[1] + trj_sz, 2] = color[2]
tar_sz = 10
self.trajectory_img[tar[0] - tar_sz:tar[0] + tar_sz, tar[1] - tar_sz:tar[1] + tar_sz, 0] = 0
self.trajectory_img[tar[0] - tar_sz:tar[0] + tar_sz, tar[1] - tar_sz:tar[1] + tar_sz, 1] = 0
self.trajectory_img[tar[0] - tar_sz:tar[0] + tar_sz, tar[1] - tar_sz:tar[1] + tar_sz, 2] = 0
def save_trajectory_image(self, episode_name):
out_name = os.path.join(self._recording._path, '_images/episode_{:s}_trajectory.png'.format(episode_name))
folder = os.path.dirname(out_name)
if not os.path.isdir(folder):
os.makedirs(folder)
cv2.imwrite(out_name, self.trajectory_img)
def measurements_to_pos_yaw(self, measurements):
pos = [measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y ]
ori = [measurements.player_measurements.transform.orientation.x,
measurements.player_measurements.transform.orientation.y,
measurements.player_measurements.transform.orientation.z ]
city_name = {"Town01": "01",
"Town02": "02",
"RFS_MAP": "10",
"Exp_Town": "11",
"Exp_Town02": "11",
"Exp_Town01_02Shoulder": "13"}[self._city_name]
return {"town_id": city_name, "pos": pos, "ori": ori}
def _run_navigation_episode(
self,
agent,
client,
time_out,
target,
episode_name):
"""
Run one episode of the benchmark (Pose) for a certain agent.
Args:
agent: the agent object
client: an object of the carla client to communicate
with the CARLA simulator
time_out: the time limit to complete this episode
target: the target to reach
episode_name: The name for saving images of this episode
"""
# Send an initial command.
measurements, sensor_data = client.read_data()
client.send_control(VehicleControl())
initial_timestamp = measurements.game_timestamp
current_timestamp = initial_timestamp
# The vector containing all measurements produced on this episode
measurement_vec = []
# The vector containing all controls produced on this episode
control_vec = []
frame = 0
distance = 10000
success = False
self.load_empty_trajectory()
agent.debug_i = 0
agent.temp_image_path = self._recording._path
stuck_counter = 0
pre_x = 0.0
pre_y = 0.0
last_collision_ago = 1000000000
is_first = True
while (current_timestamp - initial_timestamp) < (time_out * 1000) and not success:
# Read data from server with the client
measurements, sensor_data = client.read_data()
# The directions to reach the goal are calculated.
directions = self._get_directions(measurements.player_measurements.transform, target)
# Agent process the data.
control = agent.run_step(measurements, sensor_data, directions, target, self.measurements_to_pos_yaw(measurements))
# Send the control commands to the vehicle
client.send_control(control)
# save images if the flag is activated
#sensor_data = self._recording.yang_annotate_images(sensor_data, directions)
#self._recording.save_images(sensor_data, episode_name, frame)
current_x = measurements.player_measurements.transform.location.x
current_y = measurements.player_measurements.transform.location.y
logging.info("Controller is Inputting:")
logging.info('Steer = %f Throttle = %f Brake = %f ',
control.steer, control.throttle, control.brake)
# game timestamp is in microsecond
current_timestamp = measurements.game_timestamp
# Get the distance travelled until now
distance = sldist([current_x, current_y],
[target.location.x, target.location.y])
self.update_trajectory(current_x, current_y, target.location.x, target.location.y, is_first, directions)
is_first = False
# Write status of the run on verbose mode
logging.info('Status:')
logging.info(
'[d=%f] c_x = %f, c_y = %f ---> t_x = %f, t_y = %f',
float(distance), current_x, current_y, target.location.x,
target.location.y)
# Check if reach the target
if distance < self._distance_for_success:
success = True
# Increment the vectors and append the measurements and controls.
frame += 1
measurement_vec.append(measurements.player_measurements)
control_vec.append(control)
pm = measurements.player_measurements
if pm.collision_vehicles > 0.0 \
or pm.collision_pedestrians > 0.0 \
or pm.collision_other > 0.0:
last_collision_ago = 0
else:
last_collision_ago += 1
if sldist([current_x, current_y], [pre_x, pre_y]) < 0.1:
stuck_counter += 1
else:
stuck_counter = 0
pre_x = current_x
pre_y = current_y
if stuck_counter > 400:
print("breaking because of stucking too long")
break
if stuck_counter > 30 and last_collision_ago < 50:
#print("breaking because of collision and stuck")
#break
# disable the breaking
pass
if math.fabs(directions) < 0.1:
# The goal state is reached.
success = True
# convert the images saved by the underlying to a video
if self._recording._save_images:
# convert the saved images to a video, and store it in the right place
# we have to save the image within the carla_machine, since only that will know what's the cutting plane
# assume that the images lies around at ./temp/%05d.png
print("converting images to a video...")
out_name = os.path.join(self._recording._path, '_images/episode_{:s}.mp4'.format(episode_name))
folder = os.path.dirname(out_name)
if not os.path.isdir(folder):
os.makedirs(folder)
cmd = ["ffmpeg", "-y", "-i", agent.temp_image_path+"/%09d.png", "-c:v", "libx264", out_name]
call(" ".join(cmd), shell=True)
cmd = ["find", agent.temp_image_path, "-name", "00*png", "-print | xargs rm"]
call(" ".join(cmd), shell=True)
self.save_trajectory_image(episode_name)
if success:
return 1, measurement_vec, control_vec, float(
current_timestamp - initial_timestamp) / 1000.0, distance
return 0, measurement_vec, control_vec, time_out, distance
class DrivingBenchmark(object):
"""
The Benchmark class, controls the execution of the benchmark interfacing
an Agent class with a set Suite.
The benchmark class must be inherited with a class that defines the
all the experiments to be run by the agent
"""
def __init__(
self,
city_name='Town01',
name_to_save='Test',
continue_experiment=False,
save_images=False,
distance_for_success=2.0
):
self.__metaclass__ = abc.ABCMeta
self._city_name = city_name
self._base_name = name_to_save
# The minimum distance for arriving into the goal point in
# order to consider ir a success
self._distance_for_success = distance_for_success
# The object used to record the benchmark and to able to continue after
self._recording = Recording(name_to_save=name_to_save,
continue_experiment=continue_experiment,
save_images=save_images
)
# We have a default planner instantiated that produces high level commands
self._planner = Planner(city_name)
def benchmark_agent(self, experiment_suite, agent, client):
"""
Function to benchmark the agent.
It first check the log file for this benchmark.
if it exist it continues from the experiment where it stopped.
Args:
experiment_suite
agent: an agent object with the run step class implemented.
client:
Return:
A dictionary with all the metrics computed from the
agent running the set of experiments.
"""
# Instantiate a metric object that will be used to compute the metrics for
# the benchmark afterwards.
metrics_object = Metrics(experiment_suite.metrics_parameters,
experiment_suite.dynamic_tasks)
# Function return the current pose and task for this benchmark.
start_pose, start_experiment = self._recording.get_pose_and_experiment(
experiment_suite.get_number_of_poses_task())
logging.info('START')
for experiment in experiment_suite.get_experiments()[int(start_experiment):]:
positions = client.load_settings(
experiment.conditions).player_start_spots
self._recording.log_start(experiment.task)
for pose in experiment.poses[start_pose:]:
for rep in range(experiment.repetitions):
start_index = pose[0]
end_index = pose[1]
client.start_episode(start_index)
# Print information on
logging.info('======== !!!! ==========')
logging.info(' Start Position %d End Position %d ',
start_index, end_index)
self._recording.log_poses(start_index, end_index,
experiment.Conditions.WeatherId)
# Calculate the initial distance for this episode
initial_distance = \
sldist(
[positions[start_index].location.x, positions[start_index].location.y],
[positions[end_index].location.x, positions[end_index].location.y])
time_out = experiment_suite.calculate_time_out(
self._get_shortest_path(positions[start_index], positions[end_index]))
# running the agent
(result, reward_vec, control_vec, final_time, remaining_distance) = \
self._run_navigation_episode(
agent, client, time_out, positions[end_index],
str(experiment.Conditions.WeatherId) + '_'
+ str(experiment.task) + '_' + str(start_index)
+ '.' + str(end_index))
# Write the general status of the just ran episode
self._recording.write_summary_results(
experiment, pose, rep, initial_distance,
remaining_distance, final_time, time_out, result)
# Write the details of this episode.
self._recording.write_measurements_results(experiment, rep, pose, reward_vec,
control_vec)
if result > 0:
logging.info('+++++ Target achieved in %f seconds! +++++',
final_time)
else:
logging.info('----- Timeout! -----')
start_pose = 0
self._recording.log_end()
return metrics_object.compute(self._recording.path)
def get_path(self):
"""
Returns the path were the log was saved.
"""
return self._recording.path
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 _get_shortest_path(self, start_point, end_point):
"""
Calculates the shortest path between two points considering the road netowrk
"""
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])
def _run_navigation_episode(
self,
agent,
client,
time_out,
target,
episode_name):
"""
Run one episode of the benchmark (Pose) for a certain agent.
Args:
agent: the agent object
client: an object of the carla client to communicate
with the CARLA simulator
time_out: the time limit to complete this episode
target: the target to reach
episode_name: The name for saving images of this episode
"""
# Send an initial command.
measurements, sensor_data = client.read_data()
client.send_control(VehicleControl())
initial_timestamp = measurements.game_timestamp
current_timestamp = initial_timestamp
# The vector containing all measurements produced on this episode
measurement_vec = []
# The vector containing all controls produced on this episode
control_vec = []
frame = 0
distance = 10000
success = False
while (current_timestamp - initial_timestamp) < (time_out * 1000) and not success:
# Read data from server with the client
measurements, sensor_data = client.read_data()
# The directions to reach the goal are calculated.
directions = self._get_directions(measurements.player_measurements.transform, target)
# Agent process the data.
control = agent.run_step(measurements, sensor_data, directions, target)
# Send the control commands to the vehicle
client.send_control(control)
# save images if the flag is activated
self._recording.save_images(sensor_data, episode_name, frame)
current_x = measurements.player_measurements.transform.location.x
current_y = measurements.player_measurements.transform.location.y
logging.info("Controller is Inputting:")
logging.info('Steer = %f Throttle = %f Brake = %f ',
control.steer, control.throttle, control.brake)
current_timestamp = measurements.game_timestamp
# Get the distance travelled until now
distance = sldist([current_x, current_y],
[target.location.x, target.location.y])
# Write status of the run on verbose mode
logging.info('Status:')
logging.info(
'[d=%f] c_x = %f, c_y = %f ---> t_x = %f, t_y = %f',
float(distance), current_x, current_y, target.location.x,
target.location.y)
# Check if reach the target
if distance < self._distance_for_success:
success = True
# Increment the vectors and append the measurements and controls.
frame += 1
measurement_vec.append(measurements.player_measurements)
control_vec.append(control)
if success:
return 1, measurement_vec, control_vec, float(
current_timestamp - initial_timestamp) / 1000.0, distance
return 0, measurement_vec, control_vec, time_out, distance
class DrivingBenchmark(object):
"""
The Benchmark class, controls the execution of the benchmark interfacing
an Agent class with a set Suite.
The benchmark class must be inherited with a class that defines the
all the experiments to be run by the agent
"""
def __init__(self,
city_name='Town01',
name_to_save='Test',
continue_experiment=False,
save_images=False,
distance_for_success=2.0):
"""
Args
city_name:
name_to_save:
continue_experiment:
save_images:
distance_for_success:
collisions_as_failure: if this flag is set to true, episodes will terminate as failure, when the car collides.
"""
self.__metaclass__ = abc.ABCMeta
self._city_name = city_name
self._base_name = name_to_save
# The minimum distance for arriving into the goal point in
# order to consider ir a success
self._distance_for_success = distance_for_success
# The object used to record the benchmark and to able to continue after
self._recording = Recording(name_to_save=name_to_save,
continue_experiment=continue_experiment,
save_images=save_images)
# We have a default planner instantiated that produces high level commands
self._planner = Planner(city_name)
# TO keep track of the previous collisions
self._previous_pedestrian_collision = 0
self._previous_vehicle_collision = 0
self._previous_other_collision = 0
def benchmark_agent(self, experiment_suite, agent, client):
"""
Function to benchmark the agent.
It first check the log file for this benchmark.
if it exist it continues from the experiment where it stopped.
Args:
experiment_suite
agent: an agent object with the run step class implemented.
client:
Return:
A dictionary with all the metrics computed from the
agent running the set of experiments.sldi
"""
# Instantiate a metric object that will be used to compute the metrics for
# the benchmark afterwards.
metrics_object = Metrics(experiment_suite.metrics_parameters,
experiment_suite.dynamic_tasks)
# Function return the current pose and task for this benchmark.
start_pose, start_experiment = self._recording.get_pose_and_experiment(
experiment_suite.get_number_of_poses_task())
logging.info('START')
for experiment in experiment_suite.get_experiments(
)[int(start_experiment):]:
positions = client.load_settings(
experiment.conditions).player_start_spots
t = len(positions)
self._recording.log_start(experiment.task)
for pose in experiment.poses[start_pose:]:
for rep in range(experiment.repetitions):
start_index = pose[0]
end_index = pose[1]
client.start_episode(start_index)
# Print information on
logging.info('======== !!!! ==========')
logging.info(' Start Position %d End Position %d ',
start_index, end_index)
self._recording.log_poses(start_index, end_index,
experiment.Conditions.WeatherId)
# Calculate the initial distance for this episode
initial_distance = \
sldist(
[positions[start_index].location.x, positions[start_index].location.y],
[positions[end_index].location.x, positions[end_index].location.y])
time_out = experiment_suite.calculate_time_out(
self._get_shortest_path(positions[start_index],
positions[end_index]))
# running the agent
(result, reward_vec, control_vec, final_time, remaining_distance, col_ped, col_veh, col_oth,metList) = \
self._run_navigation_episode(
agent, client, time_out, positions[end_index],
str(experiment.Conditions.WeatherId) + '_'
+ str(experiment.task) + '_' + str(start_index)
+ '.' + str(end_index), experiment_suite.metrics_parameters,
experiment_suite.collision_as_failure)
# Write the general status of the just ran episode
self._recording.write_summary_results(
experiment, pose, rep, initial_distance,
remaining_distance, final_time, time_out, result,
col_ped, col_veh, col_oth)
# Write the details of this episode.
self._recording.write_measurements_results(
experiment, rep, pose, reward_vec, control_vec,
metList)
if result > 0:
logging.info(
'+++++ Target achieved in %f seconds! +++++',
final_time)
else:
logging.info('----- Timeout! -----')
start_pose = 0
self._recording.log_end()
return metrics_object.compute(self._recording.path)
def get_path(self):
"""
Returns the path were the log was saved.
"""
return self._recording.path
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 _get_shortest_path(self, start_point, end_point):
"""
Calculates the shortest path between two points considering the road netowrk
"""
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
])
def _has_agent_collided(self, measurement, metrics_parameters):
"""
This function must have a certain state and only look to one measurement.
"""
collided_veh = 0
collided_ped = 0
collided_oth = 0
if (measurement.collision_vehicles - self._previous_vehicle_collision) \
> metrics_parameters['collision_vehicles']['threshold']/2.0:
collided_veh = 1
if (measurement.collision_pedestrians - self._previous_pedestrian_collision) \
> metrics_parameters['collision_pedestrians']['threshold']/2.0:
collided_ped = 1
if (measurement.collision_other - self._previous_other_collision) \
> metrics_parameters['collision_other']['threshold']/2.0:
collided_oth = 1
self._previous_pedestrian_collision = measurement.collision_pedestrians
self._previous_vehicle_collision = measurement.collision_other
return collided_ped, collided_veh, collided_oth
def _is_agent_stuck(self, measurements, stuck_vec, old_coll):
# break the episode when the agent is stuck on a static object
coll_other = measurements.collision_other
coll_other -= old_coll
otherlane = measurements.intersection_otherlane > 0.4
offroad = measurements.intersection_offroad > 0.3
logging.info(
"offroad: {}, otherlane: {}, coll_other: {}, old_coll: {}".format(
offroad, otherlane, coll_other, old_coll))
# if still driving or got unstuck (v > 4km/h)
if measurements.forward_speed * 3.6 > 4:
cycle_signal(stuck_vec, 0)
if coll_other:
old_coll += coll_other
elif offroad or otherlane or coll_other:
cycle_signal(stuck_vec, 1)
else:
cycle_signal(stuck_vec, 0)
return all(stuck_vec), stuck_vec, old_coll
def _run_navigation_episode(self, agent, client, time_out, target,
episode_name, metrics_parameters,
collision_as_failure):
"""
Run one episode of the benchmark (Pose) for a certain agent.
Args:
agent: the agent object
client: an object of the carla client to communicate
with the CARLA simulator
time_out: the time limit to complete this episode
target: the target to reach
episode_name: The name for saving images of this episode
metrics_object: The metrics object to check for collisions
"""
# Send an initial command.
time.sleep(2)
measurements, sensor_data = client.read_data()
client.send_control(VehicleControl())
### Reset CAL agent
agent.reset_state()
initial_timestamp = measurements.game_timestamp
current_timestamp = initial_timestamp
# The vector containing all measurements produced on this episode
measurement_vec = []
# The vector containing all controls produced on this episode
control_vec = []
frame = 0
distance = 10000
col_ped, col_veh, col_oth = 0, 0, 0
fail = False
success = False
### own metrics
stuck_vec = [0] * 60 # measure for 60 frames (6 seconds)
center_distance_vec = []
old_collision_value = 0
direction_vec = []
#edited ash
metList = []
while not fail and not success:
# Read data from server with the client
measurements, sensor_data = client.read_data()
# The directions to reach the goal are calculated.
directions = self._get_directions(
measurements.player_measurements.transform, target)
# Agent process the data.
curr_time = time.time()
control, prediction = agent.run_step(measurements, sensor_data,
directions, target)
control.steer = measurements.player_measurements.autopilot_control.steer
curr_time = time.time() - curr_time
# Send the control commands to the vehicle
client.send_control(control)
# save images if the flag is activated
self._recording.save_images(sensor_data, episode_name, frame)
current_x = measurements.player_measurements.transform.location.x
current_y = measurements.player_measurements.transform.location.y
logging.info("Controller is Inputting:")
logging.info('Steer = %f Throttle = %f Brake = %f ', control.steer,
control.throttle, control.brake)
current_timestamp = measurements.game_timestamp
# Get the distance travelled until now
distance = sldist([current_x, current_y],
[target.location.x, target.location.y])
# Write status of the run on verbose mode
logging.info('Distance to target: %f', float(distance))
# Check if reach the target
col_ped, col_veh, col_oth = self._has_agent_collided(
measurements.player_measurements, metrics_parameters)
### CHANGE TO ORIGINAL CODE #####################
is_stuck, stuck_vec, old_collision_value = self._is_agent_stuck(
measurements.player_measurements, stuck_vec,
old_collision_value)
if distance < self._distance_for_success:
success = True
elif (current_timestamp - initial_timestamp) > (time_out * 1000):
fail = True
elif is_stuck:
fail = True
elif collision_as_failure and (col_ped or col_veh or col_oth):
fail = True
time_remain = (time_out * 1000 -
(current_timestamp - initial_timestamp)) / 1000
logging.info('Time remaining: %i m %i s', time_remain / 60,
time_remain % 60)
logging.info('')
# Increment the vectors and append the measurements and controls.
frame += 1
measurement_vec.append(measurements.player_measurements)
control_vec.append(control)
temp = agent.getMetData()
temp['game_timestamp'] = measurements.game_timestamp
metList.append(temp)
print("stp number: ", frame, " ", temp, " elapsed time: ",
curr_time)
#my code...
player = {}
peds = []
px = measurements.player_measurements.transform.location.x
py = measurements.player_measurements.transform.location.y
player['pos_x'] = px
player['pos_y'] = py
yaw = measurements.player_measurements.transform.rotation.yaw
yaw = math.radians(yaw)
sfile = open(
'/home/self-driving/Desktop/CARLA_0.8.2/Indranil/CAL-master_copy_new/python_client/_benchmarks_results/sigData.csv',
'a+')
pfile = open(
'/home/self-driving/Desktop/CARLA_0.8.2/Indranil/CAL-master_copy_new/python_client/_benchmarks_results/pData.csv',
'a+')
hazard = False
sig = False
sigState = ''
speed_post = False
speed_post_state = ''
veh_inside_box = False
veh_dist = 50.0
'''
prediction keys:
center_distance
hazard_stop
red_light
relative_angle
speed_sign
veh_distance
'''
front_axel_x = 2.33
for a in measurements.non_player_agents:
if a.HasField('traffic_light'):
llx, lly = a.traffic_light.transform.location.x, a.traffic_light.transform.location.y
nx, ny = self.getNewCord((px, py), (llx, lly), yaw)
if self.inside_a2(nx, ny):
print("\n--traffic light found---!!")
print("State :", a.traffic_light.state)
sig = True
sigState = str(a.traffic_light.state)
if a.HasField('speed_limit_sign'):
llx, lly = a.speed_limit_sign.transform.location.x, a.speed_limit_sign.transform.location.y
nx, ny = self.getNewCord((px, py), (llx, lly), yaw)
if self.inside_a2(nx, ny):
print("\n--Speed Sign found---!!")
print("Limit :", a.speed_limit_sign.speed_limit * 3.6)
speed_post = True
speed_post_state = str(
math.floor(a.speed_limit_sign.speed_limit * 3.6))
if a.HasField('pedestrian'):
llx, lly = a.pedestrian.transform.location.x, a.pedestrian.transform.location.y
ped = {}
ped['pos_x'] = llx
ped['pos_y'] = lly
peds.append(ped)
nx, ny = self.getNewCord((px, py), (llx, lly), yaw)
if self.inside_a1(nx, ny):
print("\n--Pedestrian Hazard---!!")
print("Pedestrian in front ")
hazard = True
if a.HasField('vehicle'):
llx, lly = a.vehicle.transform.location.x, a.vehicle.transform.location.y
nx, ny = self.getNewCord((px, py), (llx, lly), yaw)
hlen = a.vehicle.bounding_box.extent.x
if self.inside_a3(nx, ny):
distan = self.getdis(0 + front_axel_x, 0, nx,
ny - hlen)
veh_inside_box = True
veh_dist = min(distan, veh_dist)
print("Vehicle in front distance,", distan,
" Predicted ", prediction['veh_distance'])
player['peds'] = peds
#write to file if hazard stop is true in current frame...
if hazard is True:
sfile.write(
str('{:0>6d}'.format(frame)) + ",Hazard," + str(hazard) +
"," + str(prediction['hazard_stop'][0]) + "," +
str(prediction['hazard_stop'][1]) + "\n")
else:
sfile.write(
str('{:0>6d}'.format(frame)) + ",Hazard," + str(False) +
"," + str(prediction['hazard_stop'][0]) + "," +
str(prediction['hazard_stop'][1]) + "\n")
if sig is True:
sfile.write(
str('{:0>6d}'.format(frame)) + "," + "Traffic," +
sigState + "," + str(prediction['red_light'][0]) + "," +
str(prediction['red_light'][1]) + "\n")
print("Traffic light.. Actual:", a.traffic_light.state,
" Predicted", prediction['red_light'][0])
else:
sfile.write(
str('{:0>6d}'.format(frame)) + "," + "Traffic," + '0' +
"," + str(prediction['red_light'][0]) + "," +
str(prediction['red_light'][1]) + "\n")
if speed_post is True:
sfile.write(
str('{:0>6d}'.format(frame)) + ",SpeedSign," +
speed_post_state + "," + str(prediction['speed_sign'][0]) +
"," + str(prediction['speed_sign'][1]) + "\n")
else:
sfile.write(
str('{:0>6d}'.format(frame)) + ",SpeedSign," + '-1' + "," +
str(prediction['speed_sign'][0]) + "," +
str(prediction['speed_sign'][1]) + "\n")
if veh_inside_box is True:
sfile.write(
str('{:0>6d}'.format(frame)) + ",Vehicle," +
str(veh_dist) + "," + str(prediction['veh_distance']) +
"\n")
else:
sfile.write(
str('{:0>6d}'.format(frame)) + ",Vehicle," + str(50.0) +
"," + str(prediction['veh_distance']) + "\n")
sfile.write(
str('{:0>6d}'.format(frame)) + ",CenterDist," +
str(temp['centerDist']) + "," +
str(prediction['center_distance']) + "\n")
sfile.write(
str('{:0>6d}'.format(frame)) + ",Angel," + str(yaw) + "," +
str(prediction['relative_angle']) + "\n")
jout = json.dumps(player)
pfile.write(jout + "\n")
if success:
return 1, measurement_vec, control_vec, float(
current_timestamp - initial_timestamp
) / 1000.0, distance, col_ped, col_veh, col_oth, metList
return 0, measurement_vec, control_vec, time_out, distance, col_ped, col_veh, col_oth, metList
def getNewCord(self, origin, point, psi):
a = np.array([[math.cos(psi), -math.sin(psi)],
[math.sin(psi), math.cos(psi)]])
b = np.array([(point[0] - origin[0]), (point[1] - origin[1])])
x = np.linalg.solve(a, b)
return x
def getdis(self, x1, y1, x2, y2):
return math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
def inside_a2(self, x, y):
front_axel_x = 0
if (x >= (7.4 + front_axel_x) and x <=
(14.0 + front_axel_x)) and (y >= 0.8 and y <= 5.8):
return True
return False
def inside_a1(self, x, y):
front_axel_x = 2.33
if (x >= (0.0 + front_axel_x) and x <=
(8.2 + front_axel_x)) and (y >= -2.0 and y <= 2.0):
return True
return False
def inside_a3(self, x, y):
front_axel_x = 2.33
if (x >= (0 + front_axel_x) and x <=
(50 + front_axel_x)) and (y >= -1.6 and y <= 1.6):
return True
return False
class StraightDriveEnv(CarlaEnv):
def __init__(self,
client,
frame_skip=1,
cam_width=800,
cam_height=600,
town_string='Town01'):
super(StraightDriveEnv, self).__init__()
self.frame_skip = frame_skip
self.client = client
self._planner = Planner(town_string)
camera0 = Camera('CameraRGB')
camera0.set(CameraFOV=100)
camera0.set_image_size(cam_height, cam_width)
camera0.set_position(200, 0, 140)
camera0.set_rotation(-15.0, 0, 0)
self.start_goal_pairs = [[36, 40], [39, 35], [110, 114], [7,
3], [0, 4],
[68, 50], [61, 59], [47, 64], [147, 90],
[33, 87], [26, 19], [80, 76], [45, 49],
[55, 44], [29, 107], [95, 104], [84, 34],
[53, 67], [22, 17], [91, 148], [20, 107],
[78, 70], [95, 102], [68, 44], [45, 69]]
vehicles = 0
pedestrians = 0
weather = 1
settings = CarlaSettings()
settings.set(
SynchronousMode=True,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=vehicles,
NumberOfPedestrians=pedestrians,
WeatherId=weather,
)
settings.randomize_seeds()
settings.add_sensor(camera0)
self.scene = self.client.load_settings(settings)
img_shape = (cam_width, cam_height, 3)
self.observation_space = spaces.Tuple(
(spaces.Box(-np.inf, np.inf, (3, )), spaces.Box(0, 255,
img_shape)))
self.action_space = spaces.Box(-np.inf, np.inf, shape=(3, ))
self.prev_state = np.array([0., 0., 0.])
self.prev_collisions = np.array([0., 0., 0.])
self.prev_intersections = np.array([0., 0.])
def step(self, action):
steer = np.clip(action[0], -1, 1)
throttle = np.clip(action[1], 0, 1)
brake = np.clip(action[2], 0, 1)
for i in range(self.frame_skip):
self.t += 1
self.client.send_control(steer=steer,
throttle=throttle,
brake=brake,
hand_brake=False,
reverse=False)
measurements, sensor_data = self.client.read_data()
sensor_data = self._process_image(sensor_data['CameraRGB'])
current_time = measurements.game_timestamp
state, collisions, intersections, onehot = self._process_measurements(
measurements)
reward, dist_goal = self._calculate_reward(state, collisions,
intersections)
done = self._calculate_done(collisions, state, current_time)
self.prev_state = np.array(state)
self.prev_collisions = np.array(collisions)
self.prev_intersections = np.array(intersections)
measurement_obs = self._generate_obs(state, collisions, onehot,
dist_goal)
return (measurement_obs, sensor_data), reward, done, {}
def reset(self):
self._generate_start_goal_pair()
print('Starting new episode...')
# Blocking function until episode is ready
self.client.start_episode(self.start_idx)
measurements, sensor_data = self.client.read_data()
sensor_data = self._process_image(sensor_data['CameraRGB'])
state, collisions, intersections, onehot = self._process_measurements(
measurements)
self.prev_state = np.array(state)
self.prev_collisions = np.array(collisions)
self.prev_intersections = np.array(intersections)
self.start_time = measurements.game_timestamp
self.t = 0
pos = np.array(state[0:2])
dist_goal = np.linalg.norm(pos - self.goal)
measurement_obs = self._generate_obs(state, collisions, onehot,
dist_goal)
return (measurement_obs, sensor_data)
def _calculate_done(self, collisions, state, current_time):
pos = np.array(state[0:2])
dist_goal = np.linalg.norm(pos - self.goal)
return self._is_timed_out(current_time) or self._is_goal(dist_goal)
# Not described in paper, but should be there for safe driving
return self._is_timed_out() and self._collision_on_step(dist_goal)
def _calculate_planner_onehot(self, measurements):
# return np.array([0, 0, 0, 0, 1]) # TODO need to debug
print(type(self.end_point.location), type(self.end_point.orientation))
val = self._planner.get_next_command(measurements.location,
measurements.orientation,
self.end_point.location,
self.end_point.orientation)
if val == 0.0:
return np.array([1, 0, 0, 0, 0])
onehot = np.zeros(5)
val = int(val) - 1
onehot[val] = 1
return onehot
def _calculate_reward(self, state, collisions, intersections):
pos = np.array(state[0:2])
dist_goal = np.linalg.norm(pos - self.goal)
dist_goal_prev = np.linalg.norm(self.prev_state[0:2] - self.goal)
speed = state[2]
# TODO: Check this?
r = 1000 * (dist_goal_prev - dist_goal) / 10 + 0.05 * (speed - self.prev_state[2]) \
- 2 * (sum(intersections) - sum(self.prev_intersections))
return r, dist_goal
def _calculate_timeout(self):
self.timeout_t = (
(self.timeout_dist / 100000.0) / 10.0) * 3600.0 + 10.0
def _collision_on_step(self, collisions):
return sum(collisions) > 0
def _generate_obs(self, state, collisions, onehot, dist_goal):
speed = state[2]
collisions = np.sum(collisions)
return np.concatenate((np.array([speed, dist_goal,
collisions]), np.array(onehot)))
def _generate_start_goal_pair(self):
# Choose one player start at random.
self.position_index = np.random.randint(0,
len(self.start_goal_pairs) - 1)
self.start_idx = self.start_goal_pairs[self.position_index][0]
self.goal_idx = self.start_goal_pairs[self.position_index][1]
start_point = self.scene.player_start_spots[self.start_idx]
end_point = self.scene.player_start_spots[self.goal_idx]
self.end_point = end_point
self.goal = [end_point.location.x / 100,
end_point.location.y / 100] # cm -> m
self.timeout_dist = self._planner.get_shortest_path_distance(
[start_point.location.x, start_point.location.y, 22],
[start_point.orientation.x, start_point.orientation.y, 22],
[end_point.location.x, end_point.location.y, 22],
[end_point.orientation.x, end_point.orientation.y, 22])
self._calculate_timeout()
def _is_goal(self, distance):
return distance < 2.0
def _is_timed_out(self, current_time):
return (current_time - self.start_time) > (self.timeout_t * 1000)
def _process_image(self, carla_raw_img):
return resize(to_rgb_array(carla_raw_img), (84, 84))
def _process_measurements(self, measurements):
player_measurements = measurements.player_measurements
pos_x = player_measurements.transform.location.x / 100 # cm -> m
pos_y = player_measurements.transform.location.y / 100
speed = player_measurements.forward_speed
col_cars = player_measurements.collision_vehicles
col_ped = player_measurements.collision_pedestrians
col_other = player_measurements.collision_other
other_lane = player_measurements.intersection_otherlane
offroad = player_measurements.intersection_offroad
onehot = self._calculate_planner_onehot(player_measurements.transform)
return np.array([pos_x, pos_y,
speed]), np.array([col_cars, col_ped, col_other
]), np.array([other_lane,
offroad]), onehot
class DrivingBenchmark(object):
"""
The Benchmark class, controls the execution of the benchmark interfacing
an Agent class with a set Suite.
The benchmark class must be inherited with a class that defines the
all the experiments to be run by the agent
"""
def __init__(self,
city_name='Town01',
name_to_save='Test',
continue_experiment=False,
save_images=False,
distance_for_success=2.0):
"""
Args
city_name:
name_to_save:
continue_experiment:
save_images:
distance_for_success:
collisions_as_failure: if this flag is set to true, episodes will terminate as failure, when the car collides.
"""
self.__metaclass__ = abc.ABCMeta
self._city_name = city_name
self._base_name = name_to_save
# The minimum distance for arriving into the goal point in
# order to consider ir a success
self._distance_for_success = distance_for_success
# The object used to record the benchmark and to able to continue after
self._recording = Recording(name_to_save=name_to_save,
continue_experiment=continue_experiment,
save_images=save_images)
# We have a default planner instantiated that produces high level commands
self._planner = Planner(city_name)
# TO keep track of the previous collisions
self._previous_pedestrian_collision = 0
self._previous_vehicle_collision = 0
self._previous_other_collision = 0
def benchmark_agent(self, experiment_suite, agent, client):
"""
Function to benchmark the agent.
It first check the log file for this benchmark.
if it exist it continues from the experiment where it stopped.
Args:
experiment_suite
agent: an agent object with the run step class implemented.
client:
Return:
A dictionary with all the metrics computed from the
agent running the set of experiments.
"""
# Instantiate a metric object that will be used to compute the metrics for
# the benchmark afterwards.
metrics_object = Metrics(experiment_suite.metrics_parameters,
experiment_suite.dynamic_tasks)
# Function return the current pose and task for this benchmark.
start_pose, start_experiment = self._recording.get_pose_and_experiment(
experiment_suite.get_number_of_poses_task())
logging.info('START')
for experiment in experiment_suite.get_experiments(
)[int(start_experiment):]:
positions = client.load_settings(
experiment.conditions).player_start_spots
self._recording.log_start(experiment.task)
for pose in experiment.poses[start_pose:]:
for rep in range(experiment.repetitions):
start_index = pose[0]
end_index = pose[1]
client.start_episode(start_index)
# Print information on
logging.info('======== !!!! ==========')
logging.info(' Start Position %d End Position %d ',
start_index, end_index)
self._recording.log_poses(start_index, end_index,
experiment.Conditions.WeatherId)
# Calculate the initial distance for this episode
initial_distance = \
sldist(
[positions[start_index].location.x, positions[start_index].location.y],
[positions[end_index].location.x, positions[end_index].location.y])
time_out = experiment_suite.calculate_time_out(
self._get_shortest_path(positions[start_index],
positions[end_index]))
# running the agent
(result, reward_vec, control_vec, final_time, remaining_distance, col_ped, col_veh, col_oth) = \
self._run_navigation_episode(
agent, client, time_out, positions[end_index],
str(experiment.Conditions.WeatherId) + '_'
+ str(experiment.task) + '_' + str(start_index)
+ '.' + str(end_index), experiment_suite.metrics_parameters,
experiment_suite.collision_as_failure)
# Write the general status of the just ran episode
self._recording.write_summary_results(
experiment, pose, rep, initial_distance,
remaining_distance, final_time, time_out, result,
col_ped, col_veh, col_oth)
# Write the details of this episode.
self._recording.write_measurements_results(
experiment, rep, pose, reward_vec, control_vec)
if result > 0:
logging.info(
'+++++ Target achieved in %f seconds! +++++',
final_time)
else:
logging.info('----- Timeout! -----')
start_pose = 0
self._recording.log_end()
return metrics_object.compute(self._recording.path)
def get_path(self):
"""
Returns the path were the log was saved.
"""
return self._recording.path
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 _get_shortest_path(self, start_point, end_point):
"""
Calculates the shortest path between two points considering the road netowrk
"""
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
])
def _has_agent_collided(self, measurement, metrics_parameters):
"""
This function must have a certain state and only look to one measurement.
"""
collided_veh = 0
collided_ped = 0
collided_oth = 0
if (measurement.collision_vehicles - self._previous_vehicle_collision) \
> metrics_parameters['collision_vehicles']['threshold']/2.0:
collided_veh = 1
if (measurement.collision_pedestrians - self._previous_pedestrian_collision) \
> metrics_parameters['collision_pedestrians']['threshold']/2.0:
collided_ped = 1
if (measurement.collision_other - self._previous_other_collision) \
> metrics_parameters['collision_other']['threshold']/2.0:
collided_oth = 1
self._previous_pedestrian_collision = measurement.collision_pedestrians
self._previous_vehicle_collision = measurement.collision_other
return collided_ped, collided_veh, collided_oth
def _is_agent_stuck(self, measurements, stuck_vec, old_coll):
# break the episode when the agent is stuck on a static object
coll_other = measurements.collision_other
coll_other -= old_coll
otherlane = measurements.intersection_otherlane > 0.4
offroad = measurements.intersection_offroad > 0.3
logging.info(
"offroad: {}, otherlane: {}, coll_other: {}, old_coll: {}".format(
offroad, otherlane, coll_other, old_coll))
# if still driving or got unstuck (v > 4km/h)
if measurements.forward_speed * 3.6 > 4:
cycle_signal(stuck_vec, 0)
if coll_other:
old_coll += coll_other
elif offroad or otherlane or coll_other:
cycle_signal(stuck_vec, 1)
else:
cycle_signal(stuck_vec, 0)
return all(stuck_vec), stuck_vec, old_coll
def _run_navigation_episode(self, agent, client, time_out, target,
episode_name, metrics_parameters,
collision_as_failure):
"""
Run one episode of the benchmark (Pose) for a certain agent.
Args:
agent: the agent object
client: an object of the carla client to communicate
with the CARLA simulator
time_out: the time limit to complete this episode
target: the target to reach
episode_name: The name for saving images of this episode
metrics_object: The metrics object to check for collisions
"""
# Send an initial command.
time.sleep(2)
measurements, sensor_data = client.read_data()
client.send_control(VehicleControl())
### Reset CAL agent
agent.reset_state()
initial_timestamp = measurements.game_timestamp
current_timestamp = initial_timestamp
# The vector containing all measurements produced on this episode
measurement_vec = []
# The vector containing all controls produced on this episode
control_vec = []
frame = 0
distance = 10000
col_ped, col_veh, col_oth = 0, 0, 0
fail = False
success = False
### own metrics
stuck_vec = [0] * 60 # measure for 60 frames (6 seconds)
center_distance_vec = []
old_collision_value = 0
direction_vec = []
while not fail and not success:
# Read data from server with the client
measurements, sensor_data = client.read_data()
# The directions to reach the goal are calculated.
directions = self._get_directions(
measurements.player_measurements.transform, target)
# Agent process the data.
control = agent.run_step(measurements, sensor_data, directions,
target)
# Send the control commands to the vehicle
client.send_control(control)
# save images if the flag is activated
self._recording.save_images(sensor_data, episode_name, frame)
current_x = measurements.player_measurements.transform.location.x
current_y = measurements.player_measurements.transform.location.y
logging.info("Controller is Inputting:")
logging.info('Steer = %f Throttle = %f Brake = %f ', control.steer,
control.throttle, control.brake)
current_timestamp = measurements.game_timestamp
# Get the distance travelled until now
distance = sldist([current_x, current_y],
[target.location.x, target.location.y])
# Write status of the run on verbose mode
logging.info('Distance to target: %f', float(distance))
# Check if reach the target
col_ped, col_veh, col_oth = self._has_agent_collided(
measurements.player_measurements, metrics_parameters)
### CHANGE TO ORIGINAL CODE #####################
is_stuck, stuck_vec, old_collision_value = self._is_agent_stuck(
measurements.player_measurements, stuck_vec,
old_collision_value)
if distance < self._distance_for_success:
success = True
elif (current_timestamp - initial_timestamp) > (time_out * 1000):
fail = True
elif is_stuck:
fail = True
elif collision_as_failure and (col_ped or col_veh or col_oth):
fail = True
time_remain = (time_out * 1000 -
(current_timestamp - initial_timestamp)) / 1000
logging.info('Time remaining: %i m %i s', time_remain / 60,
time_remain % 60)
logging.info('')
# Increment the vectors and append the measurements and controls.
frame += 1
measurement_vec.append(measurements.player_measurements)
control_vec.append(control)
if success:
return 1, measurement_vec, control_vec, float(
current_timestamp - initial_timestamp
) / 1000.0, distance, col_ped, col_veh, col_oth
return 0, measurement_vec, control_vec, time_out, distance, col_ped, col_veh, col_oth
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)