添加帧 (c ++) [待校准@8354]

Goal目标: Learn了解如何向tf2添加额外的帧。 [待校准@8355]

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

时间: 15分钟 [Alyssa@6755]

背景

在之前的教程中，我们通过编写 tf2 broadcaster and a tf2 listener 重新创建了海龟例程。本教程将教您如何向转换树添加额外的固定和动态帧。实际上，在tf2中添加帧与创建tf2广播器非常相似，但是此示例将向您展示tf2的一些附加功能。 [待校准@8356]

许多任务转换，容易认为内本地帧。例如，最容易推理激光扫描仪中心帧中的激光扫描测量。tf2允许你定义本地帧每个传感器、连接或联合系统。当从一个帧，tf2会照顾所有隐藏中间帧转换介绍。 [待校准@8357]

tf2树 [待校准@8358]

tf2建立了帧的树结构，因此不允许在帧结构中出现闭环。这意味着一个帧只有一个父级，但它可以有多个子级。目前，我们的tf2树包含三个帧: world 、 turtle1 和 turtle2 。这两个海龟帧是 world 帧的孩子。如果要向tf2添加新帧，则现有的三个帧之一必须是父帧，而新帧将成为其子帧。 [待校准@8359]

任务

1编写固定帧广播器 [待校准@8360]

在我们的海龟例子中，我们将添加一个新的帧 carrot1 ，它将是 turtle1 的孩子。这一帧将作为第二只乌龟的目标。 [待校准@8361]

让我们先创建源文件。转到我们在之前的教程中创建的 learning_tf2_cpp 包。通过输入以下命令下载固定帧广播器代码: [待校准@8362]

LinuxmacOSWindows

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

现在打开调用ed fixed_frame_tf2_broadcaster.cpp 的文件。 [待校准@8365]

#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>

#include <memory>

using namespace std::chrono_literals;

class FixedFrameBroadcaster : public rclcpp::Node
{
public:
  FixedFrameBroadcaster()
  : Node("fixed_frame_tf2_broadcaster")
  {
    tf_publisher_ = 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()
  {
    rclcpp::Time now = this->get_clock()->now();
    geometry_msgs::msg::TransformStamped t;

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

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

不要忘记在 CMakeLists.txt 中添加可执行文件。该代码与tf2广播器教程示例非常相似，唯一的区别是此处的转换不会随时间变化。 [待校准@8366]

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

让我们看一下这段代码中的关键行。在这里，我们创建了一个新的转换，从母 turtle1 到新 carrot1 。就 turtle1 帧而言， carrot1 帧在y轴上偏移了2米。 [待校准@8368]

geometry_msgs::msg::TransformStamped t;

t.header.stamp = 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写launch文件 [待校准@8369]

现在让我们为这个例子创建一个launch文件。使用文本编辑器，创建一个调用ed turtle_tf2_fixed_frame_demo.launch.py 的新文件，并添加以下几行: [待校准@8370]

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',
      ),
   ])

这个launch文件导入所需的包，然后创建一个 demo_nodes 变量，该变量将存储我们在上一教程的launch文件中创建的节点。 [待校准@8371]

最后部分的代码添加固定 carrot1 帧的turtlesim世界我们 fixed_frame_tf2_broadcaster 节点。 [待校准@8372]

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

1.3构建和运行 [待校准@8373]

重建包并启动turtle广播器例程: [待校准@8374]

ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py

您应该注意到新的 carrot1 帧出现在转换树中。 [待校准@8375]

1.4检查结果 [待校准@8376]

如果你开着第一只乌龟到处跑，你应该注意到行为与之前的教程没有改变，即使我们添加了一个新的帧。这是因为添加额外的帧不会影响其他帧，并且我们的侦听器仍在使用先前定义的帧。 [待校准@8377]

因此，如果我们想让我们的第二只乌龟跟随胡萝卜而不是第一只乌龟，我们需要改变 target_frame 的价值。这可以通过两种方式完成。一种方法是直接从控制台将 target_frame 参数传递给launch文件: [待校准@8378]

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

第二种方法是更新launch文件。为此，打开 turtle_tf2_fixed_frame_demo.launch.py 文件，并通过 launch_arguments 参数添加 “'target_frame': 'carrot1'” 参数。 [待校准@8379]

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

现在只要重建包装，重新启动 turtle_tf2_fixed_frame_demo.launch.py ，你就会看到第二只乌龟跟着胡萝卜，而不是第一只乌龟! [待校准@8380]

2编写动态帧广播器 [待校准@8381]

我们在本教程中发布的额外帧是固定帧，相对于父帧不会随时间变化。但是，如果要发布移动帧，则可以对广播器进行编码以随时间更改帧。让我们改变我们的 carrot1 帧，使其相对于 turtle1 帧随时间变化。现在，通过输入以下命令下载动态帧广播器代码: [待校准@8382]

LinuxmacOSWindows

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

现在打开调用ed dynamic_frame_tf2_broadcaster.cpp 的文件: [待校准@8383]

#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>

#include <memory>

using namespace std::chrono_literals;

const double PI = 3.141592653589793238463;

class DynamicFrameBroadcaster : public rclcpp::Node
{
public:
  DynamicFrameBroadcaster()
  : Node("dynamic_frame_tf2_broadcaster")
  {
    tf_publisher_ = 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_publisher_->sendTransform(t);
  }
  rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_publisher_;
};

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

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

代替我们的x和y偏移的固定定义，我们在当前时间使用 sin() 和 cos() 函数，以便 carrot1 的偏移不断变化。 [待校准@8384]

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

2.2写launch文件 [待校准@8385]

测试此代码，创建新launch文件 turtle_tf2_dynamic_frame_demo.launch.py 并粘贴以下代码: [待校准@8386]

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.3构建和运行 [待校准@8387]

重建包，启动 turtle_tf2_dynamic_frame_demo.launch.py launch文件，现在你会看到第二只乌龟跟随胡萝卜不断变化的位置。 [待校准@8388]

总结

在本教程中，您将了解tf2转换树、其结构及其特性。您还了解到在本地帧内部进行思考是最容易的，并且学习了为该本地帧添加额外的固定和动态帧。 [待校准@8389]