【ROS2】中级:tf2-添加框架（Python）

目标：学习如何向 tf2 添加额外的框架。

教程级别：中级

 时间：15 分钟

 目录

•  背景

•  tf2 树

•  任务

  • 1 编写固定帧广播器

  • 1.1 检查代码

  • 1.2 添加入口点

  • 1.3 编写启动文件

  •  1.4 构建

  •  1.5 运行

  • 2 编写动态帧广播器

  • 2.1 检查代码

  • 2.2 添加入口点

  • 2.3 编写启动文件

  •  2.4 构建

  •  2.5 运行

•  摘要

 背景

在之前的教程中，我们通过编写 tf2 广播器和 tf2 监听器重新创建了乌龟演示。本教程将教您如何向变换树添加额外的固定和动态帧。事实上，在 tf2 中添加帧与创建 tf2 广播器非常相似，但本示例将向您展示 tf2 的一些附加功能。

对于许多与变换相关的任务，在局部坐标系中思考更容易。例如，最容易在激光扫描仪中心的坐标系中推理激光扫描测量值。tf2 允许您为系统中的每个传感器、链接或关节定义一个局部坐标系。当从一个坐标系变换到另一个坐标系时，tf2 将处理所有引入的隐藏中间坐标系变换。

 任务

1 编写固定帧广播器

在我们的乌龟示例中，我们将添加一个新的框架 carrot1 ，它将是 turtle1 的子框架。这个框架将作为第二只乌龟的目标。

首先创建源文件。进入我们在前面的教程中创建的 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/fixed_frame_tf2_broadcaster.py

现在打开名为 fixed_frame_tf2_broadcaster.py 的文件。

from geometry_msgs.msg import TransformStamped  # 从 geometry_msgs.msg 导入 TransformStamped 消息类型
 
 
import rclpy  # 导入 rclpy 库
from rclpy.node import Node  # 从 rclpy.node 导入 Node 类
 
 
from tf2_ros import TransformBroadcaster  # 从 tf2_ros 导入 TransformBroadcaster 类
 
 
 
 
class FixedFrameBroadcaster(Node):  # 定义一个名为 FixedFrameBroadcaster 的类，继承自 Node 类
 
 
    def __init__(self):  # 初始化方法
        super().__init__('fixed_frame_tf2_broadcaster')  # 调用父类的初始化方法，并设置节点名称为 'fixed_frame_tf2_broadcaster'
        self.tf_broadcaster = TransformBroadcaster(self)  # 创建一个 TransformBroadcaster 对象
        self.timer = self.create_timer(0.1, self.broadcast_timer_callback)  # 创建一个定时器，每 0.1 秒调用一次 broadcast_timer_callback 方法
 
 
    def broadcast_timer_callback(self):  # 定义定时器回调方法
        t = TransformStamped()  # 创建一个 TransformStamped 对象
 
 
        t.header.stamp = self.get_clock().now().to_msg()  # 设置时间戳为当前时间
        t.header.frame_id = 'turtle1'  # 设置父坐标系 ID 为 'turtle1'
        t.child_frame_id = 'carrot1'  # 设置子坐标系 ID 为 'carrot1'
        t.transform.translation.x = 0.0  # 设置平移变换的 x 坐标为 0.0
        t.transform.translation.y = 2.0  # 设置平移变换的 y 坐标为 2.0
        t.transform.translation.z = 0.0  # 设置平移变换的 z 坐标为 0.0
        t.transform.rotation.x = 0.0  # 设置旋转变换的 x 分量为 0.0
        t.transform.rotation.y = 0.0  # 设置旋转变换的 y 分量为 0.0
        t.transform.rotation.z = 0.0  # 设置旋转变换的 z 分量为 0.0
        t.transform.rotation.w = 1.0  # 设置旋转变换的 w 分量为 1.0
 
 
        self.tf_broadcaster.sendTransform(t)  # 发送变换
 
 
 
 
def main():  # 主函数
    rclpy.init()  # 初始化 rclpy
    node = FixedFrameBroadcaster()  # 创建一个 FixedFrameBroadcaster 节点
    try:
        rclpy.spin(node)  # 运行节点
    except KeyboardInterrupt:  # 捕捉键盘中断异常
        pass
 
 
    rclpy.shutdown()  # 关闭 rclpy

代码与 tf2 广播器教程示例非常相似，唯一的区别是这里的变换不会随时间变化。

1.1 检查代码

让我们看看这段代码中的关键行。这里我们创建了一个新的变换，从父 turtle1 到新的子 carrot1 。 carrot1 框架在 y 轴上相对于 turtle1 框架偏移了 2 米。

t = TransformStamped()
 
 
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'turtle1'
t.child_frame_id = 'carrot1'
t.transform.translation.x = 0.0
t.transform.translation.y = 2.0
t.transform.translation.z = 0.0

1.2 添加入口点

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

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

'fixed_frame_tf2_broadcaster = learning_tf2_py.fixed_frame_tf2_broadcaster:main',

1.3 编写启动文件

