Prcxi9320

unilabos/device_mesh/resource_visalization.py

@@ -128,14 +128,21 @@ def __init__(self, device: dict, resource: dict, enable_rviz: bool = True):
                         new_dev.set("device_name", node["id"]+"_")
                         # if node["parent"] is not None:
                         #     new_dev.set("station_name", node["parent"]+'_')
-
-                        new_dev.set("x",str(float(node["position"]["position"]["x"])/1000))
-                        new_dev.set("y",str(float(node["position"]["position"]["y"])/1000))
-                        new_dev.set("z",str(float(node["position"]["position"]["z"])/1000))
+                        if "position" in node:
+                            new_dev.set("x",str(float(node["position"]["position"]["x"])/1000))
+                            new_dev.set("y",str(float(node["position"]["position"]["y"])/1000))
+                            new_dev.set("z",str(float(node["position"]["position"]["z"])/1000))
                         if "rotation" in node["config"]:
                             new_dev.set("rx",str(float(node["config"]["rotation"]["x"])))
                             new_dev.set("ry",str(float(node["config"]["rotation"]["y"])))
                             new_dev.set("r",str(float(node["config"]["rotation"]["z"])))
+                        if "pose" in node:
+                            new_dev.set("x",str(float(node["pose"]["position"]["x"])/1000))
+                            new_dev.set("y",str(float(node["pose"]["position"]["y"])/1000))
+                            new_dev.set("z",str(float(node["pose"]["position"]["z"])/1000))
+                            new_dev.set("rx",str(float(node["pose"]["rotation"]["x"])))
+                            new_dev.set("ry",str(float(node["pose"]["rotation"]["y"])))
+                            new_dev.set("r",str(float(node["pose"]["rotation"]["z"])))
                         if "device_config" in node["config"]:
                             for key, value in node["config"]["device_config"].items():
                                 new_dev.set(key, str(value))