ROS——一文读懂：tf变换

简介

所谓TF(TransForm)， 就是坐标转换，坐标变换包括了位置和姿态两个方面的变换， ROS中的tf是一个可以让用户随时记录多个坐标系的软件包。 tf保持缓存的树形结构中的坐标系之间的关系， 并且允许用户在任何期望的时间点在任何两个坐标系之间转换点， 矢量等。

tf的可以被当做是一种标准规范， 这套标准定义了坐标转换的数据格式和数据结构， tf本质是树状的数据结构，tf也可以看成是一个topic: /tf ， 话题中的message保存的就是tf tree的数据结构格式． 维护了整个机器人的甚至是地图的坐标转换关系。
观察上图，可以看到ROS数据结构的一个抽象图， ROS中机器人模型包含大量的部件，这些部件统称之为link,每一个link上面对应着一个frame, 即一个坐标系， link和frame概念是绑定在一起的，像上图可以看到又很多的frame,错综复杂的铺置在机器人的各个link上， 维护各个坐标系之间的关系， 就要靠着tf tree来处理。

每两个frame之间都会有一个Node来发布消息来broadcaster，来发布消息维系坐标转换，进而维护着各个坐标系之间的联通。

TF消息

TransformStamped.msg的格式规范如下：

std_mags/Header header
		uint32 seq
		time stamp
		string frame_id
string child_frame_id
geometry_msgs/Transform transform
		geometry_msgs/Vector3 translation
				float64 x
				float64 y
				float64 z
		geometry_msgs/Quaternion rotation
				float64 x
				float64 y
				flaot64 z
				float64 w

许多的TransformStamped.msg发向tf， 形成了TF树

TF树的数据类型

TF tree是由很多的frame之间TF拼接而成

• tf/tfMessage.msg
• tf2_msgs/TFMessage.msg
  TF的数据类型有两个， 主要的原因是版本的迭代。 自ROS Hydro以来， tf第一代已被“弃
  用”， 转而支持tf2。 tf2相比tf更加简单高效

使用命令 rostopic info /tf 查看tf版本

格式定义

tf/tfMessage.msg或tf2_msgs/TFMessage标准格式规范如下:

geometry_msgs/TransformStamped[] transforms
std_msgs/Header header
		uint32 seq
				time stamp
				string frame_id
				string child_frame_id
		geometry_msgs/Transform transform
		geometry_msgs/Vector3 translation
				float64 x
				float64 y
				float64 z
		geometry_msgs/Quaternion rotation
				float64 x
				float64 y
				flaot64 z
				float64 w

一个TransformStamped数组就是一个TF tree。

tf in c++

TF不仅仅是一个标准、 话题， 它还是一个接口。

数据类型

C++中给我们提供了很多TF的数据类型,如下：
tf::StampedTransform只能用在C++里， 只是C++的一个类， 一种数据格式， 并不是一个消息。而geometry_msgs/TransformStamped.msg是一个message,它依赖于ROS， 与语言无关，

数据转换

TF里有可能会遇到各种各样数据的转换， 常见的四元数、 旋转矩阵、 欧拉角这三种数据之间的转换

定义空间点和空间向量

//计算两个向量的点积
tfScalar::tfDot(const Vector3 &v1, const Vector3 &v2)

//计算向量的模
tfScalar length()

//求与已知向量同方向的单位向量
Vector3 &normalize()

//计算两个向量的夹角
tfScalar::tfAngle(const Vector3 &v1, const Vector3 &v2)

//计算两个向量的距离
tfScale::tfDistance(const Vector3 &v1, const Vector3 &v2)

//计算两个向量的乘积
tfScale::tfCross(const Vector3 &v1,const Vector3 &v2)

定义四元数

//由欧拉角计算四元数
setRPY(const tfScalar& yaw, const stScalar &pitch, const tfScalar &roll)

//由四元数得到旋转轴
Vector3 getAxis()

//已知旋转轴和旋转角估计四元数
setRotation(const Vector3 &axis, const tfScalar& angle)

定义旋转矩阵

//通过四元数得到旋转矩阵
setRotaion(const Quaternion &q)

//由旋转矩阵求欧拉角
getEulerYPR(tfScalar &yaw, tfScalar &pitch, tfScalar &roll )

TF类

tf::TransformBroadcaster类

