编写tf2静态广播器 (c ++) [待校准@8697]

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_cpp 的包将取决于 rclcpp 、 tf2 、 tf2_ros 、 geometry_msgs 和 turtlesim 。本教程的代码存储 here 。 [待校准@8703]

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

ros2 pkg create --build-type ament_cmake learning_tf2_cpp

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

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

让我们先创建源文件。在 src/learning_tf2_cpp/src 目录中，通过输入以下命令下载示例静态广播器代码: [待校准@8707]

LinuxmacOSWindows

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

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

#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>

#include <memory>

using std::placeholders::_1;

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

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

private:
  void make_transforms(char * transformation[])
  {
    rclcpp::Time now = this->get_clock()->now();
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = 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_publisher_->sendTransform(t);
  }
  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_publisher_;
};

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检查代码 [待校准@8007]

现在，让我们看一下与将静态乌龟姿势发布到tf2相关的代码。第一行包括所需的头文件。首先，我们包括 geometry_msgs/msg/transform_stamped.hpp 来访问 TransformStamped 信息类型，我们将把它发布到转换树中。 [待校准@8708]

#include <geometry_msgs/msg/transform_stamped.hpp>

之后，包括 rclcpp ，因此可以使用其 “rclcpp:: node” 类。 [待校准@8709]

#include <rclcpp/rclcpp.hpp>

[需手动修复的语法]``tf2::Quaternion`` is a class for a quaternion that provides convenient functions for converting Euler angles to quaternions and vice versa. We also include tf2_ros/static_transform_broadcaster.h to use the StaticTransformBroadcaster to使静态转换的发布变得容易。 [待校准@8710]

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>

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

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

this->make_transforms(transformation);

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

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

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

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

rclcpp::Time now = this->get_clock()->now();
geometry_msgs::msg::TransformStamped t;

t.header.stamp = now;
t.header.frame_id = "world";
t.child_frame_id = transformation[1];

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

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() 函数广播静态变换。 [待校准@8717]

tf_publisher_->sendTransform(t);

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

导航到 src/learning_tf2_cpp 目录，在那里已经为您创建了 CMakeLists.txt 和 package.xml 文件。 [待校准@8718]

用文本编辑器打开 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 rclcpp</description>
<maintainer email="you@email.com">Your Name</maintainer>
<license>Apache License 2.0</license>

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

<depend>geometry_msgs</depend>
<depend>rclcpp</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>turtlesim</depend>

在构建和执行其代码时，这声明了所需的 geometry_msgs 、 rclcpp 、 tf2 、 tf2_ros 和 turtlesim 依赖关系。 [待校准@8722]

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

2.3 CMakeLists.txt [待校准@8648]

现在打开CMakeLists.txt文件。在现有的依存性 find_package(ament_cmake REQUIRED) 下面，添加以下几行: [待校准@8723]

find_package(geometry_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(turtlesim REQUIRED)

之后，添加可执行文件并将其命名为 static_turtle_tf2_broadcaster ，稍后将在 ros2 run 中使用。 [待校准@8724]

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
   turtlesim
)

最后，添加 “安装 (目标……) `` section so `` ros2运行” 可以找到您的可执行文件: [待校准@8640]

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

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

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

LinuxmacOSWindows

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

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

LinuxmacOSWindows

colcon build --packages-select learning_tf2_cpp

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

LinuxmacOSWindows

. install/setup.bash

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

ros2 run learning_tf2_cpp 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]