【ROS2】中级:tf2-编写广播器（Python）

目标：学习如何将机器人的状态广播到 tf2。

教程级别：中级

 时间：15 分钟

 目录

•  背景

•  先决条件

•  任务

  • 1. 编写广播器节点

  • 2. 编写启动文件

  •  3. 构建

  •  4.运行

•  摘要

 背景

在接下来的两个教程中，我们将编写代码来复现“tf2 入门”教程中的演示。之后，后续教程将专注于使用更高级的 tf2 功能来扩展演示，包括在变换查找和时间旅行中使用超时。

 先决条件

本教程假设您具有 ROS 2 的工作知识，并且您已经完成了 tf2 教程和 tf2 静态广播器教程（Python）的介绍。我们将重用上一个教程中的 learning_tf2_py 包。

在之前的教程中，您学习了如何创建工作区和创建包。

 任务

1. 编写广播器节点

让我们先创建源文件。转到我们在上一教程中创建的 learning_tf2_py 包。在 src/learning_tf2_py/learning_tf2_py 目录中，通过输入以下命令下载示例广播代码：

cxy@ubuntu2404-cxy:~$ cd ~/ros2_ws/src/learning_tf2_py/learning_tf2_py
cxy@ubuntu2404-cxy:~/ros2_ws/src/learning_tf2_py/learning_tf2_py$ wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py

现在使用您喜欢的文本编辑器打开名为 turtle_tf2_broadcaster.py 的文件。

# 版权 2021 开源机器人基金会
#
# 根据 Apache 许可证 2.0 版（“许可证”）授权；
# 除非符合许可证，否则您不得使用此文件。
# 您可以在以下网址获取许可证副本：
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# 除非适用法律要求或书面同意，否则根据许可证分发的软件
# 是按“原样”分发的，不附带任何明示或暗示的担保或条件。
# 请参阅许可证以了解管理权限和限制的特定语言。


import math  # 导入数学模块


from geometry_msgs.msg import TransformStamped  # 从geometry_msgs模块导入TransformStamped消息类型


import numpy as np  # 导入numpy模块并简写为np


import rclpy  # 导入rclpy模块
from rclpy.node import Node  # 从rclpy.node模块导入Node类


from tf2_ros import TransformBroadcaster  # 从tf2_ros模块导入TransformBroadcaster类


from turtlesim.msg import Pose  # 从turtlesim.msg模块导入Pose消息类型




# 这个函数是简化版的代码，来自
# https://github.com/matthew-brett/transforms3d/blob/f185e866ecccb66c545559bc9f2e19cb5025e0ab/transforms3d/euler.py
# 除了简化之外，这个版本还颠倒了顺序以返回x,y,z,w，这是ROS所偏好的方式。
def quaternion_from_euler(ai, aj, ak):
    ai /= 2.0  # 将ai除以2.0
    aj /= 2.0  # 将aj除以2.0
    ak /= 2.0  # 将ak除以2.0
    ci = math.cos(ai)  # 计算ai的余弦值
    si = math.sin(ai)  # 计算ai的正弦值
    cj = math.cos(aj)  # 计算aj的余弦值
    sj = math.sin(aj)  # 计算aj的正弦值
    ck = math.cos(ak)  # 计算ak的余弦值
    sk = math.sin(ak)  # 计算ak的正弦值
    cc = ci*ck  # 计算ci与ck的乘积
    cs = ci*sk  # 计算ci与sk的乘积
    sc = si*ck  # 计算si与ck的乘积
    ss = si*sk  # 计算si与sk的乘积


    q = np.empty((4, ))  # 创建一个空的四元数数组
    q[0] = cj*sc - sj*cs  # 计算四元数的第一个分量
    q[1] = cj*ss + sj*cc  # 计算四元数的第二个分量
    q[2] = cj*cs - sj*sc  # 计算四元数的第三个分量
    q[3] = cj*cc + sj*ss  # 计算四元数的第四个分量


    return q  # 返回四元数




