编写静态广播器 (C++)

目标： 了解如何将静态坐标系广播到 tf2。

教程级别： 中级

时间： 15 分钟

背景

发布静态变换对于定义机器人底座与其传感器或非移动部件之间的关系很有用。 例如，在激光扫描仪中心的框架中推断激光扫描测量值是最容易的。

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

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

先决条件

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

任务

1 创建包

首先，我们将创建一个将用于本教程和后续教程的包。 名为“learning_tf2_cpp”的包将依赖于“geometry_msgs”、“rclcpp”、“tf2”、“tf2_ros”和“turtlesim”。 本教程的代码存储在`此处<https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/static_turtle_tf2_broadcaster.cpp>`_。

打开一个新终端并：doc:source your ROS 2 installation，以便``ros2``命令可以正常工作。 导航到工作区的“src”文件夹并创建一个新包：

ros2 pkg create –build-type ament_cmake –license Apache-2.0 –dependencies geometry_msgs rclcpp tf2 tf2_ros turtlesim – learning_tf2_cpp

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

2 编写静态广播器节点

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

LinuxmacOSWindows

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/static_turtle_tf2_broadcaster.cpp

使用您喜欢的文本编辑器打开该文件。

#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"

class StaticFramePublisher : public rclcpp::Node
{
public:
  explicit StaticFramePublisher(char * transformation[])
  : Node("static_turtle_tf2_broadcaster")
  {
    tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

    // Publish static transforms once at startup
    this->make_transforms(transformation);
  }

private:
  void make_transforms(char * transformation[])
  {
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "world";
    t.child_frame_id = transformation[1];

    t.transform.translation.x = atof(transformation[2]);
    t.transform.translation.y = atof(transformation[3]);
    t.transform.translation.z = atof(transformation[4]);
    tf2::Quaternion q;
    q.setRPY(
      atof(transformation[5]),
      atof(transformation[6]),
      atof(transformation[7]));
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    tf_static_broadcaster_->sendTransform(t);
  }

  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
};

int main(int argc, char * argv[])
{
  auto logger = rclcpp::get_logger("logger");

  // Obtain parameters from command line arguments
  if (argc != 8) {
    RCLCPP_INFO(
      logger, "Invalid number of parameters\nusage: "
      "$ ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster "
      "child_frame_name x y z roll pitch yaw");
    return 1;
  }

  // As the parent frame of the transform is `world`, it is
  // necessary to check that the frame name passed is different
  if (strcmp(argv[1], "world") == 0) {
    RCLCPP_INFO(logger, "Your static turtle name cannot be 'world'");
    return 1;
  }

  // Pass parameters and initialize node
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<StaticFramePublisher>(argv));
  rclcpp::shutdown();
  return 0;
}

2.1 检查代码

现在让我们看看与将静态海龟姿势发布到 tf2 相关的代码。 前几行包含所需的头文件。 首先，我们包含“geometry_msgs/msg/transform_stamped.hpp”以访问“TransformStamped”消息类型，我们将把它发布到转换树。

#include "geometry_msgs/msg/transform_stamped.hpp"

之后，包含了“rclcpp”，因此可以使用其“rclcpp::Node”类。

#include "rclcpp/rclcpp.hpp"

“tf2::Quaternion” 是一个四元数类，它提供了将欧拉角转换为四元数和将四元数转换为欧拉角的便捷函数。 我们还包含了“tf2_ros/static_transform_broadcaster.h”，以便使用“StaticTransformBroadcaster”轻松发布静态变换。

#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"

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

tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

this->make_transforms(transformation);

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

#。我们需要为发布的转换提供一个时间戳，我们将用当前时间标记它，this->get_clock()->now()

#。然后我们需要设置我们正在创建的链接的父框架的名称，在本例中为 world

#。最后，我们需要设置我们正在创建的链接的子框架的名称

geometry_msgs::msg::TransformStamped t;

t.header.stamp = this->get_clock()->now();
t.header.frame_id = "world";
t.child_frame_id = transformation[1];

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

t.transform.translation.x = atof(transformation[2]);
t.transform.translation.y = atof(transformation[3]);
t.transform.translation.z = atof(transformation[4]);
tf2::Quaternion q;
q.setRPY(
  atof(transformation[5]),
  atof(transformation[6]),
  atof(transformation[7]));
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();

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

tf_static_broadcaster_->sendTransform(t);

2.2 更新 package.xml

返回上一级到 src/learning_tf2_cpp 目录，其中已为您创建了 CMakeLists.txt 和 package.xml 文件。

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

如 创建包 教程中所述，请确保填写 <description>、<maintainer> 和 <license> 标签：

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

确保保存该文件。

2.3 CMakeLists.txt

将可执行文件添加到 CMakeLists.txt 并将其命名为“static_turtle_tf2_broadcaster”，稍后您将在“ros2 run”中使用它。

add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp)
ament_target_dependencies(
   static_turtle_tf2_broadcaster
   geometry_msgs
   rclcpp
   tf2
   tf2_ros
)

最后，添加“install（TARGETS…）”部分，以便“ros2 run”可以找到您的可执行文件：

install(TARGETS
   static_turtle_tf2_broadcaster
   DESTINATION lib/${PROJECT_NAME})

3 构建

在构建之前，最好在工作区的根目录中运行“rosdep”来检查是否缺少依赖项：

LinuxmacOSWindows

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

仍然在工作区的根目录中，构建新包：

LinuxmacOSWindows

colcon build --packages-select learning_tf2_cpp

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

LinuxmacOSWindows

. install/setup.bash

4 运行

现在运行“static_turtle_tf2_broadcaster”节点：

ros2 run learning_tf2_cpp 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 偏移和以弧度为单位的滚动/俯仰/偏航将静态坐标变换发布到 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”文件中用于设置静态转换。例如：

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”可执行文件和启动文件发布所需的静态变换。