ROS学习——tf2

white_Learner

分类：ROS

发布时间 2020.11.27阅读数 10360 评论数 0

一、TF2简介

Since ROS Hydro, tf has been “deprecated” in favor of tf2. tf2 is an iteration on tf providing generally the same feature set more efficiently. As well as adding a few new features.

tf2是TF1的新版本，tf2包分为tf2和tf2_ros，前者用来进行坐标变换等具体操作，后者负责与ROS消息打交道。

二、TF2使用

2.1 broadcaster

2.1.1 静态

#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <cstdio>
#include <tf2/LinearMath/Quaternion.h>

std::string static_turtle_name;

int main(int argc, char **argv)
{
  ros::init(argc,argv, "my_static_tf2_broadcaster");
  if(argc != 8)
  {
    ROS_ERROR("Invalid number of parameters\nusage: static_turtle_tf2_broadcaster child_frame_name x y z roll pitch yaw");
    return -1;
  }
  if(strcmp(argv[1],"world")==0)
  {
    ROS_ERROR("Your static turtle name cannot be 'world'");
    return -1;

  }
  static_turtle_name = argv[1];
  static tf2_ros::StaticTransformBroadcaster static_broadcaster;
  geometry_msgs::TransformStamped static_transformStamped;

  static_transformStamped.header.stamp = ros::Time::now();
  static_transformStamped.header.frame_id = "world";
  static_transformStamped.child_frame_id = static_turtle_name;
  static_transformStamped.transform.translation.x = atof(argv[2]);
  static_transformStamped.transform.translation.y = atof(argv[3]);
  static_transformStamped.transform.translation.z = atof(argv[4]);
  tf2::Quaternion quat;
  quat.setRPY(atof(argv[5]), atof(argv[6]), atof(argv[7]));
  static_transformStamped.transform.rotation.x = quat.x();
  static_transformStamped.transform.rotation.y = quat.y();
  static_transformStamped.transform.rotation.z = quat.z();
  static_transformStamped.transform.rotation.w = quat.w();
  static_broadcaster.sendTransform(static_transformStamped);
  ROS_INFO("Spinning until killed publishing %s to world", static_turtle_name.c_str());
  ros::spin();
  return 0;
};

2.1.2 动态

广播器为动态广播器，通过节点接收坐标转换关系信息，然后通过广播器发布。

#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Quaternion.h>
 
geometry_msgs::TransformStamped transformStamped;
 
void dataCallback(const geometry_msgs::Twist::ConstPtr &msg) {
  transformStamped.header.frame_id = "origin_base";
  transformStamped.child_frame_id = "trans_base";
  transformStamped.transform.translation.x = msg->linear.x;
  transformStamped.transform.translation.y = msg->linear.y;
  transformStamped.transform.translation.z = msg->linear.z;
  tf2::Quaternion q;
  q.setRPY(0, 0, 0);
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();
}
 
int main(int argc, char **argv) {
  ros::init(argc, argv, "tf_broadcaster");
  ros::NodeHandle n;
 
  //转换信息订阅器
  ros::Subscriber sub = n.subscribe<geometry_msgs::Twist>(
      "/transform", 10, dataCallback);
 
  //坐标转换广播
  static tf2_ros::TransformBroadcaster broadcaster;
 
  //初始转换参数
  transformStamped.header.frame_id = "origin_base";
  transformStamped.child_frame_id = "trans_base";
  transformStamped.transform.translation.x = 0;
  transformStamped.transform.translation.y = 0;
  transformStamped.transform.translation.z = 0;
  tf2::Quaternion q;
  q.setRPY(0, 0, 0);
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();
  broadcaster.sendTransform(transformStamped);
 
  ros::Rate r(10);
 
  //广播坐标转换关系
  while (n.ok()) {
    transformStamped.header.stamp = ros::Time::now();
    broadcaster.sendTransform(transformStamped);
    r.sleep();
    ros::spinOnce();
  }
}

2.2 listener

接收器监视tf树整体，通过调用tf2_ros::TransformListener获取相关坐标系之间的坐标转换关系，实现坐标转换。

#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Pose.h>
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>

void transformPoint(const tf2_ros::Buffer &buffer)
{
  //设置原本坐标
  geometry_msgs::PointStamped base_point;
  base_point.header.frame_id = "base";
  base_point.header.stamp = ros::Time::now();
  base_point.point.x = 0.0;
  base_point.point.y = 0.0;
  base_point.point.z = 0.0;

  //坐标转换，显示转换后的坐标
  try
  {
    geometry_msgs::PointStamped world_point;
    
    // 方式一：坐标变换关系后，再调用doTransform变换
    // geometry_msgs::TransformStamped base_to_world;
    // base_to_world = buffer.lookupTransform("world", "base", past, ros::Duration(1.0));
    // tf2::doTransform(base_point, world_point, base_to_world);

    // 方式二：直接变换
    buffer.transform(base_point, world_point, "world");

    ROS_INFO("base_point: (%.2f, %.2f. %.2f) -----> world_point: (%.2f, %.2f, "
             "%.2f) at time %.2f",
             base_point.point.x, base_point.point.y, base_point.point.z, world_point.point.x, world_point.point.y,
             world_point.point.z, world_point.header.stamp.toSec());
  }
  catch (tf2::TransformException &ex)
  {
    ROS_ERROR("Received an exception trying to transform a point from "
              "\"base_point\" to \"world_point\": %s",
              ex.what());
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "tf_listener");
  ros::NodeHandle n;

  // 设置接收器
  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener(tfBuffer);

  // 定时获取坐标变换
  ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(tfBuffer)));
  ros::spin();
}

