【ROS2】中级:tf2-编写广播器（Python）

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

教程级别：中级

 时间：15 分钟

 目录

•  背景

•  先决条件

•  任务

  • 1. 编写广播器节点

  • 2. 编写启动文件

  •  3. 构建

  •  4.运行

•  摘要

 背景

在接下来的两个教程中，我们将编写代码来复现“tf2 入门”教程中的演示。之后，后续教程将专注于使用更高级的 tf2 功能来扩展演示，包括在变换查找和时间旅行中使用超时。

 先决条件

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

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

 任务

1. 编写广播器节点

让我们先创建源文件。转到我们在上一教程中创建的 learning_tf2_py 包。在 src/learning_tf2_py/learning_tf2_py 目录中，通过输入以下命令下载示例广播代码：

cxy@ubuntu2404-cxy:~$ cd ~/ros2_ws/src/learning_tf2_py/learning_tf2_py
cxy@ubuntu2404-cxy:~/ros2_ws/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 的文件。

# 版权 2021 开源机器人基金会
#
# 根据 Apache 许可证 2.0 版（“许可证”）授权；
# 除非符合许可证，否则您不得使用此文件。
# 您可以在以下网址获取许可证副本：
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# 除非适用法律要求或书面同意，否则根据许可证分发的软件
# 是按“原样”分发的，不附带任何明示或暗示的担保或条件。
# 请参阅许可证以了解管理权限和限制的特定语言。


import math  # 导入数学模块


from geometry_msgs.msg import TransformStamped  # 从geometry_msgs模块导入TransformStamped消息类型


import numpy as np  # 导入numpy模块并简写为np


import rclpy  # 导入rclpy模块
from rclpy.node import Node  # 从rclpy.node模块导入Node类


from tf2_ros import TransformBroadcaster  # 从tf2_ros模块导入TransformBroadcaster类


from turtlesim.msg import Pose  # 从turtlesim.msg模块导入Pose消息类型




# 这个函数是简化版的代码，来自
# https://github.com/matthew-brett/transforms3d/blob/f185e866ecccb66c545559bc9f2e19cb5025e0ab/transforms3d/euler.py
# 除了简化之外，这个版本还颠倒了顺序以返回x,y,z,w，这是ROS所偏好的方式。
def quaternion_from_euler(ai, aj, ak):
    ai /= 2.0  # 将ai除以2.0
    aj /= 2.0  # 将aj除以2.0
    ak /= 2.0  # 将ak除以2.0
    ci = math.cos(ai)  # 计算ai的余弦值
    si = math.sin(ai)  # 计算ai的正弦值
    cj = math.cos(aj)  # 计算aj的余弦值
    sj = math.sin(aj)  # 计算aj的正弦值
    ck = math.cos(ak)  # 计算ak的余弦值
    sk = math.sin(ak)  # 计算ak的正弦值
    cc = ci*ck  # 计算ci与ck的乘积
    cs = ci*sk  # 计算ci与sk的乘积
    sc = si*ck  # 计算si与ck的乘积
    ss = si*sk  # 计算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):  # 定义FramePublisher类，继承自Node类


    def __init__(self):
        super().__init__('turtle_tf2_frame_publisher')  # 调用父类的构造函数，初始化节点名称为'turtle_tf2_frame_publisher'


        # 声明并获取`turtlename`参数
        self.turtlename = self.declare_parameter(
            'turtlename', 'turtle').get_parameter_value().string_value


        # 初始化变换广播器
        self.tf_broadcaster = TransformBroadcaster(self)


        # 订阅turtle{1}{2}/pose主题，并在每个消息上调用handle_turtle_pose回调函数
        self.subscription = self.create_subscri