【ROS2】中级:tf2-编写静态广播器(Python)

最新推荐文章于 2024-07-12 14:39:22 发布

十年一梦实验室

最新推荐文章于 2024-07-12 14:39:22 发布

阅读量688

点赞数 26

文章标签： python 机器人开发语言

本文链接：https://blog.csdn.net/cxyhjl/article/details/140367694

版权

目标：学习如何向 tf2 广播静态坐标系。

教程级别：中级

时间：15 分钟

背景
先决条件
任务
- 1. 创建一个包
- 2. 编写静态广播节点
- 3. 构建
- 4. 运行
发布静态变换的正确方法
摘要

背景

发布静态变换对于定义机器人基座与其传感器或非移动部件之间的关系非常有用。例如，在激光扫描仪中心的框架内推理激光扫描测量数据是最简单的。

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

在接下来的两个教程中，我们将编写代码以复现《tf2 入门》教程中的演示 https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Tf2/Introduction-To-Tf2.html 。之后，后续教程将专注于使用更高级的 tf2 功能扩展演示。

先决条件

在之前的教程中，您学习了如何创建工作区 https://docs.ros.org/en/jazzy/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html 和创建包 https://docs.ros.org/en/jazzy/Tutorials/Beginner-Client-Libraries/Creating-Your-First-ROS2-Package.html 。

任务

1. 创建一个包

首先，我们将创建一个包，用于本教程及后续教程。这个名为 learning_tf2_py 的包将依赖于 geometry_msgs 、 python3-numpy 、 rclpy 、 tf2_ros_py 和 turtlesim 。本教程的代码存储在这里 https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/static_turtle_tf2_broadcaster.py 。

打开一个新的终端并且配置你的 ROS 2 安装 https://docs.ros.org/en/jazzy/Tutorials/Beginner-CLI-Tools/Configuring-ROS2-Environment.html ，以便 ros2 命令能够工作。导航到工作空间的 src 文件夹并创建一个新的包：

ros2 pkg create --build-type ament_python --license Apache-2.0 -- learning_tf2_py

cxy@ubuntu2404-cxy:~/ros2_ws/src$ ros2 pkg create --build-type ament_python --license Apache-2.0 -- learning_tf2_py
going to create a new package
package name: learning_tf2_py
destination directory: /home/cxy/ros2_ws/src
package format: 3
version: 0.0.0
description: TODO: Package description
maintainer: ['cxy <cxy@todo.todo>']
licenses: ['Apache-2.0']
build type: ament_python
dependencies: []
creating folder ./learning_tf2_py
creating ./learning_tf2_py/package.xml
creating source folder
creating folder ./learning_tf2_py/learning_tf2_py
creating ./learning_tf2_py/setup.py
creating ./learning_tf2_py/setup.cfg
creating folder ./learning_tf2_py/resource
creating ./learning_tf2_py/resource/learning_tf2_py
creating ./learning_tf2_py/learning_tf2_py/__init__.py
creating folder ./learning_tf2_py/test
creating ./learning_tf2_py/test/test_copyright.py
creating ./learning_tf2_py/test/test_flake8.py
creating ./learning_tf2_py/test/test_pep257.py

您的终端将返回一条消息，确认您的包 learning_tf2_py 及其所有必要的文件和文件夹已创建。

2. 编写静态广播器节点

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

cxy@ubuntu2404-cxy:~/ros2_ws$ cd 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/static_turtle_tf2_broadcaster.py
--2024-07-10 22:44:22--  https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/static_turtle_tf2_broadcaster.py
正在连接 127.0.0.1:2334... 已连接。
已发出 Proxy 请求，正在等待回应... 200 OK
长度：3480 (3.4K) [text/plain]
正在保存至: ‘static_turtle_tf2_broadcaster.py’


static_turtle_tf2_b 100%[===================>]   3.40K  --.-KB/s    用时 0s    


2024-07-10 22:44:26 (16.9 MB/s) - 已保存 ‘static_turtle_tf2_broadcaster.py’ [3480/3480])

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

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


import math  # 导入数学库
import sys  # 导入系统库


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


import numpy as np  # 导入numpy库，用于数值计算


import rclpy  # 导入ROS客户端库
from rclpy.node import Node  # 从rclpy.node导入Node类


from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster  # 从tf2_ros.static_transform_broadcaster导入StaticTransformBroadcaster类




