编写tf2静态广播器 (Python) [待校准@8739]

Goal: Learn how to broadcast static coordinate frames to tf2.

Tutorial教程级别: Intermediate中级 [待校准@6713]

时间: 15分钟 [Alyssa@6755]

背景

发布静态变换对于定义机器人基座与其传感器或非移动部件之间的关系很有用。例如，最容易推理激光扫描仪中心帧中的激光扫描测量。 [待校准@8699]

这是一个独立的教程，涵盖了静态转换的基础知识，由两部分组成。在第一部分中，我们将编写代码以将静态转换发布到tf2。第二部分我们将解释如何使用命令行 static_transform_publisher 可执行工具 tf2_ros 。 [待校准@8700]

在接下来的两个教程中，我们将编写代码来重现 Introduction to tf2 教程中的例程。之后，以下教程将重点介绍使用更高级的tf2功能扩展例程。 [待校准@8701]

先决条件

在之前的教程中，你学习了如何 create a workspace and create a package 。 [待校准@8702]

任务

1创建包

首先，我们将创建一个包，用于本教程和以下教程。调用 learning_tf2_py 的包将取决于 rclpy 、 tf2_ros 、 geometry_msgs 和 turtlesim 。本教程的代码存储为 here 。 [待校准@8740]

打开一个新的终端和 source your ROS 2 installation ，这样 ros2 命令就可以工作了。导航到工作区的 src 文件夹并创建一个新包: [待校准@8704]

ros2 pkg create --build-type ament_python learning_tf2_py

您的终端将返回一条消息，验证您的包 learning_tf2_py 及其所有必要的文件和文件夹的创建。 [待校准@8741]

2写入静态广播器节点 [待校准@8706]

让我们先创建源文件。内 src/learning_tf2_py/learning_tf2_py 目录下载示例静态广播器代码输入以下命令: [待校准@8742]

LinuxmacOSWindows

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

使用您喜欢的文本编辑器打开文件。 [待校准@8591]

import sys

from geometry_msgs.msg import TransformStamped

import rclpy
from rclpy.node import Node

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

import tf_transformations


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_publisher = StaticTransformBroadcaster(self)

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

   def make_transforms(self, transformation):
      static_transformStamped = TransformStamped()
      static_transformStamped.header.stamp = self.get_clock().now().to_msg()
      static_transformStamped.header.frame_id = 'world'
      static_transformStamped.child_frame_id = sys.argv[1]
      static_transformStamped.transform.translation.x = float(sys.argv[2])
      static_transformStamped.transform.translation.y = float(sys.argv[3])
      static_transformStamped.transform.translation.z = float(sys.argv[4])
      quat = tf_transformations.quaternion_from_euler(
            float(sys.argv[5]), float(sys.argv[6]), float(sys.argv[7]))
      static_transformStamped.transform.rotation.x = quat[0]
      static_transformStamped.transform.rotation.y = quat[1]
      static_transformStamped.transform.rotation.z = quat[2]
      static_transformStamped.transform.rotation.w = quat[3]

      self._tf_publisher.sendTransform(static_transformStamped)


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(0)
   else:
      if sys.argv[1] == 'world':
            logger.info('Your static turtle name cannot be "world"')
            sys.exit(0)

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

   rclpy.shutdown()

2.1检查代码 [待校准@8007]

现在，让我们看一下与将静态乌龟姿势发布到tf2相关的代码。第一行导入所需包。首先，我们从 geometry_msgs 导入 TransformStamped ，它为我们将发布到转换树的消息提供了一个模板。 [待校准@8743]

from geometry_msgs.msg import TransformStamped

之后， rclpy 是进口的，因此可以使用 Node 。 [待校准@8744]

import rclpy
from rclpy.node import Node

[需手动修复的语法] tf2_ros 包提供了一个 StaticTransformBroadcaster ，使静态转换的发布变得容易。使用 StaticTransformBroadcaster ，我们需要进口从[需手动修复的语法] tf2_ros 模块。 tf_transformations 提供了将欧拉角转换为四元数的函数，反之亦然。 [待校准@8745]

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

import tf_transformations

[需手动修复的语法] StaticFramePublisher 类构造函数初始化名称为 static_turtle_tf2_broadcaster 的节点。然后， StaticTransformBroadcaster 被创建，它将在启动时发送一个静态转换。 [待校准@8711]

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

