Device visualization (#39)

* 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 * add resource creat easy action * identify debug msg * mq client id * 提取lh的joint发布 * unify liquid_handler definition * 修改物料跟随与物料添加逻辑修改物料跟随与物料添加逻辑将joint_publisher类移出lh的backends，但仍需要对lh的backends进行一些改写 * Revert "修改物料跟随与物料添加逻辑" This reverts commit 498c997ad7. * Reapply "修改物料跟随与物料添加逻辑" This reverts commit 3a60d2ae81. * Revert "Merge remote-tracking branch 'upstream/dev' into device_visualization" This reverts commit fa727220af, reversing changes made to 498c997ad7. * 修改物料放下时的方法，如果选择修改物料放下时的方法，如果选择drop_trash，则删除物料显示如果选择drop，则让其解除连接 * add biomek.py demo implementation * 更新LiquidHandlerBiomek类，添加资源创建功能，优化协议创建方法，修复部分代码格式问题，更新YAML配置以支持新功能。 * Test * fix biomek success type * Convert LH action to biomek. * Update biomek.py * 注册表上报handle和schema (param input) * 修复biomek缺少的字段 * delete 's' * Remove warnings * Update biomek.py * Biomek test * Update biomek.py * 新增transfer_biomek的msg * New transfer_biomek * Updated transfer_biomek * 更新transfer_biomek的msg * 更新transfer_biomek的msg * 支持Biomek创建 * new action * fix key name typo * New parameter for biomek to run. * Refine * Update * new actions * new actions * 1 * registry * fix biomek startup add action handles * fix handles not as default entry * unilab添加moveit启动 1,整合所有moveit节点到一个move_group中，并整合所有的controller依次激活 2,添加pymoveit2的节点，使用json可直接启动 3,修改机械臂规划方式，添加约束，让冗余关节不会进行过多移动 * biomek_test.py biomek_test.py是最新的版本，运行它会生成complete_biomek_protocol.json * Update biomek.py * biomek_test.py * fix liquid_handler.biomek handles * 修改物体attach时，多次赋值当前时间导致卡顿问题， * Revert "修改物体attach时，多次赋值当前时间导致卡顿问题，" This reverts commit 56d45b94f5. * Reapply "修改物体attach时，多次赋值当前时间导致卡顿问题，" This reverts commit 07d9db20c3. * 添加缺少物料："plate_well_G12", * host node新增resource add时间统计 create_resource新增handle bump version to 0.9.2 * 修正物料上传时间改用biomek_test 增加ResultInfoEncoder 支持返回结果上传 * 正确发送return_info结果 * 同步执行状态信息 * 取消raiseValueError提示 * Update biomek_test.py * 0608 DONE * 同步了Biomek.py 现在应可用 * biomek switch back to non-test * temp disable initialize resource * add * fix tip resource data * liquid states * change to debug level * Revert "change to debug level" This reverts commit 5d9953c3e5. * Reapply "change to debug level" This reverts commit 2487bb6ffc. * fix tip resource data * add full device * add moveit yaml * 修复moveit 增加post_init阶段，给予ros_node反向 * remove necessary node * fix moveit action client * remove necessary imports * Update moveit_interface.py * fix handler_key uppercase * json add liquids * fix setup * add * change to "sources" and "targets" for lh * bump version * remove parent's parent link --------- Co-authored-by: Harvey Que <Q-Query@outlook.com> Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com> Co-authored-by: zhangshixiang <@zhangshixiang> Co-authored-by: Junhan Chang <changjh@pku.edu.cn> Co-authored-by: Guangxin Zhang <guangxin.zhang.bio@gmail.com> Co-authored-by: qxw138 <qxw@stu.pku.edu.cn>
2026-02-08 16:05:14 +00:00 · 2025-06-09 17:06:04 +08:00
parent 729a0fcf0c
commit 133ffaac17
104 changed files with 20893 additions and 2712 deletions
--- a/unilabos/ros/nodes/base_device_node.py
+++ b/unilabos/ros/nodes/base_device_node.py
@@ -740,16 +740,23 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                    self.lab_logger().info(f"更新资源状态: {k}")
                    r = ResourceUpdate.Request()
                    # 仅当action_kwargs[k]不为None时尝试转换
-                    akv = action_kwargs[k]
+                    akv = action_kwargs[k]  # 已经是完成转换的物料了，只需要转换成ros msg Resource了
                    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  # 考虑反查到最大的
-                        ]
+                        seen = set()
+                        unique_resources = []
+                        for rs in akv:
+                            res = self.resource_tracker.parent_resource(rs)  # 获取 resource 对象
+                            if id(res) not in seen:
+                                seen.add(id(res))
+                                converted_list = convert_resources_from_type([res], final_type)
+                                unique_resources.extend([convert_to_ros_msg(Resource, converted) for converted in converted_list])
+
+                        r.resources = unique_resources
+
                        response = await self._resource_clients["resource_update"].call_async(r)
                        self.lab_logger().debug(f"资源更新结果: {response}")
                    except Exception as e:
