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)
if ADD_NULL_LANDMARK:
null_pos = draw_null_landmark_pos(landmark_positions)
landmark_names.append(get_null_landmark_name())
landmark_positions.append(null_pos)
name2idx = get_landmark_name_to_index(add_empty=ADD_NULL_LANDMARK)
landmark_indices = [name2idx[name] for name in landmark_names]
return landmark_names, landmark_indices, landmark_positions
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 __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 load_and_convert_env_config(env_id):
config = load_env_config(env_id)
units = UnrealUnits()
config["x_pos_as"] = []
config["y_pos_as"] = []
for i,landmark in enumerate(config["landmarkName"]):
x = config["xPos"][i]
y = config["zPos"][i]
conv = units.pos2d_to_as(np.asarray([x,y]))
config["x_pos_as"].append(conv[0])
config["y_pos_as"].append(conv[1])
return config
def get_landmark_locations_airsim(config_json=None, add_empty=False, env_id=None):
assert config_json is not None or env_id is not None, "At least one of config_json or env_id must be provided"
if config_json is None:
config_json = load_env_config(env_id)
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)
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 convert_path(raw_path):
units = UnrealUnits()
conv_path = np.asarray([units.pos2d_to_as(p) for p in raw_path])
return conv_path
def convert_pos_from_config(pos):
units = UnrealUnits()
pos_conv = units.pos2d_to_as(pos)
return pos_conv