def __get_goal_location_airsim(goal):
units = UnrealUnits()
goal_x = goal[0]
goal_y = goal[1]
pt = np.asarray([goal_x, goal_y])
pt_as = np.zeros(2)
pt_as[0:2] = units.pos2d_to_as(pt)
return pt_as
def __get_goal_location_airsim(self, path_array):
units = UnrealUnits(1.0)
goal_x = path_array[-1][0]
goal_y = path_array[-1][1]
pt = np.asarray([goal_x, goal_y])
pt_as = np.zeros(3)
pt_as[0:2] = units.pos2d_to_as(pt)
return pt_as
def __init__(self, instance=0, flight_height=100.0):
self.instance = instance
self.port = PORTS[instance]
self.clock_speed = self._read_clock_speed()
self.control_interval = 0.5
self.flight_height = flight_height # Flight height is in config units, as with everything
self.units = UnrealUnits()
print("DroneController Initialize")
self.rate = Rate(0.1 / self.clock_speed)
self.samples = []
killAirSim()
self._write_airsim_settings()
startAirSim(self, instance, self.port)
def faux_dataset_random_pt(eval_envs):
print("Generating faux dataset")
units = UnrealUnits(scale=1.0)
dataset = []
for env_id in eval_envs:
segment_dataset = []
config = load_env_config(env_id)
template = load_template(env_id)
start_pt = np.asarray(config["startPos"])
second_pt = np.asarray(config["startHeading"])
end_x = random.uniform(0, 1000)
end_y = random.uniform(0, 1000)
end_pt = np.asarray([end_x, end_y])
state1 = DroneState(None, start_pt)
state2 = DroneState(None, second_pt)
state3 = DroneState(None, end_pt)
segment_dataset.append(bare_min_sample(state1, False, env_id))
segment_dataset.append(bare_min_sample(state2, False, env_id))
segment_dataset.append(bare_min_sample(state3, True, env_id))
dataset.append(segment_dataset)
return dataset
def plot_path(self, env_id, paths, interactive=False, show=True, bg=True):
if interactive:
plt.ion()
plt.clf()
else:
plt.ioff()
config_size = UnrealUnits().get_config_size()
if bg:
try:
img = load_env_img(env_id)
plt.imshow(img, extent=(0, config_size[0], 0, config_size[1]))
except Exception as e:
print("Error in loading and plotting path!")
print(e)
colors = ["r", "g", "b", "y", "c", "m"]
for path, color in zip(paths, colors):
# Note that x and y are swapped
x_targets, y_targets = list(zip(*path))
y_targets = config_size[1] - y_targets
plt.plot(y_targets, x_targets, color)
plt.plot(y_targets[-1], x_targets[-1], color + "o")
plt.axis([0, config_size[0], 0, config_size[1]])
if show:
plt.show()
plt.pause(0.0001)
def get_landmark_locations_airsim(config_json, add_empty=False):
landmark_names = []
landmark_positions = []
units = UnrealUnits()
for i, landmarkName in enumerate(config_json["landmarkName"]):
x_pos = config_json["xPos"][i]
y_pos = config_json["zPos"][i]
pt = np.asarray([x_pos, y_pos])
pt_as = np.zeros(3)
pt_as[0:2] = units.pos2d_to_as(pt)
pt_as[2] = -1.0 # Landmarks assumed to be floating 1m above ground.
landmark_names.append(landmarkName)
landmark_positions.append(pt_as)
landmark_indices = get_landmark_name_to_index(add_empty=add_empty)
landmark_indices = [landmark_indices[name] for name in landmark_names]
return landmark_names, landmark_indices, landmark_positions
def faux_dataset_random_landmark(eval_envs):
print("Generating faux dataset")
units = UnrealUnits(scale=1.0)
dataset = []
for env_id in eval_envs:
segment_dataset = []
config = load_env_config(env_id)
template = load_template(env_id)
path = load_path(env_id)
landmark_radii = config["radius"]
start_pt = np.asarray(config["startPos"])
second_pt = np.asarray(config["startHeading"])
landmark_choice_ids = list(range(len(config["landmarkName"])))
choice_id = random.choice(landmark_choice_ids)
target_x = config["xPos"][choice_id]
target_y = config["zPos"][choice_id]
target_lm_pos = np.asarray([target_x, target_y])
landmark_dir = target_lm_pos - start_pt
method = template["side"]
theta = math.atan2(landmark_dir[1], landmark_dir[0]) + math.pi
if method == "infront":
theta = theta
elif method == "random":
theta = random.random() * 2 * math.pi
elif method == "behind":
theta = theta + math.pi
elif method == "left":
theta = theta - math.pi / 2
elif method == "right":
theta = theta + math.pi / 2
x, z = target_lm_pos[0], target_lm_pos[1]
landmark_radius = landmark_radii[choice_id]
sample_point = np.array([
x + math.cos(theta) * landmark_radius,
z + math.sin(theta) * landmark_radius
])
state1 = DroneState(None, start_pt)
state2 = DroneState(None, second_pt)
state3 = DroneState(None, sample_point)
segment_dataset.append(bare_min_sample(state1, False, env_id))
segment_dataset.append(bare_min_sample(state2, False, env_id))
segment_dataset.append(bare_min_sample(state3, True, env_id))
dataset.append(segment_dataset)
return dataset
def get_landmark_locations_airsim(config_json):
landmark_names = []
landmark_positions = []
units = UnrealUnits()
for i, landmarkName in enumerate(config_json["landmarkName"]):
x_pos = config_json["xPos"][i]
y_pos = config_json["zPos"][i]
pt = np.asarray([x_pos, y_pos])
pt_as = np.zeros(3)
pt_as[0:2] = units.pos2d_to_as(pt)
# TODO: Grab this from the parameter server
pt_as[
2] = 0.0 if LANDMARKS_ON_FLOOR else -1.0 # Landmarks assumed to be floating 1m above ground.
landmark_names.append(landmarkName)
landmark_positions.append(pt_as)
landmark_indices = get_landmark_name_to_index()
landmark_indices = [landmark_indices[name] for name in landmark_names]
return landmark_names, landmark_indices, landmark_positions
def plot_paths(self,
segment_dataset,
segment_path=None,
file=None,
interactive=False,
bg=True,
texts=[],
entire_trajectory=False):
if interactive:
plt.ion()
else:
plt.ioff()
if len(segment_dataset) == 0:
print("Empty segment. Not plotting!")
return
path_key = "path" if entire_trajectory else "seg_path"
env_id = segment_dataset[0]["metadata"]["env_id"]
if segment_path is None:
segment_path = segment_dataset[0]["metadata"][path_key]
config_size = UnrealUnits().get_config_size()
y_targets, x_targets = list(zip(*cf_to_img(segment_path, [512, 512])))
y_targets = np.asarray(y_targets) * config_size[1] / 512
x_targets = np.asarray(x_targets) * config_size[0] / 512
y_targets = config_size[1] - y_targets
#x_targets = CONFIG_SIZE[1] - x_targets
# Note that x and y are swapped
#x_targets, y_targets = list(zip(*segment_path))
if entire_trajectory:
instructions = [
segment_dataset[i]["instruction"]
for i in range(len(segment_dataset))
]
unique_instructions = [instructions[0]]
for instruction in instructions:
if instruction != unique_instructions[-1]:
unique_instructions.append(instruction)
instruction = "; ".join(unique_instructions)
else:
instruction = segment_dataset[0]["instruction"]
if bg:
try:
img = load_env_img(env_id)
plt.imshow(img, extent=(0, config_size[0], 0, config_size[1]))
except Exception as e:
print("Error in plotting paths!")
print(e)
#y_targets = CONFIG_SIZE[1] - y_targets
plt.plot(x_targets, y_targets, "r")
plt.plot(x_targets[-1], y_targets[-1], "ro")
# Plot segment endpoints
#for segment in segment_dataset:
# end = segment.metadata["seg_path"][-1]
# end_x = end[0]
# end_y = CONFIG_SIZE[1] - end[1]
# plt.plot(end_y, end_x, "ro")
x_actual = []
y_actual = []
for sample in segment_dataset:
x_actual.append(sample["state"].state[0])
y_actual.append(sample["state"].state[1])
x_actual = np.asarray(x_actual)
y_actual = np.asarray(y_actual)
"""if len(segment_dataset) > 0:
instruction, drone_states, actions, rewards, finished = zip(*segment_dataset)
drone_states = np.asarray(drone_states)
x_actual = drone_states[:, 0]
y_actual = drone_states[:, 1]"""
plt.plot(x_actual, y_actual, "b")
plt.plot(x_actual[-1], y_actual[-1], "bo")
plt.axis([0, config_size[0], 0, config_size[1]])
instruction_split = self.split_lines(instruction, maxchars=40)
title = "\n".join(instruction_split)
plt.title("env: " + str(env_id) + " - " + title)
x = 10
y = 5
for text in texts:
if not DONT_DRAW_TEXT:
plt.text(x, y, text)
y += 40
y += len(instruction_split) * 40 + 40
for line in instruction_split:
if not DONT_DRAW_TEXT:
plt.text(x, y, line)
y -= 40
if interactive:
plt.show()
plt.pause(0.0001)
def get_top_down_ground_truth_dynamic_global(env_id, start_idx, end_idx,
drone_pos_as, img_w, img_h, map_w,
map_h):
"""
Returns the ground-truth label oriented in the global map frame
:param env_id:
:param start_idx:
:param img_w:
:param img_h:
:param map_w:
:param map_h:
:return:
"""
PROFILE = False
prof = SimpleProfiler(False, PROFILE)
path = load_path(env_id, anno=True)
#print(len(path), start_idx, end_idx)
path = path[start_idx:end_idx]
#instruction_segments = [self.all_instr[env_id][set_idx]["instructions"][seg_idx]]
prof.tick("load_path")
units = UnrealUnits(1.0)
drone_pos_cf = units.pos3d_from_as(drone_pos_as)
#print("Dynamic ground truth for ", env_id, start_idx, end_idx)
gt_dynamic = get_dynamic_ground_truth_v2(path, drone_pos_cf[:2])
#Presenter().plot_path(env_id, [path[start_idx:end_idx], gt_dynamic])
prof.tick("gen_gt_path")
seg_labels = np.zeros([img_w, img_h, 2]).astype(float)
path_in_img = cf_to_img(gt_dynamic, np.array([map_w, map_h]))
gauss_sigma = map_w / 96
seg_labels[:, :, 0] = tdd.plot_path_on_img(seg_labels[:, :, 0],
path_in_img)
if len(path_in_img) > 1:
seg_labels[:, :, 1] = tdd.plot_dot_on_img(seg_labels[:, :, 1],
path_in_img[-1], gauss_sigma)
prof.tick("plot_path")
seg_labels[:, :, 0] = gaussian_filter(seg_labels[:, :, 0], gauss_sigma)
seg_labels[:, :, 1] = gaussian_filter(seg_labels[:, :, 1], gauss_sigma)
# Standardize both channels separately (each has mean zero, unit variance)
seg_labels_path = standardize_2d_prob_dist(seg_labels[:, :, 0:1])
seg_labels_endpt = standardize_2d_prob_dist(seg_labels[:, :, 1:2])
prof.tick("process_img")
DEBUG = False
if DEBUG:
gt_path_in_img = cf_to_img(path, np.asarray([map_w, map_h]))
dbg_labels_gt = np.zeros([img_w, img_h, 1])
dbg_labels_gt[:, :, 0] = tdd.plot_path_on_img(dbg_labels_gt[:, :, 0],
gt_path_in_img)
Presenter().show_image(dbg_labels_gt,
"dbg",
torch=False,
waitkey=10,
scale=4)
Presenter().show_image(torch.from_numpy(seg_labels_path),
"l_path",
torch=True,
waitkey=10,
scale=4)
Presenter().show_image(torch.from_numpy(seg_labels_endpt),
"l_endp",
torch=True,
waitkey=100,
scale=4)
seg_labels = np.concatenate((seg_labels_path, seg_labels_endpt), axis=0)
seg_labels_t = torch.from_numpy(seg_labels).unsqueeze(0).float()
prof.tick("prep_out")
prof.print_stats()
return seg_labels_t
class DroneController():
def __init__(self, instance=0, flight_height=100.0):
self.instance = instance
self.port = PORTS[instance]
self.clock_speed = self._read_clock_speed()
self.control_interval = 0.5
self.flight_height = flight_height # Flight height is in config units, as with everything
self.units = UnrealUnits()
print("DroneController Initialize")
self.rate = Rate(0.1 / self.clock_speed)
self.samples = []
killAirSim()
self._write_airsim_settings()
startAirSim(self, instance, self.port)
def _get_config(self, name, i=None, instance_id=None):
config = load_env_config(id)
return config
def _set_config(self,
json_data,
subfolder,
name,
i=None,
instance_id=None):
folder = paths.get_current_config_folder(i, instance_id)
id = "" if i is None else "_" + str(i)
path = os.path.join(paths.get_sim_config_dir(), folder, subfolder,
name + id + ".json")
if not os.path.isdir(os.path.dirname(path)):
try:
os.makedirs(os.path.dirname(path))
except Exception:
pass
if os.path.isfile(path):
os.remove(path)
# print("Dumping config in: " + path)
with open(path, 'w') as fp:
json.dump(json_data, fp)
subprocess.check_call(["touch", path])
def _load_drone_config(self, i, instance_id=None):
conf_json = load_env_config(i)
self._set_config(conf_json,
"",
"random_config",
i=None,
instance_id=instance_id)
def _start_call(self):
try:
self.client.enableApiControl(True)
self.client.armDisarm(True)
except Exception as e:
import traceback
from utils.colors import print_error
print_error("Error encountered during policy rollout!")
print_error(e)
print_error(traceback.format_exc())
startAirSim(self, self.instance, self.port)
raise RolloutException(
"Exception encountered during start_call. Rollout should be re-tried"
)
def _start(self):
self.client = MultirotorClient('127.0.0.1', self.port)
self.client.confirmConnection()
self._start_call()
# got to initial height
pos_as = self.client.getPosition()
pos_as = [pos_as.x_val, pos_as.y_val, pos_as.z_val]
pos = self.units.pos3d_from_as(pos_as)
pos[2] = self.units.height_to_as(self.flight_height)
print("Telporting to initial position in cfg: ", pos)
self.teleport_3d(pos, [0, 0, 0])
def _read_clock_speed(self):
speed = 1.0
if "ClockSpeed" in P.get_current_parameters()["AirSim"]:
speed = P.get_current_parameters()["AirSim"]["ClockSpeed"]
print("Read clock speed: " + str(speed))
return speed
def _write_airsim_settings(self):
airsim_settings = P.get_current_parameters()["AirSim"]
airsim_settings_path = P.get_current_parameters(
)["Environment"]["airsim_settings_path"]
airsim_settings_path = os.path.expanduser(airsim_settings_path)
save_json(airsim_settings, airsim_settings_path)
print("Wrote new AirSim settings to " + str(airsim_settings_path))
def _action_to_global(self, action):
drone_yaw = self.get_yaw()
action_global = np.zeros(3)
# TODO: Add action[1]
action_global[0] = action[0] * math.cos(drone_yaw)
action_global[1] = action[0] * math.sin(drone_yaw)
action_global[2] = action[2]
return action_global
def _try_teleport_to(self, position, yaw, pitch=None, roll=None):
pos_as = self.units.pos2d_to_as(position[:2])
yaw_as = self.units.yaw_to_as(yaw)
tgt_h = self.units.height_to_as(self.flight_height)
tele_orientation = self.client.toQuaternion(0, 0, yaw_as)
self.send_local_velocity_command([0, 0, 0])
self.rate.sleep_n_intervals(3)
pos1 = self.client.getPosition()
current_z = pos1.z_val
pos1.z_val = tgt_h - 50
self.client.simSetPose(pos1, tele_orientation)
self.rate.sleep_n_intervals(2)
tele_pos = Vector3r()
tele_pos.x_val = pos_as[0]
tele_pos.y_val = pos_as[1]
tele_pos.z_val = tgt_h - 50
self.client.simSetPose(tele_pos, tele_orientation)
self.rate.sleep_n_intervals(2)
tele_pos.z_val = tgt_h
self.client.simSetPose(tele_pos, tele_orientation)
self.send_local_velocity_command([0, 0, 0])
self.rate.sleep_n_intervals(3)
time.sleep(0.1)
def get_yaw(self):
o = self.client.getOrientation()
vec, theta = quaternions.quat2axangle(
[o.w_val, o.x_val, o.y_val, o.z_val])
new_vec = self.units.vel3d_from_as(vec)
roll, pitch, yaw = euler.axangle2euler(new_vec, theta)
return yaw
def send_local_velocity_command(self, cmd_vel):
# Convert the drone reference frame command to a global command as understood by AirSim
cmd_vel = self._action_to_global(cmd_vel)
#print(" global action: ", cmd_vel)
if len(cmd_vel) == 3:
cmd_vel_as = self.units.vel2d_to_as(cmd_vel[:2])
cmd_yaw_as = self.units.yaw_rate_to_as(cmd_vel[2])
#print(" AirSim command: ", cmd_vel_as, "yaw rate: ", cmd_yaw_as)
vel_x = cmd_vel_as[0]
vel_y = cmd_vel_as[1]
yaw_rate = cmd_yaw_as
z = self.units.height_to_as(self.flight_height)
yaw_mode = YawMode(is_rate=True, yaw_or_rate=yaw_rate)
self.client.moveByVelocityZ(vel_x,
vel_y,
z,
self.control_interval,
yaw_mode=yaw_mode)
def teleport_to(self, position, yaw):
pos_as = self.units.pos2d_to_as(position[0:2])
tgt_h = self.units.height_to_as(self.flight_height)
teleported = False
for i in range(10):
self._try_teleport_to(position, yaw)
curr_pos = self.client.getPosition()
curr_pos_np = np.asarray([curr_pos.x_val, curr_pos.y_val])
if np.linalg.norm(pos_as - curr_pos_np) < 1.0 and math.fabs(
tgt_h - curr_pos.z_val) < 1.0:
teleported = True
break
if not teleported:
print("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
print("Failed to teleport after 10 attempts!")
print("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
def teleport_3d(self, position, rpy, pos_in_airsim=True, fast=False):
pos_as = position if pos_in_airsim else self.units.pos3d_to_as(
position)
# TODO: This is broken. p and r should also be converted ideally
if not pos_in_airsim:
rpy[2] = self.units.yaw_to_as(rpy[2])
tele_rot = self.client.toQuaternion(rpy[0], rpy[1], rpy[2])
self.send_local_velocity_command([0, 0, 0])
self.rate.sleep_n_intervals(1)
tele_pos = Vector3r()
tele_pos.x_val = pos_as[0]
tele_pos.y_val = pos_as[1]
tele_pos.z_val = pos_as[2]
self.client.simSetPose(tele_pos, tele_rot)
self.send_local_velocity_command([0, 0, 0])
if not fast:
time.sleep(0.2)
self.client.simSetPose(tele_pos, tele_rot)
self.send_local_velocity_command([0, 0, 0])
if not fast:
time.sleep(0.2)
def get_state(self, depth=False, segmentation=False):
# Get images
request = [ImageRequest(0, AirSimImageType.Scene, False, False)]
if depth:
request.append(ImageRequest(0, AirSimImageType.DepthPlanner, True))
if segmentation:
request.append(
ImageRequest(0, AirSimImageType.Segmentation, False, False))
response = self.client.simGetImages(request)
pos_dict = response[0].camera_position
rot_dict = response[0].camera_orientation
cam_pos = [pos_dict[b"x_val"], pos_dict[b"y_val"], pos_dict[b"z_val"]]
cam_rot = [
rot_dict[b"w_val"], rot_dict[b"x_val"], rot_dict[b"y_val"],
rot_dict[b"z_val"]
]
img_data = np.frombuffer(response[0].image_data_uint8, np.uint8)
image_raw = img_data.reshape(response[0].height, response[0].width, 4)
image_raw = image_raw[:, :, 0:3]
concat = [image_raw]
if depth:
# Depth is cast to uint8, because otherwise it takes up a lot of space and we don't need such fine-grained
# resolution
depth_data = np.asarray(response[1].image_data_float)
depth_raw = depth_data.reshape(response[1].height,
response[1].width, 1)
depth_raw = np.clip(depth_raw, 0, 254 / DEPTH_SCALE)
depth_raw *= DEPTH_SCALE
depth_raw = depth_raw.astype(np.uint8)
concat.append(depth_raw)
if segmentation:
seg_data = np.frombuffer(response[2].image_data_uint8, np.uint8)
seg_raw = seg_data.reshape(response[2].height, response[2].width,
4)
concat.append(seg_raw)
img_state = np.concatenate(concat, axis=2)
# Get drone's physical location, orientation and velocity
pos = self.client.getPosition()
vel = self.client.getVelocity()
o = self.client.getOrientation()
# Turn it into a nice 9-D vector with euler angles
r_as, p_as, y_as = euler.quat2euler(
[o.w_val, o.x_val, o.y_val, o.z_val])
roll, pitch, yaw = self.units.euler_from_as(r_as, p_as, y_as)
pos_3d_as = np.asarray([pos.x_val, pos.y_val, pos.z_val])
pos_state = self.units.pos3d_from_as(pos_3d_as)
vel_state = self.units.vel3d_from_as(
np.asarray([vel.x_val, vel.y_val, vel.z_val]))
ori_state = [roll, pitch, yaw]
drone_state = np.concatenate(
(pos_state, ori_state, vel_state, cam_pos, cam_rot), axis=0)
return drone_state, img_state
def set_current_env_from_config(self, config, instance_id=None):
"""
Store the provided dict representation of a random_config.json into the
correct folder for instance_id simulator instance to read it when resetting it's pomdp.
:param config: Random pomdp configuration to store
:param instance_id: Simulator instance for which to store it.
"""
self._set_config(config, "", "random_config", instance_id=instance_id)
def set_current_env_id(self, env_id, instance_id=None):
# TODO: This was used before to render the path in the simulator, but now that feature is unused anyways
env_confs = ["config"] # , "random_curve"]
folders = ["configs"] # , "paths"]
#for env_conf, folder in zip(env_confs, folders):
self._load_drone_config(env_id, instance_id)
gc.collect()
def reset_environment(self):
self._start_call()
self.client.simResetEnv()
self._start_call()