ROS学习——tf2

一、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

ros message type	tf2 datatype
geometry_msgs::Quaternion	tf2::Quaternion
geometry_msgs::QuaternionStamped	tf2::Stamped<tf2::Quaternion>
geometry_msgs::Transform	tf2::Transform
geometry_msgs::Pose	tf2::Transform
geometry_msgs::Vector3	tf2::Vector3
geometry_msgs::Vector3Stamped	tf2::Stamped<tf2::Vector3>
geometry_msgs::Point	tf2::Vector3
geometry_msgs::PointStamped	tf2::Stamped<tf2::Vector3>

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 )

3.6 MessageFilter

tf2_ros::MessageFilter可以订阅任何的ROS消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进行相应的处理（进入回调函数）。

tf2_ros::MessageFilter的初始化需要message_filters::Subscriber，tf转换，目标坐标系，等待时间。当message_filters::Subscriber的消息能够由tf转换到目标坐标系时，调用回调函数，回调函数由tf::MessageFilter::registerCallback()进行注册。

参考

tf2/Tutorials