ros python使用tf2的代码

在ROS (Robot Operating System) 中，TF (Transform Framework) 的Python版本（tf2）主要用于处理机器人坐标系之间的转换和同步。以下是一个简单的例子，展示了如何创建一个静态变换并监听变化：

import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped

# 初始化节点
rospy.init_node('tf_example')

# 创建TF客户端
listener = tf2_ros.TransformListener()

def callback(data):
    # 数据回调函数，处理TransformStamped消息
    print("Received transform from {} to {}: \n{}".format(
        data.child_frame_id, data.header.frame_id, str(data.transform)))

# 创建一个静态变换发布者，从"world"到"base_link"
static_transformBroadcaster = tf2_ros.StaticTransformBroadcaster()
static_msg = TransformStamped()
static_msg.header.stamp = rospy.Time.now()  # 设置当前时间
static_msg.header.frame_id = "world"  # 定义源帧
static_msg.child_frame_id = "base_link"  # 定义目标帧
static_msg.transform.translation.x = 0  # 假设x轴位置为0
static_msg.transform.translation.y = 0
static_msg.transform.translation.z = 0
static_msg.transform.rotation.x = 0  # 设置旋转为零
static_msg.transform.rotation.y = 0
static_msg.transform.rotation.z = 0
static_msg.transform.rotation.w = 1  # 设置w为1，表示四元数的单位

# 每秒广播一次静态变换
rate = rospy.Rate(1)
while not rospy.is_shutdown():
    static_transformBroadcaster.sendTransform(static_msg)
    rate.sleep()

# 注册回调函数，监听特定的transform
subscriber = tf2_ros.TransformListener(listener)
subscriber.waitForTransform("/base_link", "/camera_color_optical_frame", rospy.Time(), rospy.Duration(4.0))
subscriber.registerCallback(callback)

# 运行节点，直到ROS关闭
rospy.spin()

在这个例子中，我们首先初始化ROS节点，然后创建一个`tf2_ros.TransformListener`用于监听变换。`StaticTransformBroadcaster`则用于定期发送一个静态的变换。`callback`函数会在接收到新的变换数据时打印出来。