添加一个帧（Python）

目标: 学习如何在tf2中添加额外的帧。

教程级别： 中级

时间： 15分钟

背景

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

对于与变换相关的许多任务，以本地帧为基础更容易思考。例如，在激光扫描测量中，最容易理解的是以激光扫描仪中心的帧为基准。tf2允许你为系统中的每个传感器、链接或关节定义一个本地帧。在从一个帧转换到另一个帧时，tf2将处理引入的所有隐藏中间帧变换。

tf2树

tf2构建了一个帧的树结构，因此不允许帧结构中存在闭环。这意味着一个帧只有一个父帧，但可以有多个子帧。目前，我们的tf2树包含三个帧：world、turtle1 和 turtle2。两个乌龟帧是 world 帧的子帧。如果我们想要在tf2中添加一个新的帧，需要选择三个现有帧中的一个作为父帧，新的帧将成为其子帧。

任务

1. 编写固定帧广播器

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

首先，我们创建源文件。进入之前教程中创建的 learning_tf2_py 包。通过输入以下命令下载固定帧广播器的代码：

LinuxmacOSWindows

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

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster


class FixedFrameBroadcaster(Node):

   def __init__(self):
       super().__init__('fixed_frame_tf2_broadcaster')
       self.tf_broadcaster = TransformBroadcaster(self)
       self.timer = self.create_timer(0.1, self.broadcast_timer_callback)

   def broadcast_timer_callback(self):
       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
       t.transform.rotation.x = 0.0
       t.transform.rotation.y = 0.0
       t.transform.rotation.z = 0.0
       t.transform.rotation.w = 1.0

       self.tf_broadcaster.sendTransform(t)


def main():
    rclpy.init()
    node = FixedFrameBroadcaster()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

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

1.1 检查代码

让我们看一下这段代码中的关键部分。在这里，我们创建了一个新的变换，从父框架“turtle1”到新的子框架“carrot1”。相对于“turtle1”框架，框架“carrot1”在y轴上偏移了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 编写启动文件

现在让我们为这个示例创建一个启动文件。使用文本编辑器创建一个名为``launch/turtle_tf2_fixed_frame_demo.launch.py``的新文件，并添加以下行：

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('learning_tf2_py'), 'launch'),
            '/turtle_tf2_demo.launch.py']),
        )

    return LaunchDescription([
        demo_nodes,
        Node(
            package='learning_tf2_py',
            executable='fixed_frame_tf2_broadcaster',
            name='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，以检查缺少的依赖项。

LinuxmacOSWindows

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

仍然在工作空间的根目录中，构建您的软件包：

LinuxmacOSWindows

colcon build --packages-select learning_tf2_py

打开一个新终端，进入你的工作空间的根目录，并源化设置文件：

LinuxmacOSWindows

. install/setup.bash

1.5 运行

现在您已经准备好运行启动文件：

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py

请注意新的 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``，您将看到第二只乌龟跟随胡萝卜而不是第一只乌龟！

2 编写动态帧广播器

在本教程中，我们发布的额外帧是一个固定的帧，相对于父帧不会随时间变化。然而，如果你想要发布一个移动的帧，你可以编写广播器以随时间改变帧。让我们将``carrot1``帧改变成相对于``turtle1``帧随时间变化的帧。现在通过输入以下命令下载动态帧广播器的代码：

LinuxmacOSWindows

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

from geometry_msgs.msg import TransformStamped

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster


class DynamicFrameBroadcaster(Node):

    def __init__(self):
        super().__init__('dynamic_frame_tf2_broadcaster')
        self.tf_broadcaster = TransformBroadcaster(self)
        self.timer = self.create_timer(0.1, self.broadcast_timer_callback)

    def broadcast_timer_callback(self):
        seconds, _ = self.get_clock().now().seconds_nanoseconds()
        x = seconds * math.pi

        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 = 10 * math.sin(x)
        t.transform.translation.y = 10 * math.cos(x)
        t.transform.translation.z = 0.0
        t.transform.rotation.x = 0.0
        t.transform.rotation.y = 0.0
        t.transform.rotation.z = 0.0
        t.transform.rotation.w = 1.0

        self.tf_broadcaster.sendTransform(t)


def main():
    rclpy.init()
    node = DynamicFrameBroadcaster()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

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 编写启动文件

要测试此代码，请创建一个新的启动文件 launch/turtle_tf2_dynamic_frame_demo.launch.py，并粘贴以下代码：

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('learning_tf2_py'), 'launch'),
            '/turtle_tf2_demo.launch.py']),
       launch_arguments={'target_frame': 'carrot1'}.items(),
       )

    return LaunchDescription([
        demo_nodes,
        Node(
            package='learning_tf2_py',
            executable='dynamic_frame_tf2_broadcaster',
            name='dynamic_broadcaster',
        ),
    ])

2.4 构建

在你的工作空间的根目录中运行 rosdep，以检查缺少的依赖项。

LinuxmacOSWindows

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

仍然在工作空间的根目录中，构建您的软件包：

LinuxmacOSWindows

colcon build --packages-select learning_tf2_py

打开一个新终端，进入你的工作空间的根目录，并源化设置文件：

LinuxmacOSWindows

. install/setup.bash

1.5 运行

现在您已经准备好运行启动文件：

ros2 launch learning_tf2_py turtle_tf2_dynamic_frame_demo.launch.py

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

总结

在本教程中，你学习了tf2变换树的结构和特性。你还学会了在一个本地框架内进行思考，并学会为该本地框架添加额外的固定和动态框架。