编写静态广播器（C++）

**目标：**学习如何向 tf2 广播静态坐标框架。

教程级别： 中级

时间： 15分钟

内容

背景
先决条件
任务
发布静态变换的正确方法。
总结

背景 

发布静态转换对于定义机器人底盘与其传感器或静止部件之间的关系非常有用。例如，对于在激光扫描仪中心的坐标框架中的激光扫描测量，最容易进行推理。

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

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

任务 

1 创建一个包 

首先，我们将创建一个用于本教程和后续教程的包。该包名为 learning_tf2_cpp，它将依赖于 geometry_msgs、rclcpp、tf2、tf2_ros 和 turtlesim。本教程的代码存储在此处。

打开一个新的终端，并 source ROS 2 安装目录，以使 ros2 命令可用。导航到工作空间的 src 文件夹并创建一个新的包：

ros2 pkg create --build-type ament_cmake --dependencies geometry_msgs rclcpp tf2 tf2_ros turtlesim -- learning_tf2_cpp

终端将返回一条消息，确认已成功创建包 learning_tf2_cpp 及其所需的所有文件和文件夹。

2 编写静态广播节点 

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

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

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

在 Windows 命令行提示符中：

curl -sk https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/static_turtle_tf2_broadcaster.cpp -o static_turtle_tf2_broadcaster.cpp

或者在powershell中执行：

curl https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/static_turtle_tf2_broadcaster.cpp -o 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 添加依赖项

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

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

如在:doc:创建包教程 <../../Beginner-Client-Libraries/Creating-Your-First-ROS2-Package>`中所述，请确保填写``<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``以检查构建前是否缺少依赖项是一个好习惯：

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

rosdep 只能在 Linux 上运行，所以你需要自己安装 geometry_msgs 和 turtlesim 依赖项。

在工作区根目录中构建您的新包：

colcon build --packages-select learning_tf2_cpp

colcon build --packages-select learning_tf2_cpp

colcon build --merge-install --packages-select learning_tf2_cpp

打开一个新终端，进入你的工作空间的根目录，并源化设置文件：

. install/setup.bash

. install/setup.bash

# CMD
call install\setup.bat

# Powershell
.\install\setup.ps1

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偏移量和以弧度为单位的roll/pitch/yaw来将静态坐标变换发布到tf2。在我们的例子中，roll/pitch/yaw分别指的是绕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 可执行文件和 launch 文件来发布所需的静态变换。