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

Goal目标: Learn了解如何向tf2广播机器人的状态。 [待校准@8622]

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

时间: 15分钟 [Alyssa@6755]

背景

在接下来的两个教程中，我们将编写代码来重现 Introduction to tf2 教程中的例程。之后，以下教程将重点放在使用更高级的tf2功能扩展例程上，包括在转换查找和时间穿梭中使用超时。 [待校准@8623]

先决条件

本教程假设您具有ROS 2的工作知识，并且您已经完成了 Introduction to tf2 tutorial. In previous tutorials, you learned how to create a workspace and create a package. You also have created the learning_tf2_cpp package ，这是我们将继续工作的地方。 [待校准@8624]

任务

1写入广播器节点 [待校准@8625]

让我们先创建源文件。转到我们在上一教程中创建的 learning_tf2_cpp 包。在 src 目录中，通过输入以下命令下载示例广播器代码: [待校准@8626]

LinuxmacOSWindows

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

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

#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <turtlesim/msg/pose.hpp>

#include <memory>
#include <string>

using std::placeholders::_1;

class FramePublisher : public rclcpp::Node
{
public:
  FramePublisher()
  : Node("turtle_tf2_frame_publisher")
  {
    // Declare and acquire `turtlename` parameter
    this->declare_parameter<std::string>("turtlename", "turtle");
    this->get_parameter("turtlename", turtlename_);

    // Initialize the transform broadcaster
    tf_broadcaster_ =
      std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    // Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
    // callback function on each message
    std::ostringstream stream;
    stream << "/" << turtlename_.c_str() << "/pose";
    std::string topic_name = stream.str();

    subscription_ = this->create_subscription<turtlesim::msg::Pose>(
      topic_name, 10,
      std::bind(&FramePublisher::handle_turtle_pose, this, _1));
  }

private:
  void handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg)
  {
    rclcpp::Time now = this->get_clock()->now();
    geometry_msgs::msg::TransformStamped t;

    // Read message content and assign it to
    // corresponding tf variables
    t.header.stamp = now;
    t.header.frame_id = "world";
    t.child_frame_id = turtlename_.c_str();

    // Turtle only exists in 2D, thus we get x and y translation
    // coordinates from the message and set the z coordinate to 0
    t.transform.translation.x = msg->x;
    t.transform.translation.y = msg->y;
    t.transform.translation.z = 0.0;

    // For the same reason, turtle can only rotate around one axis
    // and this why we set rotation in x and y to 0 and obtain
    // rotation in z axis from the message
    tf2::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    // Send the transformation
    tf_broadcaster_->sendTransform(t);
  }
  rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  std::string turtlename_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<FramePublisher>());
  rclcpp::shutdown();
  return 0;
}

1.1检查代码 [待校准@8367]

现在，让我们来看看与将海龟姿势发布到tf2相关的代码。首先，我们定义并获得一个单一参数 turtlename ，它指定了海龟的名字，例如 turtle1 或 turtle2 。 [待校准@8627]

this->declare_parameter<std::string>("turtlename", "turtle");
this->get_parameter("turtlename", turtlename_);

之后，节点订阅话题 turtleX/pose ，并对每一条传入的消息运行功能 handle_turtle_pose 。 [待校准@8628]

subscription_ = this->create_subscription<turtlesim::msg::Pose>(
 topic_name, 10,
 std::bind(&FramePublisher::handle_turtle_pose, this, _1));

现在，我们创建一个 TransformStamped 对象并为其提供适当的元数据。 [待校准@8629]

1. 我们需要给正在发布的转换一个时间戳，我们只需通过调用 this->get_clock()->now() 来标记当前时间。这将返回 Node 使用的当前时间。 [待校准@8630]

2. 然后，我们需要设置正在创建的链接的父帧的名称，在本例中为 world 。 [待校准@8631]

3. 最后，我们需要设置我们正在创建的链接的子节点的名称，在这种情况下，这是turtle本身的名称。 [待校准@8632]

海龟姿势信息的处理函数广播海龟的平移和旋转，并将其发布为从帧 world 到帧 turtleX 的转换。 [待校准@8633]

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

// Read message content and assign it to
// corresponding tf variables
t.header.stamp = now;
t.header.frame_id = "world";
t.child_frame_id = turtlename_.c_str();

在这里，我们将信息从3D海龟姿势复制到3D变换中。 [待校准@8634]

// Turtle only exists in 2D, thus we get x and y translation
// coordinates from the message and set the z coordinate to 0
t.transform.translation.x = msg->x;
t.transform.translation.y = msg->y;
t.transform.translation.z = 0.0;

