编写广播程序（Python）

目标 了解如何向 tf2 广播机器人的状态。

辅导水平： 中级

时间 15 分钟

背景介绍

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

先决条件

本教程假定您已经掌握了 ROS 2 的工作知识，并且已经完成了 tf2 教程简介 和 tf2 静态广播器教程（Python）.我们将重复使用 学习_tf2_py 软件包。

在前面的教程中，您学习了如何 创建工作区 和 创建软件包.

任务

1 写入广播员节点

首先创建源文件。转到 学习_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

现在打开名为 turtle_tf2_broadcaster.py 使用您喜欢的文本编辑器。

舶来品 算术

从 几何_msgs.msg 舶来品 TransformStamped

舶来品 numpy 作为 np

舶来品 rclpy
从 rclpy.node 舶来品 节点

从 tf2_ros 舶来品 改造播音员

从 turtlesim.msg 舶来品 姿势


捍卫 来自欧拉的四元数(ai, aj, 牦牛):
    ai /= 2.0
    aj /= 2.0
    牦牛 /= 2.0
    ci = 算术.系数(ai)
    si = 算术.罪过(ai)
    cj = 算术.系数(aj)
    sj = 算术.罪过(aj)
    ck = 算术.系数(牦牛)
    鳐鱼 = 算术.罪过(牦牛)
    cc = ci*ck
    cs = ci*鳐鱼
    科学 = si*ck
    ss = si*鳐鱼

    q = np.空的((4, ))
    q[0] = cj*科学 - sj*cs
    q[1] = cj*ss + sj*cc
    q[2] = cj*cs - sj*科学
    q[3] = cj*cc + sj*ss

    返回 q


类 框架发布者(节点):

    捍卫 启动(自我):
        棒极了().启动('turtle_tf2_frame_publisher'；)

        # 声明并获取 `turtlename` 参数
        自我.海龟名 = 自我.declare_parameter(
          'turtlename'；, '乌龟'；).获取参数值().string_value

        # 初始化转换广播器
        自我.tf_broadcaster = 改造播音员(自我)

        # 订阅海龟{1}{2}/姿势主题并调用 handle_turtle_pose
        # 每条信息的回调函数
        自我.订阅费 = 自我.创建订阅(
            姿势,
            f'/{自我.海龟名}/pose'；,
            自我.乌龟姿势,
            1)
        自我.订阅费  # 防止未使用的变量发出警告

    捍卫 乌龟姿势(自我, 信息):
        t = TransformStamped()

        # 读取信息内容并将其分配给
        # 相应的 tf 变量
        t.页眉.盖章 = 自我.获取时钟().现在().to_msg()
        t.页眉.frame_id = '世界'；
        t.子帧标识 = 自我.海龟名

        # 海龟只存在于二维空间，因此我们可以得到 x 和 y 的平移值
        # 从信息中获取坐标，并将 Z 坐标设为 0
        t.变.译文.x = 信息.x
        t.变.译文.y = 信息.y
        t.变.译文.z = 0.0

        # 出于同样的原因，乌龟只能绕一条轴旋转
        # 因此，我们将 x 和 y 轴的旋转设为 0，并得到
        # 从信息中以 Z 轴为单位旋转
        q = 来自欧拉的四元数(0, 0, 信息.θ)
        t.变.自转.x = q[0]
        t.变.自转.y = q[1]
        t.变.自转.z = q[2]
        t.变.自转.w = q[3]

        # 发送转换
        自我.tf_broadcaster.发送变换(t)


捍卫 主要():
    rclpy.启动()
    网站 = 框架发布者()
    尝试:
        rclpy.后旋(网站)
    除开 键盘中断:
        通过

    rclpy.关闭()

1.1 检查代码

现在，让我们看看将乌龟姿势发布到 tf2 的相关代码。首先，我们定义并获取一个参数 海龟名，其中指定了一个乌龟名称，例如 乌龟1 或 乌龟2.

自我.海龟名 = 自我.declare_parameter(
  'turtlename'；, '乌龟'；).获取参数值().string_value

之后，节点会订阅主题 {self.turtlename}/pose 并运行函数 乌龟姿势 对每一条收到的信息进行处理。

自我 .订阅费 = 自我.创建订阅(
    姿势,
    f'/{自我.海龟名}/pose'；,
    自我.乌龟姿势,
    1)

现在，我们创建一个 TransformStamped 对象，并赋予其相应的元数据。

  1. 我们需要给正在发布的变换加上一个时间戳，然后通过调用 self.get_clock().now().这将返回 节点.

  2. 然后，我们需要设置所创建链接的父框架名称，本例中为 世界.

  3. 最后，我们需要设置所创建链接的子节点名称，在本例中就是乌龟本身的名称。

乌龟姿态信息的处理函数会广播乌龟的平移和旋转，并将其作为从帧 世界 框 乌龟X.

t = TransformStamped()

# 读取信息内容并将其分配给
# 相应的 tf 变量
t.页眉.盖章 = 自我.获取时钟().现在().to_msg()
t.页眉.frame_id = '世界'；
t.子帧标识 = 自我.海龟名

在这里，我们将三维乌龟姿势的信息复制到三维变换中。

