def load_waypoints_from_file(self, filename):
error_message = None # assume file location and contents are correct
try:
with open(filename, 'r') as f:
data = yaml.load(f)
self.clear_all_markers()
self.server.clear()
self.server.applyChanges()
for uuid, wp in data["waypoints"].items():
wp_type = wp["waypoint_type"]
if wp_type == WAYPOINT_REGULAR:
point = Point(wp["x"], wp["y"], wp["z"])
elif wp_type == WAYPOINT_TERMINATING:
point = Pose()
point.position = Point(wp["x"], wp["y"], wp["z"])
point.orientation = Quaternion(wp["orientation"]["x"], wp["orientation"]["y"],
wp["orientation"]["z"], wp["orientation"]["w"])
self.insert_marker(position=point, uuid=uuid, name=wp['name'], waypoint_type=wp_type, floor_level=wp["floor_level"])
for edge in data["edges"]:
self._connect_markers(edge['u'], edge['v'], edge['cost'], edge['edge_type'], floor_level=edge["floor_level"])
self.display_waypoints_from_graph()
except Exception as e:
error_message = e
return error_message
