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>
2025-12-17 04:51:10 +00:00 · 2025-05-07 17:37:03 +08:00
parent d1fbea3b7d
commit 5a564c0c05
93 changed files with 13081 additions and 211 deletions
--- a/unilabos/ros/nodes/base_device_node.py
+++ b/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进行创建
        # 创建设备类实例