Merge remote-tracking branch 'origin/dev' into fork/q434343/device_visualization

unilabos/app/mq.py

@@ -1,5 +1,6 @@
 import json
 import time
+import traceback
 import uuid
 
 import paho.mqtt.client as mqtt
@@ -68,8 +69,10 @@ class MQTTClient:
         except json.JSONDecodeError as e:
             logger.error(f"[MQTT] JSON 解析错误: {e}")
             logger.error(f"[MQTT] Raw message: {msg.payload}")
+            logger.error(traceback.format_exc())
         except Exception as e:
             logger.error(f"[MQTT] 处理消息时出错: {e}")
+            logger.error(traceback.format_exc())
 
     def _on_disconnect(self, client, userdata, rc, reasonCode=None, properties=None):
         if rc != 0:

unilabos/ros/msgs/message_converter.py

@@ -133,15 +133,15 @@ _msg_converter: Dict[Type, Any] = {
     String: lambda x: String(data=str(x)),
     Point: lambda x: Point(x=x.x, y=x.y, z=x.z),
     Resource: lambda x: Resource(
-        id=x["id"],
+        id=x.get("id", ""),
-        name=x["name"],
+        name=x.get("name", ""),
         sample_id=x.get("sample_id", "") or "",
         children=list(x.get("children", [])),
         parent=x.get("parent", "") or "",
-        type=x["type"],
+        type=x.get("type", ""),
-        category=x.get("class", "") or x["type"],
+        category=x.get("class", "") or x.get("type", ""),
         pose=(
-            Pose(position=Point(x=float(x["position"]["x"]), y=float(x["position"]["y"]), z=float(x["position"]["z"])))
+            Pose(position=Point(x=float(x.get("position", {}).get("x", 0)), y=float(x.get("position", {}).get("y", 0)), z=float(x.get("position", {}).get("z", 0))))
             if x.get("position", None) is not None
             else Pose()
         ),
@@ -331,16 +331,27 @@ def convert_to_ros_msg(ros_msg_type: Union[Type, Any], obj: Any) -> Any:
     ros_msg = ros_msg_type() if isinstance(ros_msg_type, type) else ros_msg_type
 
     # 提取数据
-    data = _extract_data(obj)
+    extract_data = dict(_extract_data(obj))
 
     # 转换数据到ROS消息
-    for key, value in data.items():
+    for ind, data in enumerate(ros_msg.get_fields_and_field_types().items()):
+        key, type_name = data
+        if key not in extract_data:
+            continue
+        value = extract_data[key]
         if hasattr(ros_msg, key):
             attr = getattr(ros_msg, key)
             if isinstance(attr, (float, int, str, bool)):
                 setattr(ros_msg, key, value)
             elif isinstance(attr, (list, tuple)) and isinstance(value, Iterable):
-                setattr(ros_msg, key, list(value))
+                td = ros_msg.SLOT_TYPES[ind].value_type
+                if isinstance(td, NamespacedType):
+                    target_class = msg_converter_manager.get_class(f"{'.'.join(td.namespaces)}.{td.name}")
+                    setattr(ros_msg, key, [convert_to_ros_msg(target_class, v) for v in value])
+                else:
+                    setattr(ros_msg, key, [])  # FIXME
+            elif "array.array" in str(type(attr)):
+                setattr(ros_msg, key, value)
             else:
                 nested_ros_msg = convert_to_ros_msg(type(attr)(), value)
                 setattr(ros_msg, key, nested_ros_msg)

unilabos/ros/nodes/resource_tracker.py

@@ -43,7 +43,7 @@ class DeviceNodeResourceTracker(object):
                 res_list.extend(
                     self.loop_find_resource(r, resource_cls_type, identifier_key, getattr(query_resource, identifier_key))
                 )
-        assert len(res_list) == 1, f"找到多个资源，请检查资源是否唯一: {res_list}"
+        assert len(res_list) == 1, f"{query_resource} 找到多个资源，请检查资源是否唯一: {res_list}"
         self.root_resource2resource[id(query_resource)] = res_list[0]
         # 后续加入其他对比方式
         return res_list[0]