@@ -917,6 +924,12 @@ class ROS2DeviceNode:
            )
        self._ros_node: BaseROS2DeviceNode
        self._ros_node.lab_logger().info(f"初始化完成 {self._ros_node.uuid} {self.driver_is_ros}")
+        self.driver_instance._ros_node = self._ros_node  # type: ignore
+        if hasattr(self.driver_instance, "post_init"):
+            try:
+                self.driver_instance.post_init(self._ros_node)  # type: ignore
+            except Exception as e:
+                self._ros_node.lab_logger().error(f"设备后初始化失败: {e}")

    def _start_loop(self):
        def run_event_loop():
--- a/unilabos/ros/nodes/presets/resource_mesh_manager.py
+++ b/unilabos/ros/nodes/presets/resource_mesh_manager.py
@@ -91,8 +91,11 @@ class ResourceMeshManager(BaseROS2DeviceNode):
        self.__collision_object_publisher = self.create_publisher(
            CollisionObject, "/collision_object", 10
        )
+        self.__planning_scene_publisher = self.create_publisher(
+            PlanningScene, "/planning_scene", 10
+        )
        self.__attached_collision_object_publisher = self.create_publisher(
-            AttachedCollisionObject, "/attached_collision_object", 10
+            AttachedCollisionObject, "/attached_collision_object", 0
        )

        # 创建一个Action Server用于修改resource_tf_dict
@@ -121,7 +124,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
        """检查move_group节点是否已初始化完成"""

        # 获取当前可用的节点列表
-
+        if len(self.resource_tf_dict) == 0:
+            return
        tf_ready = self.tf_buffer.can_transform("world", next(iter(self.resource_tf_dict.keys())), rclpy.time.Time(),rclpy.duration.Duration(seconds=2))
        
        # if tf_ready:
@@ -129,8 +133,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
            self.move_group_ready = True
            self.publish_resource_tf()
            self.add_resource_collision_meshes(self.resource_tf_dict)
-            
-        # time.sleep(1)
+
       
    def add_resource_mesh_callback(self, goal_handle : ServerGoalHandle):
        tf_update_msg = goal_handle.request
@@ -147,7 +150,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
        """刷新资源配置"""

        registry = lab_registry
-        resource_config = json.loads(resource_config_str)
+        resource_config = json.loads(resource_config_str.replace("'",'"'))
        
        if resource_config['id'] in self.resource_config_dict:
            self.get_logger().info(f'资源 {resource_config["id"]} 已存在')
@@ -158,7 +161,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                self.resource_model[resource_config['id']] = {
                    'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['mesh']}",
                    'mesh_tf': model_config['mesh_tf']}
-                if model_config['children_mesh'] is not None:
+                if 'children_mesh' in model_config.keys():
                    self.resource_model[f"{resource_config['id']}_"] = {
                        'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['children_mesh']}",
                        'mesh_tf': model_config['children_mesh_tf']
@@ -187,7 +190,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):

                pass
            elif parent is not None and resource_id in self.resource_model:
-                parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None","")
+                # parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None_","")
+                parent_link = f"{parent}_device_link".replace("None_","")

            else:

@@ -297,7 +301,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                    "world",
                    resource_id,
                    rclpy.time.Time(seconds=0),