# 海龟只存在于二维空间，因此我们可以得到 x 和 y 的平移值
# 从信息中获取坐标，并将 Z 坐标设为 0
t.变.译文.x = 信息.x
t.变.译文.y = 信息.y
t.变.译文.z = 0.0

# 出于同样的原因，乌龟只能绕一条轴旋转
# 因此，我们将 x 和 y 轴的旋转设为 0，并得到
# 从信息中以 Z 轴为单位旋转
q = 来自欧拉的四元数(0, 0, 信息.θ)
t.变.自转.x = q[0]
t.变.自转.y = q[1]
t.变.自转.z = q[2]
t.变.自转.w = q[3]

最后，我们将构建的变换传递给 发送变换 的方法 改造播音员 它将负责广播。

# 发送转换
自我.tf_broadcaster.发送变换(t)

1.2 添加入口

让 玫瑰2 运行 命令来运行您的节点，您必须将入口点添加到 setup.py 位于 src/learning_tf2_py 目录）。

在 控制台脚本 括号：

'turtle_tf2_broadcaster = learning_tf2_py.turtle_tf2_broadcaster:main'；,

2 编写启动文件

现在为该演示创建一个启动文件。创建一个 启动 文件夹中的 src/learning_tf2_py 目录。用文本编辑器新建一个名为 turtle_tf2_demo_launch.py 在 启动 文件夹，并添加以下几行：

从 启动 舶来品 启动说明
从 launch_ros.actions 舶来品 节点


捍卫 生成发射描述():
    返回 启动说明([
        节点(
            包装='turtlesim'；,
            可执行='turtlesim_node',
            名字='sim'；
        ),
        节点(
            包装='learning_tf2_py',
            可执行='turtle_tf2_broadcaster'；,
            名字='broadcaster1',
            参数=[
                {'turtlename'；: 'turtle1'；}
            ]
        ),
    ])

2.1 检查代码

首先，我们从 启动 和 发射 包。需要注意的是 启动 是一个通用的启动框架（非 ROS 2 专用），而 发射 具有 ROS 2 的特定功能，比如我们在这里导入的节点。

从 启动 舶来品 启动说明
从 launch_ros.actions 舶来品 节点

现在，我们运行节点，启动海龟模拟并广播 乌龟1 使用我们的 乌龟_TF2_播音员 节点。

节点(
    包装='turtlesim'；,
    可执行='turtlesim_node',
    名字='sim'；
),
节点(
    包装='learning_tf2_py',
    可执行='turtle_tf2_broadcaster'；,
    名字='broadcaster1',
    参数=[
        {'turtlename'；: 'turtle1'；}
    ]
),

2.2 添加依赖项

导航一级回到 学习_tf2_py 目录中的 setup.py, setup.cfg和 package.xml 文件的位置。

开放 package.xml 文本编辑器。在启动文件的导入语句中添加以下依赖项：

执行依赖关系；启动</exec_depend>；
执行依赖关系；发射</exec_depend>；

这声明了额外需要的 启动 和 发射 执行其代码时的依赖关系。

确保保存文件。

2.3 更新 setup.py

重新开放 setup.py 中的启动文件。 发射/ 文件夹将被安装。安装 数据文件 字段现在应该是这样的：

数据文件=[
    ...
    (os.路.加入('分享'；, 包名, 'launch'；), 水珠(os.路.加入('launch'；, '*launch.[pxy][yma]*'))),
],

同时在文件顶部添加相应的导入：

舶来品 os
从 水珠 舶来品 水珠

有关创建启动文件的更多信息，请参阅 本教程.

3 建

运行 rosdep 在工作区的根目录下，检查是否存在缺失的依赖项。

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

还是在工作区的根目录下，构建你的软件包：

colcon build --packages-select learning_tf2_py

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

install/setup.bash

4 运行

现在运行启动文件，该文件将启动 turtlesim 仿真节点并 乌龟_TF2_播音员 节点：

ros2 launch learning_tf2_py turtle_tf2_demo_launch.py

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

ros2 run turtlesim turtle_teleop_key

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

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

现在，使用 tf2_echo 工具来检查乌龟姿势是否真的被广播到了 tf2：

ros2 run tf2_ros tf2_echo world turtle1

这会显示第一只乌龟的姿势。使用方向键绕着乌龟行驶（确保你的 乌龟遥控钥匙 终端窗口处于活动状态，而不是模拟器窗口）。在控制台输出中，您将看到类似下面的内容：

时间 1714913843.708748879
- 翻译： [4.541, 3.889, 0.000][4.541, 3.889, 0.000]
- 旋转：以四元数 [0.000, 0.000, 0.999, -0.035] 表示
- 旋转：单位 RPY（弧度） [0.000，-0.000，-3.072］
- 旋转：单位 RPY（度） [0.000, -0.000, -176.013] （0.000，-0.000，-176.013
- 矩阵
 -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 之间的变换 世界 和 乌龟2，你应该看不到变换，因为第二只乌龟还没有出现。不过，一旦我们在下一个教程中添加了第二只乌龟，它的姿势就会发生变化。 乌龟2 将向 tf2 播放。

摘要

在本教程中，您将学习如何向 tf2 广播机器人的姿势（乌龟的位置和方向），以及如何使用 tf2_echo 工具。要实际使用广播到 tf2 的变换，应继续下一个教程，创建一个 tf2 监听器.