transformBroadcaster()
void sendTransform(const StampedTransform &transform)
void sendTransform(const std::vector<StampedTransform> &transforms)
void sendTransform(const geometry_msgs::TransformStamped &transform)
void sendTransform(const std::vector<geometry_msgs::TransformStamped> &transforms)

broadcaster就是一个publisher,而sendTransform的作用是来封装publish的函数。 在实际使用中， 需要在某个Node中构建tf::TransformBroadcaster类， 然后调用sendTransform(),将transform发布到 /tf 的一段transform上

tf::TransformListener类

从 /tf 上接收的类

//lookupTransform得到从源坐标系到目标坐标系之间的转换关系
void tf::TransformListener::lookupTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
//target_frame：目标坐标系
//source_frame：源坐标系
//time：查询时刻，常用 ros::Time（0） ,这个表示为最新的坐标转换关系，而 ros::time::now 则会因为收发延迟的原因， 而不能正确获取当前最新的坐标转换关系
//transform：存储转换关系的位置

//canTransform()是用来判断两个transform之间是否连通
bool canTransform()

//waitForTransform()const是用来等待某两个transform之间的连通
bool waitForTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
//target_frame：目标坐标系
//source_frame：源坐标系
//time：查询时刻
//polling_sleep_duration 转换fail重试时间
//error_msg：保存转换失败的信息，转换成功为NULL

代码演示

C++代码的思路分为两个部分：

• 编写一个节点，广播两个参考系之间的tf变换关系
• 编写另外一个节点，订阅tf树，然后从tf树中遍历到两个参考系之间的变换公式，然后通过公式计算数据的变换。

tf.broadcaster.cpp

#include 
#include 
#include 
//退出用： ctrl+z
int main(int argc, char** argv){
//初始化
		ros::init(argc, argv, "tf_broadcaster");
		ros::NodeHandle node;
		static tf::TransformBroadcaster br;
		tf::Transform transform;
		//geometry_msgs::Quaternion qw;
		tf::Quaternion q;
		//定义初始坐标和角度
		double roll=0,pitch=0,yaw=0,x=1.0,y=2.0,z=3.0;
		ros::Rate rate(1);
		while(ros::ok())
		{ 
				yaw+=0.1;//每经过一秒开始一次变换
				//输入欧拉角， 转化成四元数在终端输出
				q.setRPY(roll,pitch,yaw);
				//qw=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);方法2
				transform.setOrigin(tf::Vector3(x,y,z));
				transform.setRotation(q);
				std::cout<<"发布tf变换： sendTransform函数"<<std::endl;
				br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"base_link","link1"));
				std::cout<<"输出的四元数为： w="<<q[3]<<",x="<<q[0]<<",y="<<q[1]<<",z="<<q[2]<<std::endl;
				// std::cout<<"输出的四元数为： w="<
				rate.sleep();
				ros::spinOnce();
		}
		return 0;
};

tf_listerner.cpp

#include 
#include 
#include 

int main(int argc, char** argv){
		ros::init(argc, argv, "tf_listener");
		ros::NodeHandle node;
		tf::TransformListener listener;
		//1. 阻塞直到frame相通
		std::cout<<"1. 阻塞直到frame相通"<<std::endl;
		listener.waitForTransform("/base_link","link1",ros::Time(0),ros::Duration(4.0));
		ros::Rate rate(1);
		while (node.ok()){
		tf::StampedTransform transform;
		try{
				//2. 监听对应的tf,返回平移和旋转
				std::cout<<"2. 监听对应的tf,返回平移和旋转"<<std::endl;
				listener.lookupTransform("/base_link", "/link1",ros::Time(0), transform);
				//ros::Time(0)表示最近的一帧坐标变换， 不能写成ros::Time::now()
		} 
		catch (tf::TransformException &ex) 
		{
				ROS_ERROR("%s",ex.what());
				ros::Duration(1.0).sleep();
				continue;
		} 
		
		std::cout<<"输出的位置坐标： x="<<transform.getOrigin().x()<<",y="<<transform.getOrigin().y()<<",z="<<transform.getOrigin().z()<<std::endl;
		std::cout<<"输出的旋转四元数： w="<<transform.getRotation().getW()<<",x="<<transform.getRotation().getX()<<",y="<<transform.getRotation().getY()<<",z="<<transform.getRotation().getZ()<<std::endl;
		rate.sleep();
		} 
		return 0;
};