编写广播器（Python）

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

教程级别： 中级

时间： 15 分钟

背景 

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

先决条件 

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

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

任务 

1 编写广播器节点 

我们首先创建源文件。转到我们在上一个教程中创建的 learning_tf2_py 包。在 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

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py

In a Windows command line prompt:

curl -sk https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py -o turtle_tf2_broadcaster.py

Or in powershell:

curl https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py -o turtle_tf2_broadcaster.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

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

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

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

我们需要为正在发布的变换提供时间戳，我们只需通过调用 self.get_clock().now() 为其加上当前时间标记即可。这将返回 Node 使用的当前时间。
然后，我们需要设置我们正在创建的链接的父框架的名称，在本例中为 world。
最后，我们需要设置我们正在创建的链接的子节点的名称，在本例中，这是海龟本身的名称。

海龟姿势消息的处理函数广播此海龟的平移和旋转，并将其发布为从框架 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
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 添加依赖项

导航回上一级到“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”以检查是否缺少依赖项。

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

rosdep only runs on Linux, so you will need to install geometry_msgs and turtlesim dependencies yourself

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

colcon build --packages-select learning_tf2_py

colcon build --packages-select learning_tf2_py

colcon build --merge-install --packages-select learning_tf2_py

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

. install/setup.bash

. install/setup.bash

# CMD
call install\setup.bat

# Powershell
.\install\setup.ps1

4 运行 

现在运行启动文件，它将启动 turtlesim 模拟节点和“turtle_tf2_broadcaster”节点：

ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

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

ros2 run turtlesim turtle_teleop_key

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

../../../_images/turtlesim_broadcast.png

现在，使用“tf2_echo”工具检查海龟姿势是否真正被广播到tf2：

ros2 run tf2_ros tf2_echo world turtle1

这应该会向您显示第一只乌龟的姿势。使用箭头键绕着乌龟移动（确保您的“turtle_teleop_key”终端窗口处于活动状态，而不是您的模拟器窗口）。在您的控制台输出中，您将看到类似以下内容：

At time 1714913843.708748879
- Translation: [4.541, 3.889, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.999, -0.035]
- Rotation: in RPY (radian) [0.000, -0.000, -3.072]
- Rotation: in RPY (degree) [0.000, -0.000, -176.013]
- Matrix:
 -0.998  0.070  0.000  4.541
 -0.070 -0.998  0.000  3.889
  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 侦听器的教程。