现在让我们为这个示例创建一个启动文件。使用您的文本编辑器，在 src/learning_tf2_py/launch 目录中创建一个名为 turtle_tf2_fixed_frame_demo_launch.py 的新文件，并添加以下几行代码：

import os  # 导入os模块，用于处理文件和目录
 
 
from ament_index_python.packages import get_package_share_directory  # 导入get_package_share_directory函数，用于获取包的共享目录
 
 
from launch import LaunchDescription  # 导入LaunchDescription类，用于描述启动配置
from launch.actions import IncludeLaunchDescription  # 导入IncludeLaunchDescription类，用于包含其他的启动描述
from launch.launch_description_sources import PythonLaunchDescriptionSource  # 导入PythonLaunchDescriptionSource类，用于从Python文件加载启动描述
 
 
from launch_ros.actions import Node  # 导入Node类，用于描述ROS节点
 
 
 
 
def generate_launch_description():  # 定义一个名为generate_launch_description的函数
    demo_nodes = IncludeLaunchDescription(  # 创建一个IncludeLaunchDescription对象
        PythonLaunchDescriptionSource([os.path.join(  # 使用os.path.join函数拼接路径
            get_package_share_directory('learning_tf2_py'), 'launch'),  # 获取'learning_tf2_py'包的共享目录，并加上'launch'
            '/turtle_tf2_demo_launch.py']),  # 加上'/turtle_tf2_demo_launch.py'
        )
 
 
    return LaunchDescription([  # 返回一个LaunchDescription对象
        demo_nodes,  # 包含demo_nodes
        Node(  # 创建一个Node对象
            package='learning_tf2_py',  # 设置包名为'learning_tf2_py'
            executable='fixed_frame_tf2_broadcaster',  # 设置可执行文件名为'fixed_frame_tf2_broadcaster'
            name='fixed_broadcaster',  # 设置节点名为'fixed_broadcaster'
        ),
    ])

此启动文件导入所需的包，然后创建一个 demo_nodes 变量，该变量将存储我们在上一个教程的启动文件中创建的节点。

代码的最后部分将使用我们的 fixed_frame_tf2_broadcaster 节点将固定的 carrot1 框架添加到 turtlesim 世界中。

Node(
    package='learning_tf2_py',
    executable='fixed_frame_tf2_broadcaster',
    name='fixed_broadcaster',
),

 1.4 构建 

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

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

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

colcon build --packages-select learning_tf2_py

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

. install/setup.bash

 1.5 运行 

现在你可以开始乌龟广播演示了

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo_launch.py

您应该注意到新的 carrot1 框架出现在变换树中。

 ros2 run tf2_tools view_frames

如果你驾驶第一只乌龟，你应该注意到行为没有从以前的教程中改变，即使我们添加了一个新的框架。那是因为添加一个额外的框架不会影响其他框架，我们的监听器仍然使用先前定义的框架。

因此，如果我们希望第二只乌龟跟随carrot1而不是第一只乌龟，我们需要更改 target_frame 的值。这可以通过两种方式完成。一种方法是直接从控制台将 target_frame 参数传递给启动文件：

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo_launch.py target_frame:=carrot1

‍第二种方法是更新启动文件。为此，请打开 turtle_tf2_fixed_frame_demo_launch.py 文件，并通过 launch_arguments 参数添加 'target_frame': 'carrot1' 参数。

def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        ...,
        launch_arguments={'target_frame': 'carrot1'}.items(),
        )

现在重新构建包，重启 turtle_tf2_fixed_frame_demo_launch.py ，你会看到第二只乌龟跟着胡萝卜carrot1而不是第一只乌龟！

2 编写动态帧广播器

我们在本教程中发布的额外帧是一个固定帧，它不会随时间相对于父帧发生变化。然而，如果您想发布一个移动帧，可以编写广播器代码使帧随时间变化。让我们更改我们的 carrot1 帧，使其随时间相对于 turtle1 帧发生变化。转到我们在上一个教程中创建的 learning_tf2_py 包。在 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/dynamic_frame_tf2_broadcaster.py

现在打开名为 dynamic_frame_tf2_broadcaster.py 的文件：

