From 6155ec2798691bf05b37674842952a7119bd610c Mon Sep 17 00:00:00 2001
From: zhangshixiang <@zhangshixiang>
Date: Thu, 24 Apr 2025 01:51:26 +0800
Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E6=88=90TF=E5=8F=91=E5=B8=83?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../devices/ros_dev/resource_mesh_manager.py  | 64 ++++++++++++++++++-
 1 file changed, 62 insertions(+), 2 deletions(-)

diff --git a/unilabos/devices/ros_dev/resource_mesh_manager.py b/unilabos/devices/ros_dev/resource_mesh_manager.py
index f7e4232f..7943189e 100644
--- a/unilabos/devices/ros_dev/resource_mesh_manager.py
+++ b/unilabos/devices/ros_dev/resource_mesh_manager.py
@@ -8,12 +8,15 @@ from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, QoSHistoryPolicy
 from moveit_msgs.msg import CollisionObject, AttachedCollisionObject, AllowedCollisionEntry
 from shape_msgs.msg import Mesh, MeshTriangle, SolidPrimitive
-from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
+from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, TransformStamped
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, QoSHistoryPolicy
 from rclpy.task import Future
 import copy
 from typing import Tuple, Optional, Union, Any, List
+from tf_transformations import quaternion_from_euler
+from tf2_ros import StaticTransformBroadcaster
+
 
 class ResourceMeshManager(Node):
     def __init__(self, resource_model: dict, resource_config: list, node_name: str):
@@ -31,6 +34,10 @@ class ResourceMeshManager(Node):
         self.resource_config_dict = {item['id']: item for item in self.resource_config}
         self.move_group_ready = False
         self.resource_tf_dict = {}
+        self.tf_broadcaster = StaticTransformBroadcaster(self)
+
+        self.create_timer(1, self.publish_resource_tf)
+
         callback_group = ReentrantCallbackGroup()
         self._get_planning_scene_service = self.create_client(
             srv_type=GetPlanningScene,
@@ -99,6 +106,58 @@ class ResourceMeshManager(Node):
                                                                    "position":resource_config['position'],
                                                                    "rotation":resource_config['config']['rotation']}})
 
+    def publish_resource_tf(self):
+        """
+        发布资源之间的TF关系
+        
+        遍历self.resource_tf_dict中的每个元素，根据key，parent，以及position和rotation，
+        发布key和parent之间的tf关系
+        """
+        self.get_logger().info('开始发布资源TF关系')
+        
+        # 创建静态TF广播器
+        
+        
+        # 存储所有需要发布的静态变换
+        transforms = []
+        
+        # 遍历资源TF字典
+        for resource_id, tf_info in self.resource_tf_dict.items():
+            parent = tf_info['parent']
+            position = tf_info['position']
+            rotation = tf_info['rotation']
+            
+            # 创建静态变换消息
+            
+            transform = TransformStamped()
+            transform.header.stamp = self.get_clock().now().to_msg()
+            transform.header.frame_id = parent
+            transform.child_frame_id = resource_id
+            
+            # 设置位置
+            transform.transform.translation.x = float(position['x'])/1000
+            transform.transform.translation.y = float(position['y'])/1000
+            transform.transform.translation.z = float(position['z'])/1000
+            
+            # 从欧拉角转换为四元数
+            q = quaternion_from_euler(
+                float(rotation['x']), 
+                float(rotation['y']), 
+                float(rotation['z'])
+            )
+            
+            # 设置旋转
+            transform.transform.rotation.x = q[0]
+            transform.transform.rotation.y = q[1]
+            transform.transform.rotation.z = q[2]
+            transform.transform.rotation.w = q[3]
+            
+            transforms.append(transform)
+            
+        # 一次性发布所有静态变换
+        if transforms:
+            self.tf_broadcaster.sendTransform(transforms)
+            self.get_logger().info(f'已发布 {len(transforms)} 个资源TF关系')
 
         
     def add_collision_primitive(
@@ -4609,4 +4668,5 @@ if __name__ == '__main__':
     rclpy.init()
     resource_mesh_manager = ResourceMeshManager(resource_model, resource_config, 'resource_mesh_manager')
     resource_mesh_manager.resource_mesh_setup()
-    print(json.dumps(resource_mesh_manager.resource_tf_dict, indent=4, ensure_ascii=False))
\ No newline at end of file
+    print(json.dumps(resource_mesh_manager.resource_tf_dict, indent=4, ensure_ascii=False))
+    rclpy.spin(resource_mesh_manager)
\ No newline at end of file