< >
Home » ROS2与tf2入门教程 » ROS2与tf2入门教程-添加框架 (C++)

ROS2与tf2入门教程-添加框架 (C++)

说明：

  • 介绍如何用C++添加框架

概述：

  • 在之前的教程中，我们通过编写一个 tf2 广播器和一个 tf2 监听器重新创建了海龟演示。

  • 本教程将教您如何向转换树添加额外的固定和动态帧。

  • 实际上，在 tf2 中添加框架与创建 tf2 广播器非常相似，但是这个示例将向您展示 tf2 的一些附加功能。

  • 对于许多与转换相关的任务，在本地框架内思考会更容易。

  • 例如，最容易推断激光扫描仪中心框架中的激光扫描测量值。

  • tf2 允许您为系统中的每个传感器、链接或关节定义一个本地框架。

  • 当从一帧转换到另一帧时，tf2 将处理所有引入的隐藏中间帧转换。

TF树:

  • tf2 建立了框架的树状结构，因此不允许框架结构中出现闭环。
  • 这意味着一个框架只有一个父级，但它可以有多个子级。
  • 目前，我们的 tf2 树包含三个框架：world、turtle1和turtle2。
  • 两个海龟框架turtle1和turtle2是框架world的子框架。
  • 如果我们想在tf2中添加一个新的frame，三个已经存在的frame中的一个需要是父frame，新的frame会成为它的子frame

请输入图片描述

固定帧广播器：

  • carrot1坐标系默认位于turtle1坐标系的右侧2米位置
  • 下载固定帧广播器代码
cd ~/tf2_ws/src/learning_tf2_cpp/src
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/fixed_frame_tf2_broadcaster.cpp
  • 打开文件fixed_frame_tf2_broadcaster.cpp
#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文件，再原来的内容基础上添加fixed_frame_tf2_broadcaster.cpp的内容
add_executable(fixed_frame_tf2_broadcaster src/fixed_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
  fixed_frame_tf2_broadcaster
  geometry_msgs
  rclcpp
  tf2
  tf2_ros
  turtlesim
)
install(TARGETS
  fixed_frame_tf2_broadcaster
  DESTINATION lib/${PROJECT_NAME})
  • 创建一个启动文件
cd ~/tf2_ws/src/learning_tf2_cpp/launch
vim 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',
      ),
   ])
  • 构建
colcon build --symlink-install --packages-select learning_tf2_cpp
  • 加载工作空间
. ~/tf2_ws/install/local_setup.bash

测试：

  • 启动海龟广播器演示
. ~/tf2_ws/install/local_setup.bash
ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py
  • 新carrot1框架出现在转换树中

请输入图片描述

通过参数指定跟随不同坐标系

  • 通过参数传递给启动文件,可以这样运行
ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py target_frame:=carrot1
  • 更改启动文件为
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
  target_frame = LaunchConfiguration('target_frame')

  declare_target_frame_cmd = DeclareLaunchArgument(
    'target_frame',
    default_value='carrot1',
    description='target_frame')

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

  return LaunchDescription([
    declare_target_frame_cmd,
    demo_nodes,
    Node(
          package='learning_tf2_cpp',
          executable='fixed_frame_tf2_broadcaster',
          name='fixed_broadcaster',
    ),
  ])
  • 构建
colcon build --symlink-install --packages-select learning_tf2_cpp
  • 加载工作空间
. ~/tf2_ws/install/local_setup.bash
  • 默认跟随carrot1,执行
ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py
  • 如果要跟随turtle1,执行
ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py target_frame:=turtle1

请输入图片描述

动态帧广播器

  • 下载动态帧广播器代码
cd ~/tf2_ws/src/learning_tf2_cpp/src
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/dynamic_frame_tf2_broadcaster.cpp
  • 打开文件dynamic_frame_tf2_broadcaster.cpp
#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;
}
  • 修改CMakeLists.txt文件，再原来的内容基础上添加dynamic_frame_tf2_broadcaster.cpp的内容
add_executable(dynamic_frame_tf2_broadcaster src/dynamic_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
  dynamic_frame_tf2_broadcaster
  geometry_msgs
  rclcpp
  tf2
  tf2_ros
  turtlesim
)
install(TARGETS
  dynamic_frame_tf2_broadcaster
  DESTINATION lib/${PROJECT_NAME})
  • 创建一个启动文件
cd ~/tf2_ws/src/learning_tf2_cpp/launch
vim 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',
      ),
   ])
  • 构建
colcon build --symlink-install --packages-select learning_tf2_cpp
  • 加载工作空间
. ~/tf2_ws/install/local_setup.bash
  • 启动turtle_tf2_dynamic_frame_demo.launch.py启动文件
ros2 launch learning_tf2_cpp turtle_tf2_dynamic_frame_demo.launch.py
  • 看到第二只乌龟跟随不断变化的位置

请输入图片描述

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros2与tf2入门教程