-                    rclpy.duration.Duration(seconds=5)
+                    # rclpy.duration.Duration(seconds=5)
                )
                
                # 提取当前位姿信息
@@ -344,9 +348,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                self.resource_pose_publisher.publish(changed_poses_msg)
            self.zero_count += 1
            
-        
-        
-    
+
    def _is_pose_equal(self, pose1, pose2, tolerance=1e-7):
        """
        比较两个位姿是否相等（考虑浮点数精度）
@@ -386,14 +388,24 @@ class ResourceMeshManager(BaseROS2DeviceNode):
            self.__planning_scene = self._get_planning_scene_service.call(
                GetPlanningScene.Request()
                ).scene
-            
-            for resource_id, target_parent in cmd_dict.items():
+            self.__planning_scene.is_diff = True
+            planning_scene = PlanningScene()
+            planning_scene.is_diff = True
+            planning_scene.robot_state.is_diff = True
+            # time_start = self.get_clock().now()
+            time_start = rclpy.time.Time(seconds=0)
+            count = 0

+            for resource_id, target_parent in cmd_dict.items():
+                parent_id = target_parent
+                if target_parent == '__trash':
+                    parent_id = 'world'
                # 获取从resource_id到target_parent的转换
                transform = self.tf_buffer.lookup_transform(
-                    target_parent,
+                    parent_id,
                    resource_id,
-                    rclpy.time.Time(seconds=0)
+                    time_start,
+                    timeout=rclpy.duration.Duration(seconds=10)
                )
                
                # 提取转换中的位置和旋转信息
@@ -411,26 +423,62 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                }
                
                self.resource_tf_dict[resource_id] = {
-                    "parent": target_parent,
+                    "parent": parent_id,
                    "position": position,
                    "rotation": rotation
                }
                
+                
                # self.attach_collision_object(id=resource_id,link_name=target_parent)
-                collision_object = AttachedCollisionObject(
+                # time.sleep(0.02)
+                operation_attach = CollisionObject.ADD
+                operation_world = CollisionObject.REMOVE
+                if target_parent == 'world':
+                    operation_attach = CollisionObject.REMOVE
+                    operation_world = CollisionObject.ADD
+                elif target_parent == '__trash':
+                    operation_attach = CollisionObject.REMOVE
+
+                world_object = CollisionObject(
                    id=resource_id,
-                    link_name=target_parent,
+                    operation=operation_world
+                )
+                if target_parent != '__trash':
+                    planning_scene.world.collision_objects.append(world_object)
+
+
+                collision_object = AttachedCollisionObject(
                    object=CollisionObject(
                        id=resource_id,
-                        operation=CollisionObject.ADD   
+                        operation=operation_attach   
                    )
                )
-                
-                self.__planning_scene.robot_state.attached_collision_objects.append(collision_object)
+                if target_parent != 'world' and target_parent != '__trash':
+                    collision_object.link_name = target_parent
+                planning_scene.robot_state.attached_collision_objects.append(collision_object)
+
+                count += 1
+
+                if count > 30:
+                    req = ApplyPlanningScene.Request()
+                    req.scene = planning_scene
+                    self.publish_resource_tf()
+                    self._apply_planning_scene_service.call(req)
+                    self.__planning_scene_publisher.publish(planning_scene)
+                    count = 0
+
+                    planning_scene = PlanningScene()
+                    planning_scene.is_diff = True
+                    planning_scene.robot_state.is_diff = True
+
            req = ApplyPlanningScene.Request()
-            req.scene = self.__planning_scene
-            self._apply_planning_scene_service.call_async(req)
+            req.scene = planning_scene
            self.publish_resource_tf()
+            self._apply_planning_scene_service.call(req)
+            self.__planning_scene_publisher.publish(planning_scene)
+
+            # self.__collision_object_publisher.publish(CollisionObject())
+
            
        except Exception as e:
            self.get_logger().error(f"更新资源TF字典失败: {e}")
@@ -440,18 +488,22 @@ class ResourceMeshManager(BaseROS2DeviceNode):
        return SendCmd.Result(success=True)


+
    def add_resource_collision_meshes(self,resource_tf_dict:dict):
        """
        遍历资源配置字典，为每个在resource_model中有对应模型的资源添加碰撞网格
        
        该方法检查每个资源ID是否在self.resource_model中有对应的3D模型文件路径，