class FramePublisher(Node):  # 定义FramePublisher类，继承自Node类


    def __init__(self):
        super().__init__('turtle_tf2_frame_publisher')  # 调用父类的构造函数，初始化节点名称为'turtle_tf2_frame_publisher'


        # 声明并获取`turtlename`参数
        self.turtlename = self.declare_parameter(
            'turtlename', 'turtle').get_parameter_value().string_value


        # 初始化变换广播器
        self.tf_broadcaster = TransformBroadcaster(self)


        # 订阅turtle{1}{2}/pose主题，并在每个消息上调用handle_turtle_pose回调函数
        self.subscription = self.create_subscription(
            Pose,  # 消息类型，这里是 Pose 类型
            f'/{self.turtlename}/pose',  # 主题名称，这里是根据乌龟的名字动态生成的主题
            self.handle_turtle_pose,  # 回调函数，当接收到消息时调用这个函数
            1  # 队列大小，表示最多可以缓存多少条消息
            )
        self.subscription  # 防止未使用变量警告


    def handle_turtle_pose(self, msg):
        t = TransformStamped()  # 创建TransformStamped对象


        # 读取消息内容并将其分配给相应的tf变量
        t.header.stamp = self.get_clock().now().to_msg()  # 设置时间戳
        t.header.frame_id = 'world'  # 设置父坐标系ID
        t.child_frame_id = self.turtlename  # 设置子坐标系ID


        # 乌龟只存在于2D，因此我们从消息中获取x和y平移坐标，并将z坐标设置为0
        t.transform.translation.x = msg.x  # 设置x平移坐标
        t.transform.translation.y = msg.y  # 设置y平移坐标
        t.transform.translation.z = 0.0  # 设置z平移坐标为0


        # 出于同样的原因，乌龟只能绕一个轴旋转，这就是为什么我们将x和y的旋转设置为0，并从消息中获取z轴的旋转
        q = quaternion_from_euler(0, 0, msg.theta)  # 计算四元数
        t.transform.rotation.x = q[0]  # 设置四元数的x分量
        t.transform.rotation.y = q[1]  # 设置四元数的y分量
        t.transform.rotation.z = q[2]  # 设置四元数的z分量
        t.transform.rotation.w = q[3]  # 设置四元数的w分量


        # 发送变换
        self.tf_broadcaster.sendTransform(t)




def main():  # 定义主函数
    rclpy.init()  # 初始化ROS客户端库
    node = FramePublisher()  # 创建FramePublisher节点
    try:
        rclpy.spin(node)  # 让节点保持运行，直到被中断
    except KeyboardInterrupt:  # 如果收到键盘中断异常，则跳过
        pass


    rclpy.shutdown()  # 关闭ROS客户端库

1.1 检查代码

现在，让我们来看一下与发布乌龟姿态到 tf2 相关的代码。首先，我们定义并获取一个参数 turtlename ，它指定了一个乌龟的名称，例如 turtle1 或 turtle2 。

self.turtlename = self.declare_parameter(
  'turtlename', 'turtle').get_parameter_value().string_value

随后，节点订阅主题 {self.turtlename}/pose 并在每条传入消息上运行函数 handle_turtle_pose 。

self .subscription = self.create_subscription(
    Pose,
    f'/{self.turtlename}/pose',
    self.handle_turtle_pose,
    1)

现在，我们创建一个 TransformStamped 对象，并为其提供适当的元数据。

1. 我们需要为正在发布的变换加上一个时间戳，我们将通过调用 self.get_clock().now() 来用当前时间来标记它。这将返回 Node 使用的当前时间。

2. 然后我们需要设置我们正在创建的 Link 的父框架的名称，在这种情况下是 world 。

3. 最后，我们需要设置我们正在创建的 Link 的子节点的名称，在这种情况下，这就是乌龟本身的名称。

处理函数为 turtle 姿态消息广播此 turtle 的平移和旋转，并将其作为从框架 world 到框架 turtleX 的变换进行发布。

t = TransformStamped()


# Read message content and assign it to
# corresponding tf variables
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'world'
t.child_frame_id = self.turtlename

在这里，我们将 3D 乌龟姿势的信息复制到 3D 变换中。

# Turtle only exists in 2D, thus we get x and y translation
# coordinates from the message and set the z coordinate to 0
t.transform.translation.x = msg.x
t.transform.translation.y = msg.y
t.transform.translation.z = 0.0


# For the same reason, turtle can only rotate around one axis
# and this why we set rotation in x and y to 0 and obtain
# rotation in z axis from the message
q = quaternion_from_euler(0, 0, msg.theta)
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]

最后我们将构建的转换传递给 TransformBroadcaster 的 sendTransform 方法，由它来负责广播。

