编写静态广播器（Python）

**目标：**学习如何向 tf2 广播静态坐标框架。

教程级别： 中级

时间： 15分钟

背景

发布静态转换对于定义机器人底盘与其传感器或静止部件之间的关系非常有用。例如，对于在激光扫描仪中心的坐标框架中的激光扫描测量，最容易进行推理。

这是一个独立的教程，介绍了静态变换的基础知识，由两部分组成。在第一部分中，我们将编写代码将静态变换发布到tf2。在第二部分中，我们将解释如何使用命令行中的 static_transform_publisher 可执行工具，该工具位于 tf2_ros 中。

在接下来的两个教程中，我们将编写代码以重现 Introduction to tf2 教程中的演示。之后，下面的教程将侧重于使用更高级的tf2功能扩展演示。

先决条件

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

任务

1 创建一个包

首先，我们将创建一个用于本教程和接下来的教程的软件包。该软件包名为 learning_tf2_py，它将依赖于 geometry_msgs、python3-numpy、rclpy、tf2_ros_py 和 turtlesim。本教程的代码存储在 这里。

打开一个新的终端，并 source ROS 2 安装目录，以使 ros2 命令可用。导航到工作空间的 src 文件夹并创建一个新的包：

ros2 pkg create --build-type ament_python learning_tf2_py

您的终端将返回一条消息，验证您的软件包``learning_tf2_py``及其所有必要的文件和文件夹的创建。

2 编写静态广播节点

首先让我们创建源文件。在``src/learning_tf2_py/learning_tf2_py``目录中，通过输入以下命令下载示例静态广播器代码：

LinuxmacOSWindows

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

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

import math
import sys

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.node import Node

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster


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 StaticFramePublisher(Node):
    """
    Broadcast transforms that never change.

    This example publishes transforms from `world` to a static turtle frame.
    The transforms are only published once at startup, and are constant for all
    time.
    """

    def __init__(self, transformation):
        super().__init__('static_turtle_tf2_broadcaster')

        self.tf_static_broadcaster = StaticTransformBroadcaster(self)

        # Publish static transforms once at startup
        self.make_transforms(transformation)

    def make_transforms(self, transformation):
        t = TransformStamped()

        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = transformation[1]

        t.transform.translation.x = float(transformation[2])
        t.transform.translation.y = float(transformation[3])
        t.transform.translation.z = float(transformation[4])
        quat = quaternion_from_euler(
            float(transformation[5]), float(transformation[6]), float(transformation[7]))
        t.transform.rotation.x = quat[0]
        t.transform.rotation.y = quat[1]
        t.transform.rotation.z = quat[2]
        t.transform.rotation.w = quat[3]

        self.tf_static_broadcaster.sendTransform(t)


def main():
    logger = rclpy.logging.get_logger('logger')

    # obtain parameters from command line arguments
    if len(sys.argv) != 8:
        logger.info('Invalid number of parameters. Usage: \n'
                    '$ ros2 run learning_tf2_py static_turtle_tf2_broadcaster'
                    'child_frame_name x y z roll pitch yaw')
        sys.exit(1)

    if sys.argv[1] == 'world':
        logger.info('Your static turtle name cannot be "world"')
        sys.exit(2)

    # pass parameters and initialize node
    rclpy.init()
    node = StaticFramePublisher(sys.argv)
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

2.1 检查代码

现在让我们来看一下与将静态海龟姿势发布到tf2有关的代码。首先导入所需的软件包。首先从``geometry_msgs``中导入``TransformStamped``，它为我们提供了一个要发布到变换树的消息模板。

from geometry_msgs.msg import TransformStamped

然后导入``rclpy``，以便可以使用其``Node``类。

import rclpy
from rclpy.node import Node

tf2_ros``软件包提供了一个``StaticTransformBroadcaster，用于简化发布静态变换。要使用``StaticTransformBroadcaster``，我们需要从``tf2_ros``模块中导入它。

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

StaticFramePublisher``类的构造函数使用名称``static_turtle_tf2_broadcaster``初始化节点。然后创建``StaticTransformBroadcaster，它将在启动时发送一个静态变换。

self.tf_static_broadcaster = StaticTransformBroadcaster(self)
self.make_transforms(transformation)

在这里，我们创建了一个``TransformStamped``对象，这将是我们在填充后发送的消息。在传递实际的变换值之前，我们需要为其提供适当的元数据。