# 这个函数是以下代码的精简版本
# 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
    aj /= 2.0  # aj除以2
    ak /= 2.0  # ak除以2
    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, ))  # 创建一个空的numpy数组q，长度为4
    q[0] = cj*sc - sj*cs  # 计算q[0]
    q[1] = cj*ss + sj*cc  # 计算q[1]
    q[2] = cj*cs - sj*sc  # 计算q[2]
    q[3] = cj*cc + sj*ss  # 计算q[3]


    return q  # 返回四元数q




class StaticFramePublisher(Node):  # 定义StaticFramePublisher类，继承自Node类
    """
    广播永不改变的变换。


    这个例子从`world`发布到一个静态的turtle frame的变换。
    这些变换只在启动时发布一次，并且对所有
    时间都是常数。
    """


    def __init__(self, transformation):  # 定义构造函数，接收一个参数transformation
        super().__init__('static_turtle_tf2_broadcaster')  # 调用父类的构造函数，设置节点名为'static_turtle_tf2_broadcaster'


        self.tf_static_broadcaster = StaticTransformBroadcaster(self)  # 创建一个StaticTransformBroadcaster对象


        # 在启动时发布一次静态变换
        self.make_transforms(transformation)  # 调用make_transforms方法，传入参数transformation


    def make_transforms(self, transformation):  # 定义make_transforms方法，接收一个参数transformation
        t = TransformStamped()  # 创建一个TransformStamped对象t


        t.header.stamp = self.get_clock().now().to_msg()  # 设置t的时间戳为当前时间
        t.header.frame_id = 'world'  # 设置t的frame_id为'world'
        t.child_frame_id = transformation[1]  # 设置t的child_frame_id为transformation的第二个元素


        t.transform.translation.x = float(transformation[2])  # 设置t的translation.x为transformation的第三个元素
        t.transform.translation.y = float(transformation[3])  # 设置t的translation.y为transformation的第四个元素
        t.transform.translation.z = float(transformation[4])  # 设置t的translation.z为transformation的第五个元素
        quat = quaternion_from_euler(
            float(transformation[5]), float(transformation[6]), float(transformation[7]))  # 调用quaternion_from_euler函数，传入transformation的第六、七、八个元素，得到四元数quat
        t.transform.rotation.x = quat[0]  # 设置t的rotation.x为quat的第一个元素
        t.transform.rotation.y = quat[1]  # 设置t的rotation.y为quat的第二个元素
        t.transform.rotation.z = quat[2]  # 设置t的rotation.z为quat的第三个元素
        t.transform.rotation.w = quat[3]  # 设置t的rotation.w为quat的第四个元素


        self.tf_static_broadcaster.sendTransform(t)  # 调用tf_static_broadcaster的sendTransform方法，传入t




def main():  # 定义主函数
    logger = rclpy.logging.get_logger('logger')  # 获取一个名为'logger'的日志记录器


    # 从命令行参数获取参数
    if len(sys.argv) != 8:  # 如果命令行参数的数量不等于8
        logger.info('参数数量无效。使用方法：\n'
                    '$ ros2 run turtle_tf2_py static_turtle_tf2_broadcaster'
                    'child_frame_name x y z roll pitch yaw')  # 输出错误信息
        sys.exit(1)  # 退出程序


    if sys.argv[1] == 'world':  # 如果命令行参数的第二个元素等于'world'
        logger.info('您的静态turtle名称不能为"world"')  # 输出错误信息
        sys.exit(2)  # 退出程序


    # 传递参数并初始化节点
    rclpy.init()  # 初始化ROS客户端库
    node = StaticFramePublisher(sys.argv)  # 创建一个StaticFramePublisher对象，传入命令行参数
    try:
        rclpy.spin(node)  # 进入事件循环，等待回调函数的调用
    except KeyboardInterrupt:  # 如果收到键盘中断信号
        pass  # 不做任何操作


    rclpy.shutdown()  # 关闭ROS客户端库

检查代码 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 对象，这将是我们一旦填充后将发送的消息。在传递实际的变换值之前，我们需要为其提供适当的元数据。

我们需要为正在发布的变换提供一个时间戳，我们将用当前时间来标记它， self.get_clock().now()
然后我们需要设置我们正在创建的 link 的父框架的名称，在这种情况下是 world
最后，我们需要设置我们正在创建的 link 的子框架的名称

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 更新 package.xml

返回到 src/learning_tf2_py 目录， setup.py 、 setup.cfg 和 package.xml 文件已为您创建。