# Send the transformation
self.tf_broadcaster.sendTransform(t)

1.2 添加一个入口点 

要允许 ros2 run 命令运行您的节点，您必须将入口点添加到 setup.py （位于 src/learning_tf2_py 目录中）。

在 'console_scripts': 括号之间添加以下行：

'turtle_tf2_broadcaster = learning_tf2_py.turtle_tf2_broadcaster:main',

2. 编写启动文件

现在为这个演示创建一个启动文件。在 src/learning_tf2_py 目录中创建一个 launch 文件夹。使用你的文本编辑器，在 launch 文件夹中创建一个名为 turtle_tf2_demo_launch.py 的新文件，并添加以下几行：

from launch import LaunchDescription  # 从launch模块导入LaunchDescription类
from launch_ros.actions import Node  # 从launch_ros.actions模块导入Node类




def generate_launch_description():  # 定义generate_launch_description函数
    return LaunchDescription([  # 返回一个LaunchDescription对象
        Node(
            package='turtlesim',  # 指定节点所属的包名为'turtlesim'
            executable='turtlesim_node',  # 指定可执行文件名为'turtlesim_node'
            name='sim'  # 指定节点名称为'sim'
        ),
        Node(
            package='learning_tf2_py',  # 指定节点所属的包名为'learning_tf2_py'
            executable='turtle_tf2_broadcaster',  # 指定可执行文件名为'turtle_tf2_broadcaster'
            name='broadcaster1',  # 指定节点名称为'broadcaster1'
            parameters=[
                {'turtlename': 'turtle1'}  # 设置参数'turtlename'的值为'turtle1'
            ]
        ),
    ])

2.1 检查代码 

首先，我们从 launch 和 launch_ros 包中导入所需的模块。应当指出， launch 是一个通用的启动框架（不特定于 ROS 2），而 launch_ros 包含了 ROS 2 特定的内容，比如我们在这里导入的节点。

from launch import LaunchDescription
from launch_ros.actions import Node

现在我们运行节点，启动 turtlesim 模拟并使用我们的 turtle_tf2_broadcaster 节点将 turtle1 状态广播到 tf2。

Node(
    package='turtlesim',
    executable='turtlesim_node',
    name='sim'
),
Node(
    package='learning_tf2_py',
    executable='turtle_tf2_broadcaster',
    name='broadcaster1',
    parameters=[
        {'turtlename': 'turtle1'}
    ]
),

 2.2 添加依赖项 

返回到 learning_tf2_py 目录，该目录中包含 setup.py 、 setup.cfg 和 package.xml 文件。

打开 package.xml 并使用文本编辑器。根据启动文件的导入语句添加以下依赖项：

<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>

此声明在执行其代码时需要额外的 launch 和 launch_ros 依赖项。

确保保存文件。

<?xml version="1.0"?> <!-- XML版本为1.0 -->
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <!-- XML模型的链接和模型类型 -->
<package format="3"> <!-- 包格式为3 -->
  <name>learning_tf2_py</name> <!-- 包名为learning_tf2_py -->
  <version>0.0.0</version> <!-- 版本号为0.0.0 -->
  <description>Learning tf2 with rclpy</description> <!-- 包的描述 -->
  <maintainer email="cxy@126.com">cxy</maintainer> <!-- 维护者的信息 -->
  <license>Apache-2.0</license> <!-- 许可证信息 -->


  <exec_depend>geometry_msgs</exec_depend> <!-- 执行依赖：geometry_msgs -->
  <exec_depend>python3-numpy</exec_depend> <!-- 执行依赖：python3-numpy -->
  <exec_depend>rclpy</exec_depend> <!-- 执行依赖：rclpy -->
  <exec_depend>tf2_ros_py</exec_depend> <!-- 执行依赖：tf2_ros_py -->
  <exec_depend>turtlesim</exec_depend> <!-- 执行依赖：turtlesim -->
  
  <exec_depend>launch</exec_depend> <!-- 执行依赖：launch -->
  <exec_depend>launch_ros</exec_depend> <!-- 执行依赖：launch_ros -->
  
  <test_depend>ament_copyright</test_depend> <!-- 测试依赖：ament_copyright -->
  <test_depend>ament_flake8</test_depend> <!-- 测试依赖：ament_flake8 -->
  <test_depend>ament_pep257</test_depend> <!-- 测试依赖：ament_pep257 -->
  <test_depend>python3-pytest</test_depend> <!-- 测试依赖：python3-pytest -->


  <export>
    <build_type>ament_python</build_type> <!-- 构建类型为ament_python -->
  </export>