import math  # 导入 math 库
 
 
from geometry_msgs.msg import TransformStamped  # 从 geometry_msgs.msg 导入 TransformStamped 消息类型
 
 
import rclpy  # 导入 rclpy 库
from rclpy.node import Node  # 从 rclpy.node 导入 Node 类
 
 
from tf2_ros import TransformBroadcaster  # 从 tf2_ros 导入 TransformBroadcaster 类
 
 
 
 
class DynamicFrameBroadcaster(Node):  # 定义一个名为 DynamicFrameBroadcaster 的类，继承自 Node 类
 
 
    def __init__(self):  # 初始化方法
        super().__init__('dynamic_frame_tf2_broadcaster')  # 调用父类的初始化方法，并设置节点名称为 'dynamic_frame_tf2_broadcaster'
        self.tf_broadcaster = TransformBroadcaster(self)  # 创建一个 TransformBroadcaster 对象
        self.timer = self.create_timer(0.1, self.broadcast_timer_callback)  # 创建一个定时器，每 0.1 秒调用一次 broadcast_timer_callback 方法
 
 
    def broadcast_timer_callback(self):  # 定义定时器回调方法
        seconds, _ = self.get_clock().now().seconds_nanoseconds()  # 获取当前时间的秒数
        x = seconds * math.pi  # 计算 x 值
 
 
        t = TransformStamped()  # 创建一个 TransformStamped 对象
        t.header.stamp = self.get_clock().now().to_msg()  # 设置时间戳为当前时间
        t.header.frame_id = 'turtle1'  # 设置父坐标系 ID 为 'turtle1'
        t.child_frame_id = 'carrot1'  # 设置子坐标系 ID 为 'carrot1'
        t.transform.translation.x = 10 * math.sin(x)  # 设置平移变换的 x 坐标为 10 * sin(x)
        t.transform.translation.y = 10 * math.cos(x)  # 设置平移变换的 y 坐标为 10 * cos(x)
        t.transform.translation.z = 0.0  # 设置平移变换的 z 坐标为 0.0
        t.transform.rotation.x = 0.0  # 设置旋转变换的 x 分量为 0.0
        t.transform.rotation.y = 0.0  # 设置旋转变换的 y 分量为 0.0
        t.transform.rotation.z = 0.0  # 设置旋转变换的 z 分量为 0.0
        t.transform.rotation.w = 1.0  # 设置旋转变换的 w 分量为 1.0
 
 
        self.tf_broadcaster.sendTransform(t)  # 发送变换
 
 
 
 
def main():  # 主函数
    rclpy.init()  # 初始化 rclpy
    node = DynamicFrameBroadcaster()  # 创建一个 DynamicFrameBroadcaster 节点
    try:
        rclpy.spin(node)  # 运行节点
    except KeyboardInterrupt:  # 捕捉键盘中断异常
        pass
 
 
    rclpy.shutdown()  # 关闭 rclpy

2.1 检查代码 

相对于我们 x 和 y 偏移量的固定定义，我们使用当前时间的 sin() 和 cos() 函数，因此 carrot1 的偏移量在不断变化。

seconds, _ = self.get_clock().now().seconds_nanoseconds()
x = seconds * math.pi
...
t.transform.translation.x = 10 * math.sin(x)
t.transform.translation.y = 10 * math.cos(x)

2.2 添加入口点

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

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

'dynamic_frame_tf2_broadcaster = learning_tf2_py.dynamic_frame_tf2_broadcaster:main',

2.3 编写启动文件 

要测试此代码，请在 src/learning_tf2_py/launch 目录中创建一个新的启动文件 turtle_tf2_dynamic_frame_demo_launch.py ，并粘贴以下代码：

import os  # 导入 os 模块
 
 
from ament_index_python.packages import get_package_share_directory  # 从 ament_index_python.packages 导入 get_package_share_directory 函数
 
 
from launch import LaunchDescription  # 从 launch 导入 LaunchDescription 类
from launch.actions import IncludeLaunchDescription  # 从 launch.actions 导入 IncludeLaunchDescription 类
from launch.launch_description_sources import PythonLaunchDescriptionSource  # 从 launch.launch_description_sources 导入 PythonLaunchDescriptionSource 类
 
 
from launch_ros.actions import Node  # 从 launch_ros.actions 导入 Node 类
 
 
 
 
def generate_launch_description():  # 定义 generate_launch_description 函数
    demo_nodes = IncludeLaunchDescription(  # 创建 IncludeLaunchDescription 对象
        PythonLaunchDescriptionSource([os.path.join(  # 使用 PythonLaunchDescriptionSource 指定启动文件路径
            get_package_share_directory('learning_tf2_py'), 'launch'),  # 获取 'learning_tf2_py' 包的共享目录，并指定 'launch' 文件夹
            '/turtle_tf2_demo_launch.py']),  # 指定启动文件名
       launch_arguments={'target_frame': 'carrot1'}.items(),  # 设置启动参数 'target_frame' 为 'carrot1'
       )
 
 
    return LaunchDescription([  # 返回 LaunchDescription 对象
        demo_nodes,  # 包含 demo_nodes
        Node(  # 创建 Node 对象
            package='learning_tf2_py',  # 指定包名为 'learning_tf2_py'
            executable='dynamic_frame_tf2_broadcaster',  # 指定可执行文件名为 'dynamic_frame_tf2_broadcaster'
            name='dynamic_broadcaster',  # 设置节点名称为 'dynamic_broadcaster'
        ),
    ])

 2.4 构建 

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

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

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

colcon build --packages-select learning_tf2_py

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

. install/setup.bash

 1.5 运行 

现在你可以开始动态框架演示了：

ros2 launch learning_tf2_py turtle_tf2_dynamic_frame_demo_launch.py

你应该看到第二只乌龟正在跟随不断变化的胡萝卜的位置。

 摘要

在本教程中，您了解了 tf2 变换树、其结构及其特性。您还了解到，在本地框架内思考是最容易的，并学会了为该本地框架添加额外的固定和动态框架。