添加框架 (Python)

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

教程级别： 中级

时间： 15 分钟

背景

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

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

tf2 树

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

任务

1 编写固定帧广播器

在我们的海龟示例中，我们将添加一个新帧“carrot1”，它将是“turtle1”的子帧。

此帧将作为第二只海龟的目标。

让我们首先创建源文件。 转到我们在前面的教程中创建的“learning_tf2_py”包。 在“src/learning_tf2_py/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 编写启动文件

现在让我们为这个示例创建一个启动文件。 使用文本编辑器，在“src/learning_tf2_py/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 rolling -y

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

LinuxmacOSWindows

colcon build --packages-select learning_tf2_py

打开一个新终端，导航到工作区的根目录，然后获取安装文件：

LinuxmacOSWindows

. install/setup.bash

1.5 运行

现在您可以启动 turtle 广播器演示：

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo_launch.py

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

如果你驾驶第一只乌龟四处走动，你应该注意到，尽管我们添加了一个新框架，但其行为与上一个教程相比并没有变化。 这是因为添加额外的框架不会影响其他框架，我们的侦听器仍在使用先前定义的框架。 因此，如果我们希望第二只乌龟跟随胡萝卜而不是第一只乌龟，我们需要更改“target_frame”的值。 这可以通过两种方式完成。 一种方法是直接从控制台将“ta​​rget_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”帧随时间而变化。 转到我们在上一个教程中创建的“learning_tf2_py”包。 在“src/learning_tf2_py/learning_tf2_py”目录中，输入以下命令下载动态帧广播器代码：

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

为了测试此代码，在“src/learning_tf2_py/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 rolling -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 变换树、其结构及其功能。 您还了解到，在局部框架内思考最容易，并学会了为该局部框架添加额外的固定和动态框架。