使用您的文本编辑器打开 package.xml 。

在创建包教程中提到的，确保填写 <description> 、 <maintainer> 和 <license> 标签：

<description>Learning tf2 with rclpy</description>
  <maintainer email="cxy@126.com">cxy</maintainer>
  <license>Apache-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 依赖项。

确保保存文件。

<?xml version="1.0"?> <!-- XML版本为1.0 -->
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <!-- 定义XML模型的位置和模式类型 -->
<package format="3"> <!-- 定义包的格式版本为3 -->
  <name>learning_tf2_py</name> <!-- 包的名称为learning_tf2_py -->
  <version>0.0.0</version> <!-- 包的版本为0.0.0 -->
  <description>Learning tf2 with rclpy</description> <!-- 包的描述为Learning tf2 with rclpy -->
  <maintainer email="cxy@126.com">cxy</maintainer> <!-- 包的维护者的名字和邮箱 -->
  <license>Apache-2.0</license> <!-- 包的许可证为Apache-2.0 -->


  <exec_depend>geometry_msgs</exec_depend> <!-- 运行时依赖geometry_msgs -->
  <exec_depend>python3-numpy</exec_depend> <!-- 运行时依赖python3-numpy -->
  <exec_depend>rclpy</exec_depend> <!-- 运行时依赖rclpy -->
  <exec_depend>tf2_ros_py</exec_depend> <!-- 运行时依赖tf2_ros_py -->
  <exec_depend>turtlesim</exec_depend> <!-- 运行时依赖turtlesim -->
  
  <test_depend>ament_copyright</test_depend> <!-- 测试依赖ament_copyright -->
  <test_depend>ament_flake8</test_depend> <!-- 测试依赖ament_flake8 -->
  <test_depend>ament_pep257</test_depend> <!-- 测试依赖ament_pep257 -->
  <test_depend>python3-pytest</test_depend> <!-- 测试依赖python3-pytest -->


  <export>
    <build_type>ament_python</build_type> <!-- 构建类型为ament_python -->
  </export>
</package> <!-- 结束包的定义 -->

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 以检查构建前缺失的依赖项是一个好习惯：

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

仍在您的工作区根目录下，构建您的新包：

colcon build --packages-select learning_tf2_py

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

. 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 主题来检查静态变换是否已经发布

如果一切顺利，您应该会看到一个单一的静态变换

cxy@ubuntu2404-cxy:~$ ros2 topic echo /tf_static
transforms:
- header:
    stamp:
      sec: 1720666290
      nanosec: 451592896
    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 偏移量以及滚动/俯仰/偏航发布到 tf2 的静态坐标变换。在 ROS 2 中，滚动/俯仰/偏航分别指的是绕 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 文件中使用，用于设置静态变换。例如：

# 导入LaunchDescription模块，用于描述启动配置
from launch import LaunchDescription
# 导入Node模块，用于描述ROS节点
from launch_ros.actions import Node


# 定义一个函数，用于生成启动描述
def generate_launch_description():
    # 返回一个启动描述，其中包含一个节点
    return LaunchDescription([
        # 定义一个节点
        Node(
            # 指定节点所在的包名为'tf2_ros'
            package='tf2_ros',
            # 指定节点的可执行文件名为'static_transform_publisher'
            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 外的所有参数都是可选的；如果没有指定某个选项，则将假定为identity 。

摘要

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

https://threejs.org/docs/#api/zh/math/Euler

https://en.wikipedia.org/wiki/Rotation_matrix

内旋：绕动轴旋转，按照顺序(xyz)左乘矩阵

外旋：绕定轴旋转，按照顺序(zyx)右乘矩阵

十年一梦实验室

关注

26
点赞
踩
21

收藏

觉得还不错? 一键收藏
0
评论
【ROS2】中级:tf2-编写静态广播器(Python)

目标：学习如何向 tf2 广播静态坐标系。教程级别：中级时间：15 分钟目录背景先决条件任务1. 创建一个包2. 编写静态广播节点3. 构建4. 运行发布静态变换的正确方法摘要背景发布静态变换对于定义机器人基座与其传感器或非移动部件之间的关系非常有用。例如，在激光扫描仪中心的框架内推理激光扫描测量数据是最简单的。这是一个独立的教程，涵盖了静态变换的基础知识，包括两个部分。在第一部...
复制链接

扫一扫