-        如果有，则调用add_collision_mesh方法将其添加到碰撞环境中。
+        
        """
        self.get_logger().info('开始添加资源碰撞网格')

        self.__planning_scene = self._get_planning_scene_service.call(
            GetPlanningScene.Request()
        ).scene
+        planning_scene = PlanningScene()
+        planning_scene.is_diff = True
+        count = 0
        for resource_id, tf_info in resource_tf_dict.items():
            
            if resource_id in self.resource_model:
@@ -479,7 +531,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                    quat_xyzw=q,
                    frame_id=resource_id
                )
-                self.__planning_scene.world.collision_objects.append(collision_object)
+                count += 1
+                planning_scene.world.collision_objects.append(collision_object)
            elif f"{tf_info['parent']}_" in self.resource_model:
                # 获取资源的父级框架ID
                id_ = f"{tf_info['parent']}_"
@@ -507,14 +560,26 @@ class ResourceMeshManager(BaseROS2DeviceNode):
                    quat_xyzw=q,
                    frame_id=resource_id
                )
+                count += 1
+                planning_scene.world.collision_objects.append(collision_object)

-                self.__planning_scene.world.collision_objects.append(collision_object)
+            if count > 30:
+                req = ApplyPlanningScene.Request()
+                req.scene = planning_scene
+                self.publish_resource_tf()
+                self._apply_planning_scene_service.call(req)
+                self.__planning_scene_publisher.publish(planning_scene)
+                count = 0
+                
+                planning_scene = PlanningScene()
+                planning_scene.is_diff = True

        req = ApplyPlanningScene.Request()
-        req.scene = self.__planning_scene
-        self._apply_planning_scene_service.call_async(req)
-            
-        
+        req.scene = planning_scene
+        self.publish_resource_tf()
+        self._apply_planning_scene_service.call(req)
+        self.__planning_scene_publisher.publish(planning_scene)
+
        self.get_logger().info('资源碰撞网格添加完成')


@@ -959,9 +1024,6 @@ class ResourceMeshManager(BaseROS2DeviceNode):
        Attach collision object to the robot.
        """

-        if link_name is None:
-            link_name = self.__end_effector_name
-
        msg = AttachedCollisionObject(
            object=CollisionObject(id=id, operation=CollisionObject.ADD)
        )
--- a/unilabos/ros/nodes/resource_tracker.py
+++ b/unilabos/ros/nodes/resource_tracker.py
@@ -1,3 +1,5 @@
+from typing import List, Tuple, Any
+
 from unilabos.utils.log import logger


@@ -5,12 +7,12 @@ class DeviceNodeResourceTracker(object):

    def __init__(self):
        self.resources = []
-        self.root_resource2resource = {}
+        self.resource2parent_resource = {}
        pass

-    def root_resource(self, resource):
-        if id(resource) in self.root_resource2resource:
-            return self.root_resource2resource[id(resource)]
+    def parent_resource(self, resource):
+        if id(resource) in self.resource2parent_resource:
+            return self.resource2parent_resource[id(resource)]
        else:
            return resource

@@ -44,20 +46,21 @@ class DeviceNodeResourceTracker(object):
                    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]
+        self.resource2parent_resource[id(query_resource)] = res_list[0][0]
+        self.resource2parent_resource[id(res_list[0][1])] = res_list[0][0]
        # 后续加入其他对比方式
-        return res_list[0]
+        return res_list[0][1]

-    def loop_find_resource(self, resource, target_resource_cls_type, identifier_key, compare_value):
+    def loop_find_resource(self, resource, target_resource_cls_type, identifier_key, compare_value, parent_res=None) -> List[Tuple[Any, Any]]:
        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, target_resource_cls_type, identifier_key, compare_value))
+            res_list.extend(self.loop_find_resource(child, target_resource_cls_type, identifier_key, compare_value, resource))
        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)
+                    res_list.append((parent_res, resource))
        return res_list

    def filter_find_list(self, res_list, compare_std_dict):