添加框架 (C++)

目标： 学习如何向 tf2 添加额外框架。

教程级别： 中级

时间： 15 分钟

背景 

在之前的教程中，我们通过编写 tf2 广播器和 tf2 监听器重新创建了 turtle 演示。本教程将教您如何向转换树添加额外的固定和动态框架。事实上，在 tf2 中添加框架与创建 tf2 广播器非常相似，但此示例将向您展示 tf2 的一些其他功能。

对于许多与转换相关的任务，在本地框架内思考更容易。例如，在激光扫描仪中心的框架中推理激光扫描测量是最容易的。 tf2 允许您为系统中的每个传感器、链接或关节定义一个本地框架。当从一个框架转换到另一个框架时，tf2 将处理引入的所有隐藏的中间框架转换。

tf2 树 

tf2 构建了一个框架树结构，因此不允许框架结构中出现闭环。这意味着一个框架只有一个父框架，但可以有多个子框架。目前，我们的 tf2 树包含三个框架：“world”、“turtle1”和“turtle2”。这两个海龟框架是“world”框架的子框架。如果我们想向 tf2 添加一个新框架，则三个现有框架中的一个需要作为父框架，而新框架将成为其子框架。

任务 

1 编写固定帧广播器 

在我们的海龟示例中，我们将添加一个新帧“carrot1”，它将是“turtle1”的子帧。

此帧将作为第二只海龟的目标。

让我们首先创建源文件。转到我们在前面的教程中创建的“learning_tf2_cpp”包。在“src”目录中输入以下命令下载固定帧广播器代码：

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

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

In a Windows command line prompt:

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

Or in powershell:

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

现在打开名为“fixed_frame_tf2_broadcaster.cpp”的文件。

#include <chrono>
#include <functional>
#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"

using namespace std::chrono_literals;

class FixedFrameBroadcaster : public rclcpp::Node
{
public:
  FixedFrameBroadcaster()
  : Node("fixed_frame_tf2_broadcaster")
  {
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    timer_ = this->create_wall_timer(
      100ms, std::bind(&FixedFrameBroadcaster::broadcast_timer_callback, this));
  }

private:
  void broadcast_timer_callback()
  {
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "turtle1";
    t.child_frame_id = "carrot1";
    t.transform.translation.x = 0.0;
    t.transform.translation.y = 2.0;
    t.transform.translation.z = 0.0;
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;

    tf_broadcaster_->sendTransform(t);
  }

rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

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

该代码与 tf2 广播器教程示例非常相似，唯一的区别是此处的变换不会随时间而变化。

1.1 检查代码 

让我们来看看这段代码中的关键行。在这里，我们创建一个新的变换，从父级“turtle1”到新的子级“carrot1”。相对于“turtle1”框架，“carrot1”框架在 y 轴上偏移了 2 米。

geometry_msgs::msg::TransformStamped t;

t.header.stamp = this->get_clock()->now();
t.header.frame_id = "turtle1";
t.child_frame_id = "carrot1";
t.transform.translation.x = 0.0;
t.transform.translation.y = 2.0;
t.transform.translation.z = 0.0;

1.2 CMakeLists.txt 

返回上一级到“learning_tf2_cpp”目录，其中有“CMakeLists.txt”和“package.xml”文件。

现在打开“CMakeLists.txt”，添加可执行文件并将其命名为“fixed_frame_tf2_broadcaster”。

add_executable(fixed_frame_tf2_broadcaster src/fixed_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
    fixed_frame_tf2_broadcaster
    geometry_msgs
    rclcpp
    tf2_ros
)

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

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

1.3 编写启动文件 

现在让我们为这个示例创建一个启动文件。使用文本编辑器，在“src/learning_tf2_cpp/launch”目录中创建一个名为“turtle_tf2_fixed_frame_demo.launch.py”的新文件，并添加以下几行：

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('learning_tf2_cpp'), 'launch'),
            '/turtle_tf2_demo.launch.py']),
        )

    return LaunchDescription([
        demo_nodes,
        Node(
            package='learning_tf2_cpp',
            executable='fixed_frame_tf2_broadcaster',
            name='fixed_broadcaster',
        ),
    ])

此启动文件导入所需的包，然后创建一个“demo_nodes”变量，该变量将存储我们在上一个教程的启动文件中创建的节点。

代码的最后一部分将使用我们的“fixed_frame_tf2_broadcaster”节点将我们的固定“carrot1”框架添加到 turtlesim 世界。

Node(
    package='learning_tf2_cpp',
    executable='fixed_frame_tf2_broadcaster',
    name='fixed_broadcaster',
),

1.4 构建 

在工作区根目录中运行“rosdep”以检查是否缺少依赖项。

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

rosdep only runs on Linux, so you will need to install geometry_msgs and turtlesim dependencies yourself

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

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