在这里，我们创建一个 TransformStamped 对象，这将是我们在填充后将发送的消息。在传递实际转换值之前，我们需要为其提供适当的元数据。 [待校准@8712]

1. 我们需要给正在发布的转换一个时间戳，我们只需用当前时间标记它， self.get_clock().now() [待校准@8746]

2. 然后我们需要设置我们正在创建的链接的父帧的名称，在这个例子中是 world [待校准@8714]

3. 最后，我们需要设置正在创建的链接的子帧的名称 [待校准@8715]

static_transformStamped = TransformStamped()
static_transformStamped.header.stamp = self.get_clock().now().to_msg()
static_transformStamped.header.frame_id = 'world'
static_transformStamped.child_frame_id = sys.argv[1]

这里我们填充乌龟的6D姿势 (平移和旋转)。 [待校准@8716]

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

最后，我们使用 sendTransform() 函数广播静态变换。 [待校准@8717]

self._tf_publisher.sendTransform(static_transformStamped)

2.2添加依赖项 [待校准@8644]

导航到 src/learning_tf2_py 目录，在那里已经为您创建了 setup.py 、 setup.cfg 和 package.xml 文件。 [待校准@8747]

用文本编辑器打开 package.xml 。 [待校准@8719]

如 Creating your first ROS 2 package tutorial, make sure to fill in the <description>, <maintainer> and ``<license>` 标签中所述: [待校准@8720]

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

在上面的行之后，添加与节点的导入语句相对应的以下依赖项: [待校准@8748]

<exec_depend>geometry_msgs</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>tf_transformations</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>turtlesim</exec_depend>

当执行其代码时，这声明了所需的 geometry_msgs 、 tf_transformations 、 rclpy 、 tf2_ros 和 turtlesim 依赖关系。 [待校准@8749]

确保保存文件。 [待校准@8647]

2.3添加入口点 [待校准@8750]

要允许 ros2 run 命令运行您的节点，您必须将入口点添加到 setup.py (位于 src/learning_tf2_py 目录中)。 [待校准@8666]

最后，在 “'console_scripts':” 括号中添加以下一行: [待校准@8667]

'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main',

3构建和运行 [待校准@8033]

在构建之前，最好在工作区的根目录下运行 rosdep ，以检查是否缺少依赖项: [待校准@8725]

LinuxmacOSWindows

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

仍在工作区的根目录下，构建新包: [待校准@8727]

LinuxmacOSWindows

colcon build --packages-select learning_tf2_py

打开一个新终端，导航到工作区的根目录，然后源文件安装文件: [待校准@8728]

LinuxmacOSWindows

. install/setup.bash

现在运行 static_turtle_tf2_broadcaster 节点: [待校准@8729]

ros2 run learning_tf2_py static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

这为 mystaticturtle 1米漂浮在地面上设置了一个海龟姿势广播。 [待校准@8730]

我们现在可以通过呼应 tf_static 话题来检查静态变换是否已经发表 [待校准@8731]

ros2 topic echo --qos-reliability reliable --qos-durability transient_local /tf_static

如果一切顺利，你应该看到一个单一的静态变换 [待校准@8732]

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

发布静态转换的正确方法 [待校准@8733]

本教程旨在展示如何使用 StaticTransformBroadcaster 发布静态变换。在你真正开发版本过程你不该写这个代码应该使用专用 tf2_ros 工具。 tf2_ros 提供了一个名为 static_transform_publisher 的可执行文件，它既可以用作命令行工具，也可以用作可以添加到launch文件中的节点。 [待校准@8734]

使用以米为单位的x/y/z偏移和以弧度为单位的滚动/俯仰/偏航，发布到tf2的静态坐标变换。在我们的案例中，滚动/俯仰/偏航分别指围绕x/y/z轴的旋转。 [待校准@8735]

ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll frame_id child_frame_id

使用以米和四元数为单位的x/y/z偏移将静态坐标变换发布到tf2。 [待校准@8736]

ros2 run tf2_ros static_transform_publisher x y z qx qy qz qw frame_id child_frame_id

用于设置静态变换的``static_transform_publisher`` is designed both as a command-line tool for manual use, as well as for use within launch files。例如: [待校准@8737]

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 = ['0', '0', '1', '0', '0', '0', 'world', 'mystaticturtle']
      ),
   ])

总结

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