1. 我们需要给正在发布的变换添加一个时间戳，我们将使用当前时间进行标记，self.get_clock().now()。

2. 然后，我们需要设置链接的父框架的名称，这里是``world``。

3. 最后，我们需要设置链接的子框架的名称。

t = TransformStamped()

t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'world'
t.child_frame_id = transformation[1]

在这里，我们填充了海龟的6D姿态（平移和旋转）。

t.transform.translation.x = float(transformation[2])
t.transform.translation.y = float(transformation[3])
t.transform.translation.z = float(transformation[4])
quat = quaternion_from_euler(
    float(transformation[5]), float(transformation[6]), float(transformation[7]))
t.transform.rotation.x = quat[0]
t.transform.rotation.y = quat[1]
t.transform.rotation.z = quat[2]
t.transform.rotation.w = quat[3]

最后，我们使用``sendTransform()``函数广播静态变换。

self.tf_static_broadcaster.sendTransform(t)

2.2 添加依赖项

导航到``src/learning_tf2_py``目录的上一级，其中已为您创建了``setup.py``、``setup.cfg``和``package.xml``文件。

用文本编辑器打开``package.xml``文件。

如在:doc:创建包教程 <../../Beginner-Client-Libraries/Creating-Your-First-ROS2-Package>`中所述，请确保填写``<description>`, ``<maintainer>``和``<license>``标签：

<description>Learning tf2 with rclpy</description>
<maintainer email="you@email.com">Your Name</maintainer>
<license>Apache License 2.0</license>

在上述行之后，添加与您节点的导入语句对应的以下依赖项：

<exec_depend>geometry_msgs</exec_depend>
<exec_depend>python3-numpy</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>tf2_ros_py</exec_depend>
<exec_depend>turtlesim</exec_depend>

当执行节点的代码时，这将声明所需的``geometry_msgs``、python3-numpy、rclpy、``tf2_ros_py``和``turtlesim``依赖项。

确保保存文件.

2.3 添加入口点

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

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

'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main',

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 运行

现在运行``static_turtle_tf2_broadcaster``节点：

ros2 run learning_tf2_py static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

这将为``mystaticturtle``设置一个海龟姿态广播，使其浮在地面上方1米处。

现在我们可以通过回显``tf_static``话题来检查静态变换是否已发布。

ros2 topic echo /tf_static

如果一切正常，你应该看到一个单独的静态变换。

transforms:
- header:
   stamp:
      sec: 1622908754
      nanosec: 208515730
   frame_id: world
child_frame_id: mystaticturtle
transform:
   translation:
      x: 0.0
      y: 0.0
      z: 1.0
   rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

发布静态变换的正确方法。

本教程旨在展示如何使用``StaticTransformBroadcaster``发布静态变换。在实际开发过程中，你不应该自己编写这段代码，而应该使用专用的``tf2_ros``工具来完成。``tf2_ros``提供了一个名为``static_transform_publisher``的可执行文件，可以作为命令行工具或作为你可以添加到启动文件中的节点来使用。

使用以米为单位的x/y/z偏移量和以弧度为单位的roll/pitch/yaw来将静态坐标变换发布到tf2。在我们的例子中，roll/pitch/yaw分别指的是绕x/y/z轴的旋转。

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --yaw yaw --pitch pitch --roll roll --frame-id frame_id --child-frame-id child_frame_id

使用米为单位的 x/y/z 偏移量和四元数在 tf2 中发布一个静态坐标变换。

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --qx qx --qy qy --qz qz --qw qw --frame-id frame_id --child-frame-id child_frame_id

static_transform_publisher 既可以作为命令行工具进行手动使用，也可以在 launch 文件中用于设置静态变换。例如：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments = ['--x', '0', '--y', '0', '--z', '1', '--yaw', '0', '--pitch', '0', '--roll', '0', '--frame-id', 'world', '--child-frame-id', 'mystaticturtle']
        ),
    ])

请注意，除了 --frame-id 和 --child-frame-id 之外的所有参数都是可选的；如果没有指定特定的选项，将假设为恒等变换。

总结

在本教程中，您学习了静态变换如何有助于定义帧之间的静态关系，例如相对于 world 帧的 mystaticturtle。此外，您还学习了静态变换如何有助于理解传感器数据，例如激光扫描仪的数据，通过将数据与公共坐标帧关联起来。最后，您编写了自己的节点以将静态变换发布到 tf2，并学习了如何使用 static_transform_publisher 可执行文件和 launch 文件来发布所需的静态变换。