【入门 ROS2】ROS TF发布与监听

ROS TF发布与监听

在ROS（Robot Operating System）中，TF库用于跟踪多个坐标系之间的变换，并保持它们在时间上的一致性。TF提供了一种简单的方法来发布和接收坐标变换，使得我们可以轻松地处理机器人系统中的复杂几何关系。本文将详细介绍如何使用TF发布和监听坐标变换。

1. TF概述

TF是ROS中的一个库，它允许用户跟踪多个坐标系之间的相对变换。TF可以帮助我们回答如下问题：

• 给定时刻t，某个点在不同坐标系中的位置是什么？
• 如何将传感器数据从一个坐标系转换到另一个坐标系？

2. 安装依赖

确保你已经安装了tf和tf2_ros包。如果没有安装，可以使用以下命令进行安装：

sudo apt-get install ros-<distro>-tf
sudo apt-get install ros-<distro>-tf2-ros

将<distro>替换为你的ROS发行版名称，例如melodic、noetic等。

3. TF发布（Broadcaster）

发布者（Broadcaster）用于发布坐标系之间的变换。我们将创建一个简单的示例，发布从父坐标系到子坐标系的变换。

3.1 创建发布者节点

首先，在功能包目录下新建一个scripts文件夹，然后在scripts文件夹下新建一个Python文件，命名为tf_broadcaster.py。

cd ~/catkin_ws/src/your_package_name
mkdir scripts
cd scripts
touch tf_broadcaster.py
chmod +x tf_broadcaster.py # 增加可执行权限

3.2 编写发布者代码

在tf_broadcaster.py文件中添加以下内容：

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import tf
from geometry_msgs.msg import TransformStamped

def broadcaster():
    rospy.init_node('tf_broadcaster', anonymous=True)
    br = tf.TransformBroadcaster()
    
    rate = rospy.Rate(10)  # 10Hz
    
    while not rospy.is_shutdown():
        # 获取当前时间
        now = rospy.Time.now()
        
        # 定义变换信息
        transform = TransformStamped()
        transform.header.stamp = now
        transform.header.frame_id = "parent_frame"
        transform.child_frame_id = "child_frame"
        
        transform.transform.translation.x = 1.0
        transform.transform.translation.y = 2.0
        transform.transform.translation.z = 0.0
        
        quaternion = tf.transformations.quaternion_from_euler(0, 0, 1.57)  # Roll, Pitch, Yaw
        transform.transform.rotation.x = quaternion[0]
        transform.transform.rotation.y = quaternion[1]
        transform.transform.rotation.z = quaternion[2]
        transform.transform.rotation.w = quaternion[3]
        
        # 发布变换信息
        br.sendTransform(
            (transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z),
            (transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w),
            now,
            "child_frame",
            "parent_frame"
        )
        
        rate.sleep()

if __name__ == '__main__':
    try:
        broadcaster()
    except rospy.ROSInterruptException:
        pass

4. TF监听（Listener）

监听者（Listener）用于接收并处理坐标系之间的变换。我们将创建一个简单的示例，监听并获取从父坐标系到子坐标系的变换。

4.1 创建监听者节点

同样，在scripts文件夹下新建一个Python文件，命名为tf_listener.py。

touch tf_listener.py
chmod +x tf_listener.py # 增加可执行权限

4.2 编写监听者代码

在tf_listener.py文件中添加以下内容：

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import tf

def listener():
    rospy.init_node('tf_listener', anonymous=True)
    
    listener = tf.TransformListener()
    
    rate = rospy.Rate(10)  # 10Hz
    
    while not rospy.is_shutdown():
        try:
            now = rospy.Time.now()
            listener.waitForTransform("/parent_frame", "/child_frame", now, rospy.Duration(4.0))
            (trans, rot) = listener.lookupTransform('/parent_frame', '/child_frame', now)
            
            rospy.loginfo("Translation: x=%f, y=%f, z=%f", trans[0], trans[1], trans[2])
            rospy.loginfo("Rotation: x=%f, y=%f, z=%f, w=%f", rot[0], rot[1], rot[2], rot[3])
            
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue
        
        rate.sleep()

if __name__ == '__main__':
    try:
        listener()
    except rospy.ROSInterruptException:
        pass

5. 启动文件

为了方便启动TF发布和监听节点，我们可以创建一个启动文件。

5.1 创建启动文件

在功能包目录下的新建一个launch文件夹，然后在该文件夹下新建一个launch文件，命名为tf_demo.launch。

cd ~/catkin_ws/src/your_package_name
mkdir launch
cd launch
touch tf_demo.launch

5.2 编写启动文件内容

在tf_demo.launch文件中添加以下内容：

<launch>
    <node name="tf_broadcaster" pkg="your_package_name" type="tf_broadcaster.py" output="screen"/>
    <node name="tf_listener" pkg="your_package_name" type="tf_listener.py" output="screen"/>
</launch>

确保将所有实例中的 your_package_name 替换为你的功能包名称。

6. 运行程序

运行程序前，需要先启动roscore：

roscore &

然后使用以下命令启动TF发布和监听节点：

roslaunch your_package_name tf_demo.launch &

这将同时启动TF发布者和监听者节点，并在控制台上输出监听到的变换信息。