</package> <!-- 包定义结束 -->

 2.3 更新 setup.py 

重新打开 setup.py 并添加该行，以便安装来自 launch/ 文件夹的启动文件。 data_files 字段现在应该如下所示：

data_files=[
    ...
    (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
],

同时在文件顶部添加适当的导入：

import os
from glob import glob

您可以在本教程中了解更多关于创建启动文件的信息。

import os
from glob import glob
from setuptools import find_packages, setup  # 导入setuptools模块的find_packages和setup函数


package_name = 'learning_tf2_py'  # 定义包名为learning_tf2_py


setup(  # 使用setup函数进行包的设置
    name=package_name,  # 包名
    version='0.0.0',  # 版本号
    packages=find_packages(exclude=['test']),  # 使用find_packages函数找到所有的包，排除名为'test'的包
    data_files=[  # 数据文件
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),  # 资源文件
        ('share/' + package_name, ['package.xml']),  # 包的XML描述文件
        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),  # launch文件
    ],
    install_requires=['setuptools'],  # 安装依赖，需要setuptools
    zip_safe=True,  # 设置zip_safe为True，表示此包可以安全地作为zip文件进行分发
    maintainer='cxy',  # 维护者名字
    maintainer_email='cxy@126.com',  # 维护者邮箱
    description='TODO: Package description',  # 包的描述
    license='Apache-2.0',  # 许可证信息
    tests_require=['pytest'],  # 测试依赖，需要pytest
    entry_points={  # 入口点
        'console_scripts': [  # 控制台脚本
          'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main',  # 静态turtle_tf2广播器的主函数
          'turtle_tf2_broadcaster = learning_tf2_py.turtle_tf2_broadcaster:main',  # turtle_tf2广播器的主函数
        ],
    },
)

 3. 构建

在工作区的根目录运行 rosdep 以检查缺失的依赖项。

rosdep install -i --from-path src --rosdistro jazzy -y

仍在工作区的根目录下，构建您的包：

colcon build --packages-select learning_tf2_py

打开一个新的终端，导航到工作区的根目录，然后加载设置文件：

. install/setup.bash

4. 运行

现在运行启动文件，它将启动 turtlesim 仿真节点和 turtle_tf2_broadcaster 节点：

ros2 launch learning_tf2_py turtle_tf2_demo_launch.py

在第二个终端窗口中输入以下命令：

ros2 run turtlesim turtle_teleop_key

您现在将看到 turtlesim 模拟已经开始，您可以控制一只乌龟。

现在，使用 tf2_echo 工具来检查海龟姿势是否真的正在向 tf2 广播：

ros2 run tf2_ros tf2_echo world turtle1

这应该会显示第一只乌龟的姿势。使用箭头键绕着乌龟开（确保你的 turtle_teleop_key 终端窗口是活跃的，不是你的模拟器窗口）。在你的控制台输出中，你会看到类似这样的东西：

cxy@ubuntu2404-cxy:~$ ros2 run tf2_ros tf2_echo world turtle1
[INFO] [1720690130.610127107] [tf2_echo]: Waiting for transform world ->  turtle1: Invalid frame ID "world" passed to canTransform argument target_frame - frame does not exist
At time 1720690131.206094840
- Translation: [3.543, 6.060, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.982, -0.188]
- Rotation: in RPY (radian) [0.000, -0.000, -2.763]
- Rotation: in RPY (degree) [0.000, -0.000, -158.319]
- Matrix:
 -0.929  0.369  0.000  3.543
 -0.369 -0.929  0.000  6.060
  0.000  0.000  1.000  0.000
  0.000  0.000  0.000  1.000

如果您运行 tf2_echo 以实现 world 和 turtle2 之间的转换，您不应该看到任何转换，因为第二只乌龟还没有出现。然而，一旦我们在下一个教程中添加了第二只乌龟， turtle2 的姿态将会广播到 tf2。

 摘要

在本教程中，您学习了如何将机器人的姿态（乌龟的位置和方向）广播到 tf2，以及如何使用 tf2_echo 工具。为了实际使用广播到 tf2 的变换，您应该继续学习下一教程，关于创建 tf2 监听器。