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