// For the same reason, turtle can only rotate around one axis
// and this why we set rotation in x and y to 0 and obtain
// rotation in z axis from the message
tf2::Quaternion q;
q.setRPY(0, 0, msg->theta);
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 方法 TransformBroadcaster 负责广播。 [待校准@8635]

// Send the transformation
tf_broadcaster_->sendTransform(t);

注解

您也可以通过实例化 “tf2_ros:: 静态转换广播公司 `` instead of a `` 2_ros:: 转换广播公司” 来发布具有相同模式的静态转换。静态转换将在 /tf_static 话题上发布，并且仅在需要时发送，而不是周期调用y。更多详情见 here 。 [待校准@8636]

1.2 CMakeLists.txt [待校准@8637]

导航一层回到 learning_tf2_cpp 目录， CMakeLists.txt 和 package.xml 文件就在那里。 [待校准@8638]

现在打开 CMakeLists.txt ，添加可执行文件并将其命名为 turtle_tf2_broadcaster ，您稍后将在 ros2 run 中使用它。 [待校准@8639]

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

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

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

2写launch文件 [待校准@7572]

现在为此例程创建一个launch文件。使用文本编辑器，在 “ launch ” 文件夹中创建一个调用ed turtle_tf2_demo.launch.py 的新文件，并添加以下几行: [待校准@8641]

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='learning_tf2_cpp',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
    ])

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

首先，我们从 launch 和 launch_ros 包中导入所需模块。应该注意的是， launch 是一个通用的launch帧工作 (不是特定于ROS 2)， launch_ros 有特定于ROS 2的东西，比如我们在这里导入的节点。 [待校准@8642]

from launch import LaunchDescription
from launch_ros.actions import Node

现在，我们运行启动turtlesim仿真的节点，并使用我们的 turtle_tf2_broadcaster 节点向tf2广播 turtle1 状态。 [待校准@8643]

Node(
    package='turtlesim',
    executable='turtlesim_node',
    name='sim'
),
Node(
    package='learning_tf2_cpp',
    executable='turtle_tf2_broadcaster',
    name='broadcaster1',
    parameters=[
        {'turtlename': 'turtle1'}
    ]
),

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

导航一层回到 learning_tf2_cpp 目录， CMakeLists.txt 和 package.xml 文件就在那里。 [待校准@8638]

用文本编辑器打开 package.xml 。添加以下与您的launch文件的导入语句相对应的依赖项: [待校准@8645]

<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>

这声明了在执行其代码时所需的额外 launch 和 launch_ros 依赖关系。 [待校准@8646]

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

2.3 CMakeLists.txt [待校准@8648]

重新打开 CMakeLists.txt 并添加行，以便安装 launch/ 文件夹中的launch文件。更新 “安装 (目录…)” 以包括 launch 行。 [待校准@8649]

install(DIRECTORY
    launch
    ...
)

您可以了解更多关于在 this tutorial 中创建launch文件的信息。 [待校准@8650]

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

在工作区的根目录下运行 rosdep ，以检查是否缺少依赖项。 [待校准@8651]

LinuxmacOSWindows

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

构建更新的包，并源文件安装文件。 [待校准@8653]

现在运行将启动turtlesim仿真节点和 turtle_tf2_broadcaster 节点的launch文件: [待校准@8654]

ros2 launch learning_tf2_cpp turtle_tf2_demo.launch.py

在第二个终端窗口中，键入以下命令: [待校准@8442]

ros2 run turtlesim turtle_teleop_key

现在，您将看到turtlesim仿真已经从您可以控制的一只乌龟开始。 [待校准@8655]

现在，使用 tf2_echo 工具检查海龟姿势是否真的被广播到tf2: [待校准@8656]

ros2 run tf2_ros tf2_echo world turtle1

这应该会告诉你第一只乌龟的姿势。使用箭头键绕着海龟行驶 (确保你的 turtle_teleop_key 终端窗口处于活动状态，而不是你的模拟器窗口)。在控制台输出中，您将看到类似的内容: [待校准@8657]

At time 1625137663.912474878
- Translation: [5.276, 7.930, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137664.950813527
- Translation: [3.750, 6.563, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137665.906280726
- Translation: [2.320, 5.282, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137666.850775673
- Translation: [2.153, 5.133, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.365, 0.931]

如果你用 tf2_echo 在 world 和 turtle2 之间转换，你不应该看到转换，因为第二只乌龟还没有出现。然而，一旦我们在下一个教程中添加了第二只乌龟， turtle2 的姿势将被广播到tf2。 [待校准@8658]

总结

在本教程中，您学习了如何向tf2广播机器人的姿势 (海龟的位置和方向) 以及如何使用 tf2_echo 工具。要实际使用广播到tf2的转换，您应该继续下一个关于创建 tf2 listener 的教程。 [待校准@8659]