2.3 获取历史变换

坐标树随着时间不端变化，但tf2会存储一段时间的快照（默认最大为10s），因此我们可以获取特定时间的坐标变换信息。

直接获取历史坐标变换结果

void transformPoint(const tf2_ros::Buffer &buffer)
{
  //设置原本坐标
  geometry_msgs::PointStamped base_point;
  base_point.header.frame_id = "base";
  base_point.header.stamp = ros::Time::now()- ros::Duration(1.0);
  base_point.point.x = 0.0;
  base_point.point.y = 0.0;
  base_point.point.z = 0.0;

  //坐标转换，显示转换后的坐标
  try
  {
    geometry_msgs::PointStamped world_point;

	//获取变换，计算base的点在world坐标系下的位置
    buffer.transform(base_point, world_point, "world");

    ROS_INFO("base_point: (%.2f, %.2f. %.2f) -----> world_point: (%.2f, %.2f, "
             "%.2f) at time %.2f",
             base_point.point.x, base_point.point.y, base_point.point.z, world_point.point.x, world_point.point.y,
             world_point.point.z, world_point.header.stamp.toSec());
  }
  catch (tf2::TransformException &ex)
  {
    ROS_ERROR("Received an exception trying to transform a point from "
              "\"base_point\" to \"world_point\": %s",
              ex.what());
  }
}

获取历史变换

此处通过lookupTransform获取变换关系，在调用doTransform进行变换时忽略点的frame和stamp信息，以TransformStamped为准。

ros::Time past = ros::Time::now() - ros::Duration(1.0);
ros::Time now = ros::Time::now();

// 1. past的turtle1相对past的turtle2的坐标变换
geometry_msgs::TransformStamped turtle1_to_turtle2 = 
    buffer.lookupTransform("turtle2", "turtle1", past, ros::Duration(1.0));

// 2. past的turtle1相对now的turtle2的坐标变换
geometry_msgs::TransformStamped turtle1_to_turtle2 = 
    buffer.lookupTransform("turtle2", now, "turtle1", past, ros::Duration(1.0));

// 使用变换进行坐标转换，计算turtle1的点在turtle2坐标系上的位置
tf2::doTransform(turtle1_point, turtle2_point, turtle1_to_turtle2);

2.4 tf2_geometry_msgs

tf2的数据格式带有更好的封装，可以获取原点，四元数，逆矩阵和插补等，在计算时更友好。

tf2为独立包，其数据格式可以通过tf2_geometry_msgs与ros消息进行转换。

void tf2::fromMsg(msg,out)：将ros消息msg转换到对应的tf数据格式out
msg tf2::toMsg(in)：将tf数据格式in转换到对应的ros消息msg

2.5 旋转变换

欧拉角->四元数

  tf2::Quaternion q;
  q.setRPY(0, 0, 0);
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

四元数->欧拉角

tf2::Quaternion quat(x,y,z,w);

/**
 * @brief Return the yaw, pitch, roll of anything that can be converted to a tf2::Quaternion The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h
 * 
 * @param q tf2::Quaternion
 * @param yaw yaw
 * @param pitch pitch
 * @param roll roll
 */
void tf2::impl::getEulerYPR (const tf2::Quaternion &q, double &yaw, double &pitch,double &roll )

参考

tf2/Tutorials

ROS 坐标变换 TF数

原创文章作者：white_Learner。如若转载，请注明出处：古月居 http://admin.guyuehome.com/18145

打赏 0

上一篇：机械臂——六轴机械臂操作空间运动分析

下一篇：SLAM学习——卡尔曼滤波（KF/EKF）

ROS学习——tf2

white_Learner

一、TF2简介

二、TF2使用

2.1 broadcaster

2.1.1 静态

2.1.2 动态

2.2 listener

2.3 获取历史变换

2.4 tf2_geometry_msgs

2.5 旋转变换

参考

为你推荐

视觉SLAM中的对极约束、三角测量、PnP、ICP问题

无人机自动驾驶GAAS学习一

7.gmapping建图拓展篇：ase_exploration包| 源码梳理(三)

TF2 坐标变换 监听 实例

【Webots项目实战｜超市机器人（一）】需求建模｜模型设计｜基于Webots的Youbot模型搭建

精选Rviz插件分析（status_panel）

评论（0）

关于作者

white_Learner

38

0

0

2

移动机器人(一）——运动模型

移动机器人——里程计矫正

移动机器人——自定义规划算法

相关推荐

智能车仿真之机器人建图、定位、导航三部曲（一）

ROS——一文读懂：param参数

第五天：了解越疆dobot机械臂，使用python，编写通过ros服务控制机械臂运动程序

基于ROS2开发的点云栅格化

浅析ros下订阅消息，并发布apriltags算法位姿

精选myArm 全新七轴桌面型机械臂

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

TF2 坐标变换监听实例

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板