Dev v0.9.0 (#23)

Add high-level PLR functions
Add Laiyu/Zhida driver support
Fix ROS node discovery issues
Add hostname and resource query support
Fix ROS message conversion logic
Support configuration via environment variables

* Update README and MQTTClient for installation instructions and code improvements

* feat: 支持local_config启动
add: 增加对crt path的说明，为传入config.py的相对路径
move: web component

* add: registry description

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* feat: node_info_update srv
fix: OTDeck cant create

* close #12
feat: slave node registry

* feat: show machine name
fix: host node registry not uploaded

* feat: add hplc registry

* feat: add hplc registry

* fix: hplc status typo

* fix: devices/

* 完成启动OT并联动rviz

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* fix: device.class possible null

* fix: HPLC additions with online service

* fix: slave mode spin not working

* fix: slave mode spin not working

* 修复rviz位置问题,

修复rviz位置问题,
在无tf变动时减缓发送频率
在backend中添加物料跟随方法

* feat: 多ProtocolNode 允许子设备ID相同
feat: 上报发现的ActionClient
feat: Host重启动，通过discover机制要求slaveNode重新注册，实现信息及时上报

* feat: 支持env设置config

* fix: running logic

* fix: running logic

* fix: missing ot

* 在main中直接初始化republisher和物料的mesh节点

* 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中

* Device visualization (#14)

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* 修复rviz位置问题,

修复rviz位置问题,
在无tf变动时减缓发送频率
在backend中添加物料跟随方法

* fix: running logic

* fix: running logic

* fix: missing ot

* 在main中直接初始化republisher和物料的mesh节点

* 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中

---------

Co-authored-by: zhangshixiang <@zhangshixiang>
Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com>

* fix: missing hostname in devices_names
fix: upload_file for model file

* fix: missing paho-mqtt package
bump version to 0.9.0

* fix startup
add ResourceCreateFromOuter.action

* fix type hint

* update actions

* update actions

* host node add_resource_from_outer
fix cmake list

* pass device config to device class

* add: bind_parent_ids to resource create action
fix: message convert string

* fix: host node should not be re_discovered

* feat: resource tracker support dict

* feat: add more necessary params

* feat: fix boolean null in registry action data

* feat: add outer resource

* 编写mesh添加action

* feat: append resource

* add action

* feat: vis 2d for plr

* fix

* fix: browser on rviz

* fix: cloud bridge error fallback to local

* fix: salve auto run rviz

* 初始化两个plate

* Device visualization (#22)

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* 修复rviz位置问题,

修复rviz位置问题,
在无tf变动时减缓发送频率
在backend中添加物料跟随方法

* fix: running logic

* fix: running logic

* fix: missing ot

* 在main中直接初始化republisher和物料的mesh节点

* 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中

* 编写mesh添加action

* add action

* fix

* fix: browser on rviz

* fix: cloud bridge error fallback to local

* fix: salve auto run rviz

* 初始化两个plate

---------

Co-authored-by: zhangshixiang <@zhangshixiang>
Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com>

* fix: multi channel

* fix: aspirate

* fix: aspirate

* fix: aspirate

* fix: aspirate

* 提交

* fix: jobadd

* fix: jobadd

* fix: msg converter

* tijiao

---------

Co-authored-by: Harvey Que <Q-Query@outlook.com>
Co-authored-by: zhangshixiang <@zhangshixiang>
Co-authored-by: q434343 <73513873+q434343@users.noreply.github.com>

unilabos/ros/device_node_wrapper.py

@@ -18,6 +18,7 @@ class ROS2DeviceNodeWrapper(ROS2DeviceNode):
 
 def ros2_device_node(
     cls: Type[T],
+    device_config: Optional[Dict[str, Any]] = None,
     status_types: Optional[Dict[str, Any]] = None,
     action_value_mappings: Optional[Dict[str, Any]] = None,
     hardware_interface: Optional[Dict[str, Any]] = None,
@@ -30,6 +31,7 @@ def ros2_device_node(
         cls: 要封装的设备类
         status_types: 需要发布的状态和传感器信息，每个(PROP: TYPE)，PROP应该匹配cls.PROP或cls.get_PROP()，
             TYPE应该是ROS2消息类型。默认为{}。
+        device_config: 初始化时的config。
         action_value_mappings: 设备动作。默认为{}。
             每个(ACTION: {'type': CMD_TYPE, 'goal': {FIELD: PROP}, 'feedback': {FIELD: PROP}, 'result': {FIELD: PROP}}),
         hardware_interface: 硬件接口配置。默认为{"name": "hardware_interface", "write": "send_command", "read": "read_data", "extra_info": []}。
@@ -42,6 +44,8 @@ def ros2_device_node(
     # 从属性中自动发现可发布状态
     if status_types is None:
         status_types = {}
+    if device_config is None:
+        device_config = {}
     if action_value_mappings is None:
         action_value_mappings = {}
     if hardware_interface is None:
@@ -73,6 +77,7 @@ def ros2_device_node(
             "__init__": lambda self, *args, **kwargs: init_wrapper(
                 self,
                 driver_class=cls,
+                device_config=device_config,
                 status_types=status_types,
                 action_value_mappings=action_value_mappings,
                 hardware_interface=hardware_interface,

unilabos/ros/initialize_device.py

@@ -1,9 +1,9 @@
-import rclpy
-from rclpy.node import Node
+import copy
 from typing import Optional
+
 from unilabos.registry.registry import lab_registry
-from unilabos.ros.nodes.base_device_node import ROS2DeviceNode, DeviceInitError
 from unilabos.ros.device_node_wrapper import ros2_device_node
+from unilabos.ros.nodes.base_device_node import ROS2DeviceNode, DeviceInitError
 from unilabos.utils import logger
 from unilabos.utils.import_manager import default_manager
 
@@ -22,17 +22,21 @@ def initialize_device_from_dict(device_id, device_config) -> Optional[ROS2Device
         None
     """
     d = None
+    original_device_config = copy.deepcopy(device_config)
     device_class_config = device_config["class"]
     if isinstance(device_class_config, str):  # 如果是字符串，则直接去lab_registry中查找，获取class
         if device_class_config not in lab_registry.device_type_registry:
             raise ValueError(f"Device class {device_class_config} not found.")
         device_class_config = device_config["class"] = lab_registry.device_type_registry[device_class_config]["class"]
+    else:
+        raise ValueError("不再支持class为字典传入，class必须为注册表中已经提供的设备，您可以新增注册表并通过--registry传入")
     if isinstance(device_class_config, dict):
         DEVICE = default_manager.get_class(device_class_config["module"])
         # 不管是ros2的实例，还是python的，都必须包一次，除了HostNode
         DEVICE = ros2_device_node(
             DEVICE,
             status_types=device_class_config.get("status_types", {}),
+            device_config=original_device_config,
             action_value_mappings=device_class_config.get("action_value_mappings", {}),
             hardware_interface=device_class_config.get(
                 "hardware_interface",

unilabos/ros/main_slave_run.py

@@ -2,9 +2,13 @@ import copy
 import json
 import os
 import threading
+import time
 from typing import Optional, Dict, Any, List
 
 import rclpy
+from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
+from unilabos.ros.nodes.presets.resource_mesh_manager import ResourceMeshManager
+from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker
 from unilabos_msgs.msg import Resource  # type: ignore
 from unilabos_msgs.srv import ResourceAdd, SerialCommand  # type: ignore
 from rclpy.executors import MultiThreadedExecutor
@@ -40,17 +44,19 @@ def exit() -> None:
 
 def main(
     devices_config: Dict[str, Any] = {},
-    resources_config={},
+    resources_config: list=[],
     graph: Optional[Dict[str, Any]] = None,
     controllers_config: Dict[str, Any] = {},
     bridges: List[Any] = [],
-    args: List[str] = ["--log-level", "debug"],
+    visual: str = "disable",
+    resources_mesh_config: dict = {},
+    rclpy_init_args: List[str] = ["--log-level", "debug"],
     discovery_interval: float = 5.0,
 ) -> None:
     """主函数"""
-    rclpy.init(args=args)
-    rclpy.__executor = executor = MultiThreadedExecutor()
 
+    rclpy.init(args=rclpy_init_args)
+    executor = rclpy.__executor = MultiThreadedExecutor()
     # 创建主机节点
     host_node = HostNode(
         "host_node",
@@ -62,11 +68,26 @@ def main(
         discovery_interval,
     )
 
+    if visual != "disable":
+        resource_mesh_manager = ResourceMeshManager(
+            resources_mesh_config,
+            resources_config,
+            resource_tracker= DeviceNodeResourceTracker(),
+            device_id = 'resource_mesh_manager',
+        )
+        joint_republisher = JointRepublisher(
+            'joint_republisher',
+            DeviceNodeResourceTracker()
+        )
+
+        executor.add_node(resource_mesh_manager)
+        executor.add_node(joint_republisher)
+
     thread = threading.Thread(target=executor.spin, daemon=True, name="host_executor_thread")
     thread.start()
 
     while True:
-        input()
+        time.sleep(1)
 
 
 def slave(
@@ -75,11 +96,16 @@ def slave(
     graph: Optional[Dict[str, Any]] = None,
     controllers_config: Dict[str, Any] = {},
     bridges: List[Any] = [],
-    args: List[str] = ["--log-level", "debug"],
+    visual: str = "disable",
+    resources_mesh_config: dict = {},
+    rclpy_init_args: List[str] = ["--log-level", "debug"],
 ) -> None:
     """从节点函数"""
-    rclpy.init(args=args)
-    rclpy.__executor = executor = MultiThreadedExecutor()
+    if not rclpy.ok():
+        rclpy.init(args=rclpy_init_args)
+    executor = rclpy.__executor
+    if not executor:
+        executor = rclpy.__executor = MultiThreadedExecutor()
     devices_config_copy = copy.deepcopy(devices_config)
     for device_id, device_config in devices_config.items():
         d = initialize_device_from_dict(device_id, device_config)
@@ -94,6 +120,21 @@ def slave(
     n = Node(f"slaveMachine_{BasicConfig.machine_name}", parameter_overrides=[])
     executor.add_node(n)
 
+    if visual != "disable":
+        resource_mesh_manager = ResourceMeshManager(
+            resources_mesh_config,
+            resources_config,
+            resource_tracker= DeviceNodeResourceTracker(),
+            device_id = 'resource_mesh_manager',
+        )
+        joint_republisher = JointRepublisher(
+            'joint_republisher',
+            DeviceNodeResourceTracker()
+        )
+
+        executor.add_node(resource_mesh_manager)
+        executor.add_node(joint_republisher)
+        
     thread = threading.Thread(target=executor.spin, daemon=True, name="slave_executor_thread")
     thread.start()
 
@@ -112,7 +153,7 @@ def slave(
         logger.info(f"Slave node info updated.")
 
         rclient = n.create_client(ResourceAdd, "/resources/add")
-        rclient.wait_for_service()  # FIXME 可能一直等待，加一个参数
+        rclient.wait_for_service()
 
         request = ResourceAdd.Request()
         request.resources = [convert_to_ros_msg(Resource, resource) for resource in resources_config]
@@ -120,7 +161,7 @@ def slave(
         logger.info(f"Slave resource added.")
 
     while True:
-        input()
+        time.sleep(1)
 
 if __name__ == "__main__":
     main()

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"],
-        name=x["name"],
+        id=x.get("id", ""),
+        name=x.get("name", ""),
         sample_id=x.get("sample_id", "") or "",
         children=list(x.get("children", [])),
         parent=x.get("parent", "") or "",
-        type=x["type"],
-        category=x.get("class", "") or x["type"],
+        type=x.get("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)
@@ -566,6 +577,7 @@ basic_type_map = {
     'float32': {'type': 'number'},
     'float64': {'type': 'number'},
     'string': {'type': 'string'},
+    'boolean': {'type': 'boolean'},
     'char': {'type': 'string', 'maxLength': 1},
     'byte': {'type': 'integer', 'minimum': 0, 'maximum': 255},
 }

unilabos/ros/nodes/base_device_node.py

@@ -10,13 +10,16 @@ import asyncio
 
 import rclpy
 from rclpy.node import Node
-from rclpy.action import ActionServer
+from rclpy.action import ActionServer, ActionClient
 from rclpy.action.server import ServerGoalHandle
 from rclpy.client import Client
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.service import Service
+from unilabos_msgs.action import SendCmd
+from unilabos_msgs.srv._serial_command import SerialCommand_Request, SerialCommand_Response
 
-from unilabos.resources.graphio import convert_resources_to_type, convert_resources_from_type
+from unilabos.resources.graphio import convert_resources_to_type, convert_resources_from_type, resource_ulab_to_plr, \
+    initialize_resources
 from unilabos.ros.msgs.message_converter import (
     convert_to_ros_msg,
     convert_from_ros_msg,
@@ -101,6 +104,7 @@ def init_wrapper(
     self,
     device_id: str,
     driver_class: type[T],
+    device_config: Dict[str, Any],
     status_types: Dict[str, Any],
     action_value_mappings: Dict[str, Any],
     hardware_interface: Dict[str, Any],
@@ -118,6 +122,7 @@ def init_wrapper(
         children = []
     kwargs["device_id"] = device_id
     kwargs["driver_class"] = driver_class
+    kwargs["device_config"] = device_config
     kwargs["driver_params"] = driver_params
     kwargs["status_types"] = status_types
     kwargs["action_value_mappings"] = action_value_mappings
@@ -302,10 +307,77 @@ class BaseROS2DeviceNode(Node, Generic[T]):
             res.response = ""
             return res
 
+        def append_resource(req: SerialCommand_Request, res: SerialCommand_Response):
+            # 物料传输到对应的node节点
+            rclient = self.create_client(ResourceAdd, "/resources/add")
+            rclient.wait_for_service()
+            request = ResourceAdd.Request()
+            command_json = json.loads(req.command)
+            namespace = command_json["namespace"]
+            bind_parent_id = command_json["bind_parent_id"]
+            edge_device_id = command_json["edge_device_id"]
+            location = command_json["bind_location"]
+            other_calling_param = command_json["other_calling_param"]
+            resources = command_json["resource"]
+            initialize_full = other_calling_param.pop("initialize_full", False)
+            # 本地拿到这个物料，可能需要先做初始化?
+            if isinstance(resources, list):
+                if initialize_full:
+                    resources = initialize_resources(resources)
+                request.resources = [convert_to_ros_msg(Resource, resource) for resource in resources]
+            else:
+                if initialize_full:
+                    resources = initialize_resources([resources])
+                request.resources = [convert_to_ros_msg(Resource, resources)]
+            response = rclient.call(request)
+            # 应该先add_resource了
+            res.response = "OK"
+            # 接下来该根据bind_parent_id进行assign了，目前只有plr可以进行assign，不然没有办法输入到物料系统中
+            resource = self.resource_tracker.figure_resource({"name": bind_parent_id})
+            request.resources = [convert_to_ros_msg(Resource, resources)]
+
+            try:
+                from pylabrobot.resources.resource import Resource as ResourcePLR
+                from pylabrobot.resources.deck import Deck
+                from pylabrobot.resources import Coordinate
+                from pylabrobot.resources import OTDeck
+                contain_model = not isinstance(resource, Deck)
+                if isinstance(resource, ResourcePLR):
+                    # resources.list()
+                    plr_instance = resource_ulab_to_plr(resources, contain_model)
+                    if isinstance(resource, OTDeck) and "slot" in other_calling_param:
+                        resource.assign_child_at_slot(plr_instance, **other_calling_param)
+                    resource.assign_child_resource(plr_instance, Coordinate(location["x"], location["y"], location["z"]), **other_calling_param)
+                # 发送给ResourceMeshManager
+                action_client = ActionClient(
+                    self, SendCmd, "/devices/resource_mesh_manager/add_resource_mesh", callback_group=self.callback_group
+                )
+                goal = SendCmd.Goal()
+                goal.command = json.dumps({
+                    "resources": resources,
+                    "bind_parent_id": bind_parent_id,
+                })
+                future = action_client.send_goal_async(goal, goal_uuid=uuid.uuid4())
+
+                def done_cb(*args):
+                    self.lab_logger().info(f"向meshmanager发送新增resource完成")
+
+                future.add_done_callback(done_cb)
+            except ImportError:
+                self.lab_logger().error("Host请求添加物料时，本环境并不存在pylabrobot")
+            except Exception as e:
+                self.lab_logger().error("Host请求添加物料时出错")
+                self.lab_logger().error(traceback.format_exc())
+            return res
+
+        # noinspection PyTypeChecker
         self._service_server: Dict[str, Service] = {
             "query_host_name": self.create_service(
                 SerialCommand, f"/srv{self.namespace}/query_host_name", query_host_name_cb, callback_group=self.callback_group
             ),
+            "append_resource": self.create_service(
+                SerialCommand, f"/srv{self.namespace}/append_resource", append_resource, callback_group=self.callback_group
+            ),
         }
 
         # 向全局在线设备注册表添加设备信息
@@ -437,26 +509,36 @@ class BaseROS2DeviceNode(Node, Generic[T]):
 
             action_kwargs = convert_from_ros_msg_with_mapping(goal, action_value_mapping["goal"])
             self.lab_logger().debug(f"接收到原始目标: {action_kwargs}")
-
-            # 向Host查询物料当前状态
-            for k, v in goal.get_fields_and_field_types().items():
-                if v in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
-                    self.lab_logger().info(f"查询资源状态: Key: {k} Type: {v}")
-                    try:
-                        r = ResourceGet.Request()
-                        r.id = action_kwargs[k]["id"] if v == "unilabos_msgs/Resource" else action_kwargs[k][0]["id"]
-                        r.with_children = True
-                        response = await self._resource_clients["resource_get"].call_async(r)
-                    except Exception:
-                        logger.error(f"资源查询失败，默认使用本地资源")
-                    # 删除对response.resources的检查，因为它总是存在
-                    resources_list = [convert_from_ros_msg(rs) for rs in response.resources]  # type: ignore  # FIXME
-                    self.lab_logger().debug(f"资源查询结果: {len(resources_list)} 个资源")
-                    type_hint = action_paramtypes[k]
-                    final_type = get_type_class(type_hint)
-                    # 判断 ACTION 是否需要特殊的物料类型如 pylabrobot.resources.Resource，并做转换
-                    final_resource = convert_resources_to_type(resources_list, final_type)
-                    action_kwargs[k] = self.resource_tracker.figure_resource(final_resource)
+            # 向Host查询物料当前状态，如果是host本身的增加物料的请求，则直接跳过
+            if action_name != "add_resource_from_outer":
+                for k, v in goal.get_fields_and_field_types().items():
+                    if v in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
+                        self.lab_logger().info(f"查询资源状态: Key: {k} Type: {v}")
+                        current_resources = []
+                        try:
+                            if len(action_kwargs[k]) > 1:
+                                for i in action_kwargs[k]:
+                                    r = ResourceGet.Request()
+                                    r.id = i["id"]
+                                    r.with_children = True
+                                    response = await self._resource_clients["resource_get"].call_async(r)
+                                    current_resources.extend(response.resources)
+                            else:
+                                r = ResourceGet.Request()
+                                r.id = action_kwargs[k]["id"] if v == "unilabos_msgs/Resource" else action_kwargs[k][0]["id"]
+                                r.with_children = True
+                                response = await self._resource_clients["resource_get"].call_async(r)
+                                current_resources.extend(response.resources)
+                        except Exception:
+                            logger.error(f"资源查询失败，默认使用本地资源")
+                        # 删除对response.resources的检查，因为它总是存在
+                        resources_list = [convert_from_ros_msg(rs) for rs in current_resources]  # type: ignore  # FIXME
+                        self.lab_logger().debug(f"资源查询结果: {len(resources_list)} 个资源")
+                        type_hint = action_paramtypes[k]
+                        final_type = get_type_class(type_hint)
+                        # 判断 ACTION 是否需要特殊的物料类型如 pylabrobot.resources.Resource，并做转换
+                        final_resource = [convert_resources_to_type([i], final_type)[0] for i in resources_list]
+                        action_kwargs[k] = self.resource_tracker.figure_resource(final_resource)
 
             self.lab_logger().info(f"准备执行: {action_kwargs}, 函数: {ACTION.__name__}")
             time_start = time.time()
@@ -527,27 +609,28 @@ class BaseROS2DeviceNode(Node, Generic[T]):
             del future
 
             # 向Host更新物料当前状态
-            for k, v in goal.get_fields_and_field_types().items():
-                if v not in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
-                    continue
-                self.lab_logger().info(f"更新资源状态: {k}")
-                r = ResourceUpdate.Request()
-                # 仅当action_kwargs[k]不为None时尝试转换
-                akv = action_kwargs[k]
-                apv = action_paramtypes[k]
-                final_type = get_type_class(apv)
-                if final_type is None:
-                    continue
-                try:
-                    r.resources = [
-                        convert_to_ros_msg(Resource, self.resource_tracker.root_resource(rs))
-                        for rs in convert_resources_from_type(akv, final_type)  # type: ignore  # FIXME  # 考虑反查到最大的
-                    ]
-                    response = await self._resource_clients["resource_update"].call_async(r)
-                    self.lab_logger().debug(f"资源更新结果: {response}")
-                except Exception as e:
-                    self.lab_logger().error(f"资源更新失败: {e}")
-                    self.lab_logger().error(traceback.format_exc())
+            if action_name != "add_resource_from_outer":
+                for k, v in goal.get_fields_and_field_types().items():
+                    if v not in ["unilabos_msgs/Resource", "sequence<unilabos_msgs/Resource>"]:
+                        continue
+                    self.lab_logger().info(f"更新资源状态: {k}")
+                    r = ResourceUpdate.Request()
+                    # 仅当action_kwargs[k]不为None时尝试转换
+                    akv = action_kwargs[k]
+                    apv = action_paramtypes[k]
+                    final_type = get_type_class(apv)
+                    if final_type is None:
+                        continue
+                    try:
+                        r.resources = [
+                            convert_to_ros_msg(Resource, self.resource_tracker.root_resource(rs))
+                            for rs in convert_resources_from_type(akv, final_type)  # type: ignore  # FIXME  # 考虑反查到最大的
+                        ]
+                        response = await self._resource_clients["resource_update"].call_async(r)
+                        self.lab_logger().debug(f"资源更新结果: {response}")
+                    except Exception as e:
+                        self.lab_logger().error(f"资源更新失败: {e}")
+                        self.lab_logger().error(traceback.format_exc())
 
             # 发布结果
             goal_handle.succeed()
@@ -627,6 +710,7 @@ class ROS2DeviceNode:
         self,
         device_id: str,
         driver_class: Type[T],
+        device_config: Dict[str, Any],
         driver_params: Dict[str, Any],
         status_types: Dict[str, Any],
         action_value_mappings: Dict[str, Any],
@@ -641,6 +725,8 @@ class ROS2DeviceNode:
         Args:
             device_id: 设备标识符
             driver_class: 设备类
+            device_config: 原始初始化的json
+            driver_params: driver初始化的参数
             status_types: 状态类型映射
             action_value_mappings: 动作值映射
             hardware_interface: 硬件接口配置
@@ -657,11 +743,12 @@ class ROS2DeviceNode:
         # 保存设备类是否支持异步上下文
         self._has_async_context = hasattr(driver_class, "__aenter__") and hasattr(driver_class, "__aexit__")
         self._driver_class = driver_class
+        self.device_config = device_config
         self.driver_is_ros = driver_is_ros
         self.resource_tracker = DeviceNodeResourceTracker()
 
         # use_pylabrobot_creator 使用 cls的包路径检测
-        use_pylabrobot_creator = driver_class.__module__.startswith("pylabrobot")
+        use_pylabrobot_creator = driver_class.__module__.startswith("pylabrobot") or driver_class.__name__ == "DPLiquidHandler"
 
         # TODO: 要在创建之前预先请求服务器是否有当前id的物料，放到resource_tracker中，让pylabrobot进行创建
         # 创建设备类实例

unilabos/ros/nodes/presets/host_node.py

@@ -7,11 +7,13 @@ import uuid
 from typing import Optional, Dict, Any, List, ClassVar, Set
 
 from action_msgs.msg import GoalStatus
-from unilabos_msgs.msg import Resource  # type: ignore
-from unilabos_msgs.srv import ResourceAdd, ResourceGet, ResourceDelete, ResourceUpdate, ResourceList, SerialCommand  # type: ignore
+from geometry_msgs.msg import Point
 from rclpy.action import ActionClient, get_action_server_names_and_types_by_node
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.service import Service
+from unilabos_msgs.msg import Resource  # type: ignore
+from unilabos_msgs.srv import ResourceAdd, ResourceGet, ResourceDelete, ResourceUpdate, ResourceList, \
+    SerialCommand  # type: ignore
 from unique_identifier_msgs.msg import UUID
 
 from unilabos.registry.registry import lab_registry
@@ -23,11 +25,9 @@ from unilabos.ros.msgs.message_converter import (
     convert_from_ros_msg,
     convert_to_ros_msg,
     msg_converter_manager,
-    ros_action_to_json_schema,
 )
 from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode, ROS2DeviceNode, DeviceNodeResourceTracker
 from unilabos.ros.nodes.presets.controller_node import ControllerNode
-from unilabos.utils.type_check import TypeEncoder
 
 
 class HostNode(BaseROS2DeviceNode):
@@ -50,7 +50,7 @@ class HostNode(BaseROS2DeviceNode):
         self,
         device_id: str,
         devices_config: Dict[str, Any],
-        resources_config: Any,
+        resources_config: list,
         physical_setup_graph: Optional[Dict[str, Any]] = None,
         controllers_config: Optional[Dict[str, Any]] = None,
         bridges: Optional[List[Any]] = None,
@@ -76,7 +76,7 @@ class HostNode(BaseROS2DeviceNode):
             driver_instance=self,
             device_id=device_id,
             status_types={},
-            action_value_mappings={},
+            action_value_mappings=lab_registry.device_type_registry["host_node"]["class"]["action_value_mappings"],
             hardware_interface={},
             print_publish=False,
             resource_tracker=DeviceNodeResourceTracker(),  # host node并不是通过initialize 包一层传进来的
@@ -97,15 +97,13 @@ class HostNode(BaseROS2DeviceNode):
         self.bridges = bridges
 
         # 创建设备、动作客户端和目标存储
-        self.devices_names: Dict[str, str] = {}  # 存储设备名称和命名空间的映射
+        self.devices_names: Dict[str, str] = {device_id: self.namespace}  # 存储设备名称和命名空间的映射
         self.devices_instances: Dict[str, ROS2DeviceNode] = {}  # 存储设备实例
         self.device_machine_names: Dict[str, str] = {device_id: "本地", }  # 存储设备ID到机器名称的映射
         self._action_clients: Dict[str, ActionClient] = {}  # 用来存储多个ActionClient实例
-        self._action_value_mappings: Dict[str, Dict] = (
-            {}
-        )  # 用来存储多个ActionClient的type, goal, feedback, result的变量名映射关系
+        self._action_value_mappings: Dict[str, Dict] = {}  # 用来存储多个ActionClient的type, goal, feedback, result的变量名映射关系
         self._goals: Dict[str, Any] = {}  # 用来存储多个目标的状态
-        self._online_devices: Set[str] = set()  # 用于跟踪在线设备
+        self._online_devices: Set[str] = {f"{self.namespace}/{device_id}"}  # 用于跟踪在线设备
         self._last_discovery_time = 0.0  # 上次设备发现的时间
         self._discovery_lock = threading.Lock()  # 设备发现的互斥锁
         self._subscribed_topics = set()  # 用于跟踪已订阅的话题
@@ -259,16 +257,41 @@ class HostNode(BaseROS2DeviceNode):
                     self.lab_logger().debug(f"[Host Node] Created ActionClient (Discovery): {action_id}")
                     action_name = action_id[len(namespace) + 1:]
                     edge_device_id = namespace[9:]
-                    from unilabos.app.mq import mqtt_client
-                    info_with_schema = ros_action_to_json_schema(action_type)
-                    mqtt_client.publish_actions(action_name, {
-                        "device_id": edge_device_id,
-                        "action_name": action_name,
-                        "schema": info_with_schema,
-                    })
+                    # from unilabos.app.mq import mqtt_client
+                    # info_with_schema = ros_action_to_json_schema(action_type)
+                    # mqtt_client.publish_actions(action_name, {
+                    #     "device_id": edge_device_id,
+                    #     "device_type": "",
+                    #     "action_name": action_name,
+                    #     "schema": info_with_schema,
+                    # })
                 except Exception as e:
                     self.lab_logger().error(f"[Host Node] Failed to create ActionClient for {action_id}: {str(e)}")
 
+    def add_resource_from_outer(self, resources: list["Resource"], device_ids: list[str], bind_parent_ids: list[str], bind_locations: list[Point], other_calling_params: list[str]):
+        for resource, device_id, bind_parent_id, bind_location, other_calling_param in zip(resources, device_ids, bind_parent_ids, bind_locations, other_calling_params):
+            # 这里要求device_id传入必须是edge_device_id
+            namespace = "/devices/" + device_id
+            srv_address = f"/srv{namespace}/append_resource"
+            sclient = self.create_client(SerialCommand, srv_address)
+            sclient.wait_for_service()
+            request = SerialCommand.Request()
+            request.command = json.dumps({
+                "resource": resource,
+                "namespace": namespace,
+                "edge_device_id": device_id,
+                "bind_parent_id": bind_parent_id,
+                "bind_location": {
+                    "x": bind_location.x,
+                    "y": bind_location.y,
+                    "z": bind_location.z,
+                },
+                "other_calling_param": json.loads(other_calling_param) if other_calling_param else {},
+            }, ensure_ascii=False)
+            response = sclient.call(request)
+            pass
+        pass
+
     def initialize_device(self, device_id: str, device_config: Dict[str, Any]) -> None:
         """
         根据配置初始化设备，
@@ -297,13 +320,14 @@ class HostNode(BaseROS2DeviceNode):
                 action_type = action_value_mapping["type"]
                 self._action_clients[action_id] = ActionClient(self, action_type, action_id)
                 self.lab_logger().debug(f"[Host Node] Created ActionClient (Local): {action_id}")  # 子设备再创建用的是Discover发现的
-                from unilabos.app.mq import mqtt_client
-                info_with_schema = ros_action_to_json_schema(action_type)
-                mqtt_client.publish_actions(action_name, {
-                    "device_id": device_id,
-                    "action_name": action_name,
-                    "schema": info_with_schema,
-                })
+                # from unilabos.app.mq import mqtt_client
+                # info_with_schema = ros_action_to_json_schema(action_type)
+                # mqtt_client.publish_actions(action_name, {
+                #     "device_id": device_id,
+                #     "device_type": device_config["class"],
+                #     "action_name": action_name,
+                #     "schema": info_with_schema,
+                # })
             else:
                 self.lab_logger().warning(f"[Host Node] ActionClient {action_id} already exists.")
         device_key = f"{self.devices_names[device_id]}/{device_id}"  # 这里不涉及二级device_id
@@ -619,7 +643,8 @@ class HostNode(BaseROS2DeviceNode):
                 self.lab_logger().debug(f"[Host Node-Resource] Retrieved from bridge: {len(r)} resources")
             except Exception as e:
                 self.lab_logger().error(f"[Host Node-Resource] Error retrieving from bridge: {str(e)}")
-                r = []
+                r = [resource for resource in self.resources_config if resource.get("id") == request.id]
+                self.lab_logger().warning(f"[Host Node-Resource] Retrieved from local: {len(r)} resources")
         else:
             # 本地物料服务，根据 id 查询物料
             r = [resource for resource in self.resources_config if resource.get("id") == request.id]

unilabos/ros/nodes/presets/joint_republisher.py

@@ -0,0 +1,60 @@
+import rclpy,json
+from rclpy.node import Node
+from sensor_msgs.msg import JointState 
+from std_msgs.msg import String
+from rclpy.callback_groups import ReentrantCallbackGroup
+from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
+
+class JointRepublisher(BaseROS2DeviceNode):
+    def __init__(self,device_id,resource_tracker):
+        super().__init__(
+            driver_instance=self,
+            device_id=device_id,
+            status_types={},
+            action_value_mappings={},
+            hardware_interface={},
+            print_publish=False,
+            resource_tracker=resource_tracker,  
+        )  
+        
+        # print('-'*20,device_id)
+        self.joint_repub = self.create_publisher(String,f'joint_state_repub',10)
+        # 创建订阅者
+        self.create_subscription(
+            JointState,               
+            '/joint_states',         
+            self.listener_callback,  
+            10,
+            callback_group=ReentrantCallbackGroup()
+        )
+        self.msg = String()
+
+    def listener_callback(self, msg:JointState):
+        
+        try:
+            json_dict = {}
+            json_dict["name"]           = list(msg.name)
+            json_dict["position"]       = list(msg.position)
+            json_dict["velocity"]       = list(msg.velocity)
+            json_dict["effort"]         = list(msg.effort)
+
+            self.msg.data = str(json_dict)
+            self.joint_repub.publish(self.msg)
+            # print('-'*20)
+            # print(self.msg.data)
+            
+        except Exception as e:
+            print(e)
+
+
+def main():
+
+    rclpy.init()
+    subscriber = JointRepublisher()
+    rclpy.spin(subscriber)
+    subscriber.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

unilabos/ros/nodes/resource_tracker.py

@@ -1,7 +1,7 @@
 from unilabos.utils.log import logger
 
 
-class DeviceNodeResourceTracker:
+class DeviceNodeResourceTracker(object):
 
     def __init__(self):
         self.resources = []
@@ -15,43 +15,46 @@ class DeviceNodeResourceTracker:
             return resource
 
     def add_resource(self, resource):
-        # 使用内存地址跟踪是否为同一个resource
         for r in self.resources:
             if id(r) == id(resource):
                 return
-        # 添加资源到跟踪器
         self.resources.append(resource)
 
     def clear_resource(self):
         self.resources = []
 
-    def figure_resource(self, resource):
-        # 使用内存地址跟踪是否为同一个resource
-        if isinstance(resource, list):
-            return [self.figure_resource(r) for r in resource]
-        res_id = resource.id if hasattr(resource, "id") else None
-        res_name = resource.name if hasattr(resource, "name") else None
+    def figure_resource(self, query_resource):
+        if isinstance(query_resource, list):
+            return [self.figure_resource(r) for r in query_resource]
+        res_id = query_resource.id if hasattr(query_resource, "id") else (query_resource.get("id") if isinstance(query_resource, dict) else None)
+        res_name = query_resource.name if hasattr(query_resource, "name") else (query_resource.get("name") if isinstance(query_resource, dict) else None)
         res_identifier = res_id if res_id else res_name
         identifier_key = "id" if res_id else "name"
-        resource_cls_type = type(resource)
+        resource_cls_type = type(query_resource)
         if res_identifier is None:
-            logger.warning(f"resource {resource} 没有id或name，暂不能对应figure")
+            logger.warning(f"resource {query_resource} 没有id或name，暂不能对应figure")
         res_list = []
         for r in self.resources:
-            res_list.extend(
-                self.loop_find_resource(r, resource_cls_type, identifier_key, getattr(resource, identifier_key))
-            )
-        assert len(res_list) == 1, f"找到多个资源，请检查资源是否唯一: {res_list}"
-        self.root_resource2resource[id(resource)] = res_list[0]
+            if isinstance(query_resource, dict):
+                res_list.extend(
+                    self.loop_find_resource(r, resource_cls_type, identifier_key, query_resource[identifier_key])
+                )
+            else:
+                res_list.extend(
+                    self.loop_find_resource(r, resource_cls_type, identifier_key, getattr(query_resource, identifier_key))
+                )
+        assert len(res_list) == 1, f"{query_resource} 找到多个资源，请检查资源是否唯一: {res_list}"
+        self.root_resource2resource[id(query_resource)] = res_list[0]
         # 后续加入其他对比方式
         return res_list[0]
 
-    def loop_find_resource(self, resource, resource_cls_type, identifier_key, compare_value):
+    def loop_find_resource(self, resource, target_resource_cls_type, identifier_key, compare_value):
         res_list = []
+        # print(resource, target_resource_cls_type, identifier_key, compare_value)
         children = getattr(resource, "children", [])
         for child in children:
-            res_list.extend(self.loop_find_resource(child, resource_cls_type, identifier_key, compare_value))
-        if resource_cls_type == type(resource):
+            res_list.extend(self.loop_find_resource(child, target_resource_cls_type, identifier_key, compare_value))
+        if target_resource_cls_type == type(resource) or target_resource_cls_type == dict:
             if hasattr(resource, identifier_key):
                 if getattr(resource, identifier_key) == compare_value:
                     res_list.append(resource)

unilabos/ros/utils/driver_creator.py

@@ -6,6 +6,7 @@
 """
 import asyncio
 import inspect
+import json
 import traceback
 from abc import abstractmethod
 from typing import Type, Any, Dict, Optional, TypeVar, Generic
@@ -218,12 +219,22 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
                         logger.error(f"PyLabRobot反序列化失败: {deserialize_error}")
                         logger.error(f"PyLabRobot反序列化堆栈: {stack}")
 
-            return self.device_instance
+        return self.device_instance
 
     def post_create(self):
         if hasattr(self.device_instance, "setup") and asyncio.iscoroutinefunction(getattr(self.device_instance, "setup")):
             from unilabos.ros.nodes.base_device_node import ROS2DeviceNode
-            ROS2DeviceNode.run_async_func(getattr(self.device_instance, "setup")).add_done_callback(lambda x: logger.debug(f"PyLabRobot设备实例 {self.device_instance} 设置完成"))
+            def done_cb(*args):
+                logger.debug(f"PyLabRobot设备实例 {self.device_instance} 设置完成")
+                from unilabos.config.config import BasicConfig
+                if BasicConfig.vis_2d_enable:
+                    from pylabrobot.visualizer.visualizer import Visualizer
+                    vis = Visualizer(resource=self.device_instance, open_browser=True)
+                    def vis_done_cb(*args):
+                        logger.info(f"PyLabRobot设备实例开启了Visualizer {self.device_instance}")
+                    ROS2DeviceNode.run_async_func(vis.setup).add_done_callback(vis_done_cb)
+                    logger.debug(f"PyLabRobot设备实例提交开启Visualizer {self.device_instance}")
+            ROS2DeviceNode.run_async_func(getattr(self.device_instance, "setup")).add_done_callback(done_cb)
 
 
 class ProtocolNodeCreator(DeviceClassCreator[T]):