编写广播器（Python）

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

教程级别： 中级

时间： 15分钟

背景

在接下来的两个教程中，我们将编写代码来重现 Introduction to tf2 教程中的演示。之后，后续教程将重点介绍如何使用更高级的tf2功能扩展演示，包括在转换查找中使用超时和时间旅行。

先决条件

本教程假设你具有ROS 2的基本知识，并且已经完成了：Introduction to tf2 tutorial。在之前的教程中，你学会了如何创建工作空间和软件包。你还创建了``learning_tf2_py``包，我们将从这里继续工作。

任务

1. 编写广播器节点

首先，让我们创建源代码文件。进入之前创建的``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/turtle_tf2_broadcaster.py

使用您偏爱的文本编辑器打开该文件。

import math

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster

from turtlesim.msg import Pose


def quaternion_from_euler(ai, aj, ak):
    ai /= 2.0
    aj /= 2.0
    ak /= 2.0
    ci = math.cos(ai)
    si = math.sin(ai)
    cj = math.cos(aj)
    sj = math.sin(aj)
    ck = math.cos(ak)
    sk = math.sin(ak)
    cc = ci*ck
    cs = ci*sk
    sc = si*ck
    ss = 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):

    def __init__(self):
        super().__init__('turtle_tf2_frame_publisher')

        # Declare and acquire `turtlename` parameter
        self.turtlename = self.declare_parameter(
          'turtlename', 'turtle').get_parameter_value().string_value

        # Initialize the transform broadcaster
        self.tf_broadcaster = TransformBroadcaster(self)

        # Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
        # callback function on each message
        self.subscription = self.create_subscription(
            Pose,
            f'/{self.turtlename}/pose',
            self.handle_turtle_pose,
            1)
        self.subscription  # prevent unused variable warning

    def handle_turtle_pose(self, msg):
        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

        # 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]

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


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

    rclpy.shutdown()

1.1 检查代码

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

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

接下来，节点订阅了 turtleX/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. 然后，我们需要设置正在创建的链接的父帧的名称，本例中为 world。

3. 最后，我们需要设置链接创建的子节点的名称，这里的子节点名称就是乌龟自身的名称。

处理乌龟姿态消息的处理函数会广播该乌龟的平移和旋转，并将其发布为从帧``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)

注解

你也可以通过实例化``tf2_ros.StaticTransformBroadcaster``而不是``tf2_ros.TransformBroadcaster``来发布相同模式的静态变换。静态变换将发布到``/tf_static``话题，并且仅在需要时发送，而不是周期性地发送。有关更多详细信息，请参阅：此处。

1.2 添加入口点

为了让 ros2 run 命令能够运行你的节点，你必须在 setup.py``（位于 ``src/learning_tf2_py 目录中）中添加入口点。

最后，在``'console_scripts':``括号之间添加以下行：

'turtle_tf2_broadcaster = learning_tf2_py.turtle_tf2_broadcaster:main',

2 编写启动文件

现在为此演示创建一个启动文件。使用文本编辑器，在``launch``文件夹中创建一个名为``turtle_tf2_demo.launch.py``的新文件，并添加以下内容：

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='learning_tf2_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'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 添加依赖项

返回到``src/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 依赖项。

确保保存文件.

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

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

3 构建

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

LinuxmacOSWindows

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

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

LinuxmacOSWindows

colcon build --packages-select learning_tf2_py

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

LinuxmacOSWindows

. 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``终端窗口处于活动状态，而不是仿真器窗口）。在控制台输出中，你会看到类似于以下内容:

At time 1625137663.912474878
- Translation: [5.276, 7.930, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137664.950813527
- Translation: [3.750, 6.563, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137665.906280726
- Translation: [2.320, 5.282, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137666.850775673
- Translation: [2.153, 5.133, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.365, 0.931]

如果你对``world``和``turtle2``之间的变换运行``tf2_echo``命令，你将不会看到任何变换，因为第二只乌龟尚未出现。然而，一旦我们在下一个教程中添加第二只乌龟，``turtle2``的位姿将会被广播到tf2。

总结

在本教程中，您学习了如何将机器人的姿态（乌龟的位置和方向）广播到tf2，以及如何使用``tf2_echo``工具。要实际使用广播到tf2的变换，请继续下一个有关创建:doc:`tf2监听器 <./Writing-A-Tf2-Listener-Py>`的教程。