1.5 运行 

现在您可以启动 turtle 广播器演示：

ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py

您应该注意到，新的“carrot1”框架出现在变换树中。

../../../_images/turtlesim_frames_carrot.png

如果你驾驶第一只乌龟四处走动，你应该注意到，尽管我们添加了一个新框架，但其行为与上一个教程相比并没有变化。这是因为添加额外的框架不会影响其他框架，我们的侦听器仍在使用先前定义的框架。因此，如果我们希望第二只乌龟跟随胡萝卜而不是第一只乌龟，我们需要更改“target_frame”的值。这可以通过两种方式完成。一种方法是直接从控制台将“target_frame”参数传递给启动文件：

ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py target_frame:=carrot1

第二种方法是更新启动文件。为此，打开“turtle_tf2_fixed_frame_demo.launch.py”文件，并通过“launch_arguments”参数添加“’target_frame’: ‘carrot1’”参数。

def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        ...,
        launch_arguments={'target_frame': 'carrot1'}.items(),
        )

现在重建包，重新启动“turtle_tf2_fixed_frame_demo.launch.py”，你会看到第二只海龟跟着胡萝卜，而不是第一只海龟！

2 编写动态帧广播器 

我们在本教程中发布的额外帧是一个固定帧，相对于父帧不会随时间而变化。但是，如果您想发布移动帧，您可以编写广播器以随时间更改帧。让我们更改我们的“carrot1”帧，使其相对于“turtle1”帧随时间而变化。转到我们在上一个教程中创建的“learning_tf2_cpp”包。在“src”目录中输入以下命令下载动态帧广播器代码：

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

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

In a Windows command line prompt:

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

Or in powershell:

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

现在打开名为“dynamic_frame_tf2_broadcaster.cpp”的文件：

#include <chrono>
#include <functional>
#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"

using namespace std::chrono_literals;

const double PI = 3.141592653589793238463;

class DynamicFrameBroadcaster : public rclcpp::Node
{
public:
  DynamicFrameBroadcaster()
  : Node("dynamic_frame_tf2_broadcaster")
  {
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    timer_ = this->create_wall_timer(
      100ms, std::bind(&DynamicFrameBroadcaster::broadcast_timer_callback, this));
  }

private:
  void broadcast_timer_callback()
  {
    rclcpp::Time now = this->get_clock()->now();
    double x = now.seconds() * PI;

    geometry_msgs::msg::TransformStamped t;
    t.header.stamp = now;
    t.header.frame_id = "turtle1";
    t.child_frame_id = "carrot1";
    t.transform.translation.x = 10 * sin(x);
    t.transform.translation.y = 10 * cos(x);
    t.transform.translation.z = 0.0;
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;

    tf_broadcaster_->sendTransform(t);
  }

  rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

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

2.1 检查代码 

我们没有对 x 和 y 偏移进行固定定义，而是在当前时间使用“sin()”和“cos()”函数，这样“carrot1”的偏移就会不断变化。

double x = now.seconds() * PI;
...
t.transform.translation.x = 10 * sin(x);
t.transform.translation.y = 10 * cos(x);

2.2 CMakeLists.txt 

返回上一级到“learning_tf2_cpp”目录，其中有“CMakeLists.txt”和“package.xml”文件。

现在打开“CMakeLists.txt”，添加可执行文件并将其命名为“dynamic_frame_tf2_broadcaster”。

add_executable(dynamic_frame_tf2_broadcaster src/dynamic_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
    dynamic_frame_tf2_broadcaster
    geometry_msgs
    rclcpp
    tf2_ros
)

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

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

2.3 编写启动文件 

为了测试此代码，在“src/learning_tf2_cpp/launch”目录中创建一个新的启动文件“turtle_tf2_dynamic_frame_demo.launch.py”，并粘贴以下代码：

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('learning_tf2_cpp'), 'launch'),
            '/turtle_tf2_demo.launch.py']),
        launch_arguments={'target_frame': 'carrot1'}.items(),
        )

    return LaunchDescription([
        demo_nodes,
        Node(
            package='learning_tf2_cpp',
            executable='dynamic_frame_tf2_broadcaster',
            name='dynamic_broadcaster',
        ),
    ])

2.4 构建 

在工作区根目录中运行“rosdep”以检查是否缺少依赖项。

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

rosdep only runs on Linux, so you will need to install geometry_msgs and turtlesim dependencies yourself

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

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

2.5 运行 

现在可以启动动态框架演示了：

ros2 launch learning_tf2_cpp turtle_tf2_dynamic_frame_demo.launch.py

你应该看到第二只乌龟正在跟随不断变化的胡萝卜的位置。

摘要 

在本教程中，您了解了 tf2 变换树、其结构及其功能。您还了解到，在局部框架内思考最容易，并学会了为该局部框架添加额外的固定和动态框架。