ROS-TF与URDF

📖 机器人的坐标变换一直以来是机器人学的一个难点，我们人类在进行一个简单的动作时，从思考到实施行动再到完成动作可能仅仅需要几秒钟，但是机器人来讲就需要大量的计算和坐标转换。

---

1. 认识TF

1.1. 简介

TF是一个ROS世界里的一个基本的也是很重要的概念，所谓TF(TransForm)，就是坐标转换．在现实生活中，我们做出各种行为模式都可以在很短的时间里完成，比如拿起身边的物品，但是在机器人的世界里，则远远没有那么简单．观察下图，我们来分析机器人拿起身边的物品需要做到什么，而TF又起到什么样的作用．

观察这个机器人，我们直观上不认为拿起物品会又什么难度，站在人类的立场上，我们也许会想到手向前伸，抓住，手收回．就完成了这整个一系列的动作．但是如今的机器人远远没有这么智能，它能得到的只是各种传感器发送回来的数据，然后它再处理各种数据进行操作，比如手臂弯曲45度，再向前移动２０cm等这样的各种十分精确的数据，尽管如此，机器人依然没法做到像人类一样自如的进行各种行为操作．那么在这个过程中，TF又扮演着什么样的角色呢？还拿该图来说，当机器人的＂眼睛＂获取一组数据，关于物体的坐标方位，但是相对于机器人手臂来说，这个坐标只是相对于机器人头部的传感器，并不直接适用于机器人手臂执行，那么物体相对于头部和手臂之间的坐标转换，就是TF.

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

1.2. ROS中的TF

tf的定义不是那么的死板，它可以被当做是一种标准规范，这套标准定义了坐标转换的数据格式和数据结构．tf本质是树状的数据结构，所以我们通常称之为**“tf tree”**,tf也可以看成是一个topic:/tf，话题中的message保存的就是tf tree的数据结构格式．维护了整个机器人的甚至是地图的坐标转换关系．tf还可以看成是一个package,它当中包含了很多的工具．比如可视化，查看关节间的tf,debug tf等等．tf含有一部分的接口，就是我们前面章节介绍的roscpp和rospy里关于tf的API．所以可以看成是话题转换的标准，工具，接口．

观察上图，我们可以看到ROS数据结构的一个抽象图，ROS中机器人模型包含大量的部件，这些部件统称之为link,每一个link上面对应着一个frame, 即一个坐标系．link和frame概念是绑定在一起的．像上图pr2模型中我们可以看到又很多的frame,错综复杂的铺置在机器人的各个link上，维护各个坐标系之间的关系，就要靠着tf tree来处理，维护着各个坐标系之间的联通．如下图：

上图是我们常用的robot_sim_demo运行起来的tf　tree结构，每一个圆圈代表一个frame,对应着机器人上的一个link，任意的两个frame之间都必须是联通的，如果出现某一环节的断裂，就会引发error系统报错．所以完整的tf tree不能有任何断层的地方，这样我们才能查清楚任意两个frame之间的关系．仔细观察上图，我们发现每两个frame之间都有一个broadcaster,这就是为了使得两个frame之间能够正确连通，中间都会有一个Node来发布消息来broadcaster.如果缺少Node来发布消息维护连通，那么这两个frame之间的连接就会断掉．broadcaster就是一个publisher,如果两个frame之间发生了相对运动，broadcaster就会发布相关消息．

2. TF消息

2.1. TransformStamped.msg

上一节在介绍ROS中的TF时候我们已经初步的认识了TF和TF树，了解了在每个frame之间都会有broadcaster来发布消息维系坐标转换．那么这个消息到底是什么样子的呢？这个消息TransformStampde.msg,它就是处理两个frame之间一小段tf的数据格式．

2.2. 格式规范

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

观察标准的格式规范，首先header定义了序号，时间以及frame的名称．接着还写了child_frame，这两个frame之间要做那种变换就是由geometry_msgs/Transform来定义．Vector3三维向量表示平移，Quaternion四元数表示旋转．像下图TF树中的两个frame之间的消息，就是由这种格式来定义的．odom就是frame_id,baselink_footprint就是child_frame_id.我们知道，一个topic上面可能会有很多个node向上面发送消息。如图所示，不仅有我们看到的frame发送坐标变换个tf，还有别的frame也在同样的向它发送消息。最终，许多的TransformStamped.msg发向tf，形成了TF树。

2.3. TF树的数据类型

上面我们讲了，TF tree是由很多的frame之间TF拼接而成。那么TF tree是什么类型呢？如下：

• tf/tfMessage.msg
• tf2_msgs/TFMessage.msg

这里TF的数据类型有两个，主要的原因是版本的迭代。自ROS Hydro以来，tf第一代已被“弃用”，转而支持tf2。tf2相比tf更加简单高效。此外也添加了一些新的功能。

由于tf2是一个重大的变化，tf API一直保持现有的形式。由于tf2具有tf特性的超集和一部分依赖关系，所以tf实现已经被移除，并被引用到tf2下。这意味着所有用户都将与tf2兼容。官网建议新工作直接使用tf2，因为它有一个更清洁的界面，和更好的使用体验。

如何查看自己使用的TF是哪一个版本，使用命令rostopic info /tf即可。

2.4. 格式定义

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。

3. tf in c++

3.1. 简介

前面内容我们介绍了TF的基本的概念和TF树消息的格式类型，我们知道，TF不仅仅是一个标准、话题，它还是一个接口。本节课我们就介绍c++中TF的一些函数和写法。

3.2. 数据类型

C++中给我们提供了很多TF的数据类型，如下表：

名称	数据类型
向量	tf::Vector3
点	tf::Point
四元数	tf::Quaternion
3*3矩阵（旋转矩阵）	tf::Matrix3x3
位姿	tf::pose
变换	tf::Transform
带时间戳的以上类型	tf::Stamped
带时间戳的变换	tf::StampedTransform

**易混注意：**虽然此表的最后带时间戳的变换数据类型为tf::StampedTransform,和上节我们所讲的geometry_msgs/TransformStamped.msg看起来很相似，但是其实数据类型完全不一样，tf::StampedTransform只能用在C里，只是C的一个类，一种数据格式，并不是一个消息。而geometry_msgs/TransformStamped.msg是一个message,它依赖于ROS，与语言无关，也即是无论何种语言，C++、Python、Java等等，都可以发送该消息。

3.3. 数据转换

在TF里有可能会遇到各种各样数据的转换，例如常见的四元数、旋转矩阵、欧拉角这三种数据之间的转换。tf in roscpp给了我们解决该问题的函数。详细源码在我们教学课程的代码包中。 首先在tf中与数据转化的数据都类型都包含在#include头文件中，我们将与数据转换相关API都存在tf_demo中的coordinate_transformation.cpp当中，其中列表如下：

3.4. 第1部分定义空间点和空间向量

编号	函数名称	函数功能
1.1	tfScalar::tfDot(const Vector3 &v1, const Vector3 &v2)	计算两个向量的点积
1.2	tfScalar length()	计算向量的模
1.3	Vector3 &normalize()	求与已知向量同方向的单位向量
1.4	tfScalar::tfAngle(const Vector3 &v1, const Vector3 &v2)	计算两个向量的夹角
1.5	tfScale::tfDistance(const Vector3 &v1, const Vector3 &v2)	计算两个向量的距离
1.6	tfScale::tfCross(const Vector3 &v1,const Vector3 &v2)	计算两个向量的乘积

示例代码：

#include <ros/ros.h>
#include <tf/tf.h>
//退出用：ctrl+z
int main(int argc, char** argv){
//初始化
ros::init(argc, argv, "coordinate_transformation");
ros::NodeHandle node;
tf::Vector3 v1(1,1,1);
tf::Vector3 v2(1,0,1);
//第1部分，定义空间点和空间向量
std::cout<<"第1部分，定义空间点和空间向量"<<std::endl;
//1.1 计算两个向量的点积
std::cout<<"向量v1:"<<"("<<v1[0]<<","<<v1[1]<<","<<v1[2]<<"),";
std::cout<<"向量v2:"<<"("<<v2[0]<<","<<v2[1]<<","<<v2[2]<<")"<<std::endl;
std::cout<<"两个向量的点积："<<tfDot(v1,v2)<<std::endl;
//1.2 计算向量的模
std::cout<<"向量v2的模值:"<<v2.length()<<std::endl;
//1.3 求与已知向量同方向的单位向量
tf::Vector3 v3;
v3=v2.normalize();
std::cout<<"与向量v2的同方向的单位向量v3:"<<"("<<v3[0]<<","<<v3[1]<<","<<v3[2]<<")"<<std::endl;
//1.4 计算两个向量的夹角
std::cout<<"两个向量的夹角(弧度):"<<tfAngle(v1,v2)<<std::endl;
//1.5 计算两个向量的距离
std::cout<<"两个向量的距离:"<<tfDistance2(v1,v2)<<std::endl;
//1.6 计算两个向量的乘积
tf::Vector3 v4;
v4=tfCross(v1,v2);
std::cout<<"两个向量的乘积v4:"<<"("<<v4[0]<<","<<v4[1]<<","<<v4[2]<<")"<<std::endl;
return 0;
}

3.5. 第2部分定义四元数

编号	函数名称	函数功能
2.1	setRPY(const tfScalar& yaw, const stScalar &pitch, const tfScalar &roll)	由欧拉角计算四元数
2.2	Vector3 getAxis()	由四元数得到旋转轴
2.3	setRotation(const Vector3 &axis, const tfScalar& angle)	已知旋转轴和旋转角估计四元数

示例代码：

#include <ros/ros.h>
#include <tf/tf.h>
//退出用：ctrl+z
int main(int argc, char** argv){
//初始化
ros::init(argc, argv, "coordinate_transformation");
ros::NodeHandle node;
std::cout<<"第2部分，定义四元数"<<std::endl;
//2.1 由欧拉角计算四元数
tfScalar yaw,pitch,roll;
yaw=0;pitch=0;roll=0;
std::cout<<"欧拉角rpy("<<roll<<","<<pitch<<","<<yaw<<")";
tf::Quaternion q;
q.setRPY(yaw,pitch,roll);
std::cout<<"，转化到四元数q:"<<"("<<q[3]<<","<<q[0]<<","<<q[1]<<","<<q[2]<<")"<<std::endl;
//2.2 由四元数得到旋转轴
tf::Vector3 v5;
v5=q.getAxis();
std::cout<<"四元数q的旋转轴v5"<<"("<<v5[0]<<","<<v5[1]<<","<<v5[2]<<")"<<std::endl;
//2.3 由旋转轴和旋转角来估计四元数
tf::Quaternion q2;
q2.setRotation(v5,1.570796);
std::cout<<"旋转轴v5和旋转角度90度，转化到四元数q2:"<<"("<<q2[3]<<","<<q2[0]<<","<<q2[1]<<","<<q2[2]<<")"<<std::endl;
return 0;
}

3.6. 第3部分定义旋转矩阵

编号	函数名称	函数功能
3.1	setRotaion(const Quaternion &q)	通过四元数得到旋转矩阵
3.2	getEulerYPR(tfScalar &yaw, tfScalar &pitch, tfScalar &roll )	由旋转矩阵求欧拉角

示例代码：

#include <ros/ros.h>
#include <tf/tf.h>
//退出用：ctrl+z
int main(int argc, char** argv){
//初始化
ros::init(argc, argv, "coordinate_transformation");
ros::NodeHandle node;
//第3部分，定义旋转矩阵
std::cout<<"第3部分，定义旋转矩阵"<<std::endl;
//3.1 由旋转轴和旋转角来估计四元数
tf::Quaternion q2(1,0，0,0）;
tf::Matrix3x3 Matrix;
tf::Vector3 v6,v7,v8;
Matrix.setRotation(q2);
v6=Matrix[0];
v7=Matrix[1];
v8=Matrix[2];
std::cout<<"四元数q2对应的旋转矩阵M:"<<v6[0]<<","<<v6[1]<<","<<v6[2]<<std::endl;
std::cout<<"                       "<<v7[0]<<","<<v7[1]<<","<<v7[2]<<std::endl;
std::cout<<"                       "<<v8[0]<<","<<v8[1]<<","<<v8[2]<<std::endl;
//3.2 通过旋转矩阵求欧拉角
tfScalar m_yaw,m_pitch,m_roll;
Matrix.getEulerYPR(m_yaw,m_pitch,m_roll);
std::cout<<"由旋转矩阵M,得到欧拉角rpy("<<m_roll<<","<<m_pitch<<","<<m_yaw<<")"<<std::endl;
return 0;
};

此外，在tf_demo的教学包中，我们还提供常见的欧拉角与四元数的互换，详见Euler2Quaternion.cpp与Quaternion2Euler.cpp Euler2Quaternion.cpp

#include <ros/ros.h>
#include <tf/tf.h>
//退出用：ctrl+z
int main(int argc, char** argv){
//初始化
  ros::init(argc, argv, "Euler2Quaternion");
  ros::NodeHandle node;
  geometry_msgs::Quaternion q;
  double roll,pitch,yaw;
  while(ros::ok())
  {
  //输入一个相对原点的位置
  std::cout<<"输入的欧拉角：roll,pitch,yaw:";
  std::cin>>roll>>pitch>>yaw;
  //输入欧拉角，转化成四元数在终端输出
 q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
  //ROS_INFO("输出的四元数为：w=%d,x=%d,y=%d,z=%d"，q.w,q.x,q.y,q.z);
  std::cout<<"输出的四元数为：w="<<q.w<<",x="<<q.x<<",y="<<q.y<<",z="<<q.z<<std::endl;
  ros::spinOnce();
  }
  return 0;
};

Quaternion2Euler.cpp

#include <ros/ros.h>
#include "nav_msgs/Odometry.h"
#include <tf/tf.h>
//退出用：ctrl+z
int main(int argc, char** argv){
//初始化
  ros::init(argc, argv, "Quaternion2Euler");
  ros::NodeHandle node;
  nav_msgs::Odometry position;
  tf::Quaternion RQ2;  
  double roll,pitch,yaw;
  while(ros::ok())
  {
  //输入一个相对原点的位置
  std::cout<<"输入的四元数：w,x,y,z:";
  std::cin>>position.pose.pose.orientation.w>>position.pose.pose.orientation.x>>position.pose.pose.orientation.y>>position.pose.pose.orientation.z;
  //输入四元数，转化成欧拉角数在终端输出
  tf::quaternionMsgToTF(position.pose.pose.orientation,RQ2);  
 // tf::Vector3 m_vector3; 方法2
 // m_vector3=RQ2.getAxis();
  tf::Matrix3x3(RQ2).getRPY(roll,pitch,yaw);  
  std::cout<<"输出的欧拉角为：roll="<<roll<<",pitch="<<pitch<<",yaw="<<yaw<<std::endl;
  //std::cout<<"输出欧拉角为：roll="<<m_vector3[0]<<",pitch="<<m_vector3[1]<<",yaw="<<m_vector3[2]<<std::endl;
  ros::spinOnce();
  }
  return 0;
};    

3.7. TF类

3.8. 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)

这个类在前面讲TF树的时候提到过，这个broadcaster就是一个publisher,而sendTransform的作用是来封装publish的函数。在实际的使用中，我们需要在某个Node中构建tf::TransformBroadcaster类，然后调用sendTransform(),将transform发布到/tf的一段transform上。/tf里的transform为我们重载了多种不同的函数类型。在我们的tf_demo教学包当中提供了相关的示例代码tf.broadcaster.cpp，具体如下：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
//退出用：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="<<qw.w<<",x="<<qw.x<<",y="<<qw.y<<",z="<<qw.z<<std::endl;
  rate.sleep();
  ros::spinOnce();
  }
  return 0;
};

3.9. tf::TransformListener类

void lookupTranform(const std::string &target_frame,const std::string &source_frame,const ros::Time &time,StampedTransform &transform)const
bool canTransform()
bool waitForTransform()const

上一个类是向/tf上发的类，那么这一个就是从/tf上接收的类。首先看lookuptransform()函数，第一个参数是目标坐标系，第二个参数为源坐标系，也即是得到从源坐标系到目标坐标系之间的转换关系，第三个参数为查询时刻，第四个参数为存储转换关系的位置。值得注意，第三个参数通常用ros::Time（0）,这个表示为最新的坐标转换关系，而ros::time::now则会因为收发延迟的原因，而不能正确获取当前最新的坐标转换关系。canTransform()是用来判断两个transform之间是否连通，waitForTransform()const是用来等待某两个transform之间的连通，在我们的tf_demo教学包当中提供了相关的示例代码tf_listerner.cpp，具体如下：

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
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;
};

4. tf in python

4.1. 简介

我们知道tf中不仅有C的接口，也有Python的接口。相比C，tf在Python中的具体实现相对简单好用。

4.2. 数据类型

TF的相关数据类型，向量、点、四元数、矩阵都可以表示成类似数组形式，就是它们都可以用Tuple，List，Numpy, Array来表示。
例如：

t = (1.0,1.5,0) #平移
q = [1,0,0,0] #四元数
m = numpy.identity(3) #旋转矩阵

第一个平移数据使用Tuple表示的，同时也可以用List表示成t=[1.0,1.5,0],也能用numpy.array(1.0,1.5,0)来表示都是可以的。这些数据类型没有特殊对应，全部是通用的，所以这里也就没有了各种数据类型的转换的麻烦。

4.3. 库

4.4. tf.transformations

基本数学运算函数

函数	注释
euler_matrix(ai,aj,ak,axes=‘sxyz’)	欧拉角到矩阵
eulaer_form_matrix(matrix,axes=‘sxyz’)	矩阵到欧拉角
eular_from_quaternion(quaternion,axes=‘sxyz’)	四元数到欧拉角
quaternion_form_euler(ai,aj,ak,axes=‘sxyz’)	欧拉角到四元数
quaternion_matrix(quaternion)	四元数到矩阵
quaternion_form_matrix(matrix)	矩阵到四元数
…	…

使用该函数库时候，首先import tf，tf.transformations给我们提供了一些基本的数学运算函数如上，使用起来非常方便。在tf_demo中教学包当中，我们列举了一些tf.transformations常见的API和示例代码，具详见下表。

4.4.1. 第1部分，定义空间点和空间向量

编号	函数名称	函数功能
1.1	tf.transformations.random_quaternion(rand=None)	返回均匀随机单位四元数
1.2	tf.transformations.random_rotation_matrix(rand=None)	返回均匀随机单位旋转矩阵
1.3	tf.transformations.random_vector(size)	返回均匀随机单位向量
1.4	tf.transformations.translation_matrix(v)	通过向量来求旋转矩阵
1.5	tf.transformations.translation_from_matrix(m)	通过旋转矩阵来求向量

4.4.2. 第2部分，定义四元数

编号	函数名称	函数功能
2.1	tf.transformations.quaternion_about_axis(angle axis)	通过旋转轴和旋转角返回四元数
2.2	tf.transformations.quaternion_conjugate(quaternion)	返回四元数的共轭
2.3	tf.transformations.quaternion_from_euler(ai,aj,ak, axes’ryxz’)	从欧拉角和旋转轴，求四元数
2.4	tf.transformations.quaternion_from_matrix(matrix)	从旋转矩阵中，返回四元数
2.5	tf.transformations.quaternion_multiply(quaternion1,quaternion2)	两个四元数相乘

4.4.3. 第3部分，定义四元数

编号	函数名称	函数功能
3.1	tf.transformations.euler_matrix(ai,aj,ak,axes=‘xyz’)	由欧拉角和旋转轴返回旋转矩阵
3.2	tf.transformations.euler_from_matrix(matrix)	由旋转矩阵和特定的旋转轴返回欧拉角
3.3	tf.transformations.euler_from_quaternion(quaternion)	由四元数和特定的轴得到欧拉角

示例代码：

#!/usr/bin/env python  
# -*- coding:utf-8 -*-  
import rospy  
import math  
import tf  
if __name__ == '__main__':  
    rospy.init_node('py_coordinate_transformation')
#第1部分，定义空间点和空间向量
    print '第1部分，定义空间点和空间向量'
#1.1 返回均匀随机单位四元数
    q=tf.transformations.random_quaternion(rand=None)
    print '定义均匀随机四元数：'
    print q
#1.2 返回均匀随机单位旋转矩阵
    m=tf.transformations.random_rotation_matrix(rand=None)
    print '定义均匀随机单位旋转矩阵：'
    print m
#1.3 返回均匀随机单位向量
    v=tf.transformations.random_vector(3)
    print '定义均匀随机单位向量：'
    print v
#1.4 通过向量来求旋转矩阵
    v_m=tf.transformations.translation_matrix(v)
    print '通过向量来求旋转矩阵：'
    print v_m
#1.5 通过旋转矩阵来求向量
    m_v=tf.transformations.translation_from_matrix(m)
    print '通过旋转矩阵来求向量：'
    print  m_v
#第2部分，定义四元数
    print '第2部分，定义四元数'
#2.1 通过旋转轴和旋转角返回四元数
    axis_q=tf.transformations.quaternion_about_axis(0.123, (1, 0, 0))
    print '通过旋转轴和旋转角返回四元数：'
    print  axis_q
#2.2 返回四元数的共轭
    n_q=tf.transformations.quaternion_conjugate(q)
    print '返回四元数q的共轭：'
    print  n_q
#2.3 从欧拉角和旋转轴，求四元数
    o_q=tf.transformations.quaternion_from_euler(1, 2, 3, 'ryxz')
    print '从欧拉角和旋转轴，求四元数：'
    print  o_q    
#2.4 从旋转矩阵中，返回四元数
    m_q=tf.transformations.quaternion_from_matrix(m)
    print '从旋转矩阵中，返回四元数：'
    print  m_q 
#2.5 两个四元数相乘
    qxq=tf.transformations.quaternion_multiply(q,n_q)
    print '两个四元数相乘'
    print  qxq   
`

4.5. TF类

4.5.1. tf.TransformListener类

方法	作用
canTransform(self,target_frame,source_frame,time)	frame是否相通
waitForTransform(self,target_frame,source_frame,time,timeout)	阻塞直到frame相通
lookup Transform(self,target_frame,source_frame,time)	查看相对的tf，返回（trans，quat）

tf.TransformListener类中主要包含以上三种方法，它的构造函数不需要填值。注意这里的time参数，依然是使用rospy.Time(0)而不是rospy.Time.now().具体原因上节已经介绍，这里不再赘述。除了上述三种重要的方法，这个类中还有一些辅助用的方法如下：

方法	作用
chain(target_frame,target_time,source_frame,source_time,fixed_frame)	frame的连接关系
frameExists(self,frame_id)	frame是否存在
getFrameStrings(self)	返回所有tf的名称
fromTranslationRotation(translation,rotation)	根据平移和旋转返回4X4矩阵
transformPoint(target_frame,point_msg)	将PointStamped消息转换到新frame下
transformPose(target_frame,pose_msg)	将PoseStamped消息转换到新frame下
transformQuaternion(target_frame,quat_msg)	将QuaternionStamped…返回相同类型
…	…

在tf_demo教学包当中的scripts/py_tf_listerner.py给出了示例程序，详见如下。

py_tf_listerner.py

#!/usr/bin/env python  
# -*- coding:utf-8 -*-  
import rospy  
import math  
import tf   
if __name__ == '__main__':  
    rospy.init_node('py_tf_turtle')
    listener = tf.TransformListener() #TransformListener创建后就开始接受tf广播信息，最多可以缓存10s  目前存在的问题，是四个数值的顺序我还有点问题
    rate = rospy.Rate(1.0)  
    #1. 阻塞直到frame相通
    print '1. 阻塞直到frame相通'  
    listener.waitForTransform("/base_link", "/link1", rospy.Time(), rospy.Duration(4.0))
    while not rospy.is_shutdown():  
        try:  
        #2. 监听对应的tf,返回平移和旋转
            print '2. 监听对应的tf,返回平移和旋转'  
            (trans,rot) = listener.lookupTransform('/base_link', '/link1', rospy.Time(0)) #rospy.Time(0)不表示0时刻的tf，而是指最近一帧tf 
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):  
            continue    
        rospy.loginfo('距离原点的位置: x=%f ,y= %f，z=%f \n 旋转四元数: w=%f ,x= %f，y=%f z=%f ',trans[0],trans[1],trans[2],rot[0],rot[1],rot[2],rot[3])
        #3. 判断两个frame是否相通
        print '3. 判断两个frame是否相通'
        if listener.canTransform('/link1','/base_link',rospy.Time(0)) :
            print 'true'
        else :
            print 'false'
        rate.sleep()

4.5.2.

4.5.3. tf.TransformBroadcaster类

类似的，我们介绍的是发布方，tf.TransformBroadcaster类。该类的构造函数也是不需要填值，成员函数有两个如下：

• sendTransform(translation,rotation,time,child,parent)#向/tf发布消息
• sendTransformMessage(transform)#向/tf发布消息

第一个sendTransform()把transform的平移和旋转填好，打上时间戳，然后表示出从父到子的frame流，然后发向/tf的topic。第二种是发送transform已经封装好的Message给/tf，这两种不同的发送方式，功能是一致的。在tf_demo教学包当中的scripts/py_tf_broadcaster.py和scripts/py_tf_broadcaster02.py给出了示例程序，详见如下。

py_tf_broadcaster.py

#!/usr/bin/env python  
# -*- coding:utf-8 -*-  

import rospy  
import math  
import tf    
if __name__ == '__main__':  
    rospy.init_node('py_tf_broadcaster')
    print '讲解tf.transformBroadcaster类'
    print '第1种发布方式：sendTransform(translation,rotation,time,child,parent)'
#第一部分，发布sendTransform(translation,rotation,time,child,parent)
    br = tf.TransformBroadcaster()
#输入相对原点的值和欧拉角
    x=1.0 
    y=2.0
    z=3.0  
    roll=0 
    pitch=0
    yaw=1.57 
    rate = rospy.Rate(1)
    while not rospy.is_shutdown(): 
        yaw=yaw+0.1   
        br.sendTransform((x,y,z),  
                     tf.transformations.quaternion_from_euler(roll,pitch,yaw),  
                     rospy.Time.now(),  
                     "base_link",  
                     "link1")  #发布base_link到link1的平移和翻转   
        rate.sleep()

py_tf_broadcaster02.py

#!/usr/bin/env python  
# -*- coding:utf-8 -*-  

import rospy 
import geometry_msgs.msg
import tf2_ros.transform_broadcaster
import math  
import tf   

if __name__ == '__main__':  
    rospy.init_node('py_tf_broadcaster')
    print '讲解tf.transformBroadcaster类'
    print '第2种发布方式：sendTransformMessage(transform)'
#第二部分，发布sendTransformMessage(transform)
    m=tf.TransformBroadcaster()
    t = geometry_msgs.msg.TransformStamped()
    t.header.frame_id = 'base_link'
    t.header.stamp = rospy.Time(0)
    t.child_frame_id = 'link1'
    t.transform.translation.x = 1
    t.transform.translation.y = 2
    t.transform.translation.z = 3
    t.transform.rotation.w=1
    t.transform.rotation.x=0
    t.transform.rotation.y=0
    t.transform.rotation.z=0
#输入相对原点的值和欧拉角
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        m.sendTransformMessage(t)
        rate.sleep()

4.6. TF相关工具命令

1. 根据当前的tf树创建一个pdf图：

   $ rosrun tf view_frames

   这个工具首先订阅/tf，订阅5秒钟，根据这段时间接受到的tf信息，绘制成一张tf tree，然后创建成一个pdf图。

2. 查看当前的tf树：

   $ rosrun rqt_tf_tree rqt_tf_tree

   该命令同样是查询tf tree的，但是与第一个命令的区别是该命令是动态的查询当前的tf tree,当前的任何变化都能当即看到，例如何时断开何时连接，捕捉到这些然后通过rqt插件显示出来。

3. 查看两个frame之间的变换关系：

   $ rosrun tf tf_echo[reference_frame][target_frame]

5. tf in python

5.1. 简介

我们知道tf中不仅有C的接口，也有Python的接口。相比C，tf在Python中的具体实现相对简单好用。

5.2. 数据类型

TF的相关数据类型，向量、点、四元数、矩阵都可以表示成类似数组形式，就是它们都可以用Tuple，List，Numpy Array来表示。
例如：

t = (1.0,1.5,0) #平移
q = [1,0,0,0] #四元数
m = numpy.identity(3) #旋转矩阵

第一个平移数据使用Tuple表示的，同时也可以用List表示成t=[1.0,1.5,0],也能用numpy.array(1.0,1.5,0)来表示都是可以的。这些数据类型没有特殊对应，全部是通用的，所以这里也就没有了各种数据类型的转换的麻烦。

5.3. TF库

5.3.1. tf.transformations

基本数学运算函数

函数	注释
euler_matrix(ai,aj,ak,axes=‘sxyz’)	欧拉角到矩阵
eulaer_form_matrix(matrix,axes=‘sxyz’)	矩阵到欧拉角
eular_from_quaternion(quaternion,axes=‘sxyz’)	四元数到欧拉角
quaternion_form_euler(ai,aj,ak,axes=‘sxyz’)	欧拉角到四元数
quaternion_matrix(quaternion)	四元数到矩阵
quaternion_form_matrix(matrix)	矩阵到四元数
…	…

使用该函数库时候，首先import tf，tf.transformations给我们提供了一些基本的数学运算函数如上，使用起来非常方便。在tf_demo中教学包当中，我们列举了一些tf.transformations常见的API和示例代码，具详见下表。

5.3.2. 第1部分，定义空间点和空间向量

编号	函数名称	函数功能
1.1	tf.transformations.random_quaternion(rand=None)	返回均匀随机单位四元数
1.2	tf.transformations.random_rotation_matrix(rand=None)	返回均匀随机单位旋转矩阵
1.3	tf.transformations.random_vector(size)	返回均匀随机单位向量
1.4	tf.transformations.translation_matrix(v)	通过向量来求旋转矩阵
1.5	tf.transformations.translation_from_matrix(m)	通过旋转矩阵来求向量

5.3.3. 第2部分，定义四元数

编号	函数名称	函数功能
2.1	tf.transformations.quaternion_about_axis(angle axis)	通过旋转轴和旋转角返回四元数
2.2	tf.transformations.quaternion_conjugate(quaternion)	返回四元数的共轭
2.3	tf.transformations.quaternion_from_euler(ai,aj,ak, axes’ryxz’)	从欧拉角和旋转轴，求四元数
2.4	tf.transformations.quaternion_from_matrix(matrix)	从旋转矩阵中，返回四元数
2.5	tf.transformations.quaternion_multiply(quaternion1,quaternion2)	两个四元数相乘

5.3.4. 第3部分，定义四元数

编号	函数名称	函数功能
3.1	tf.transformations.euler_matrix(ai,aj,ak,axes=‘xyz’)	由欧拉角和旋转轴返回旋转矩阵
3.2	tf.transformations.euler_from_matrix(matrix)	由旋转矩阵和特定的旋转轴返回欧拉角
3.3	tf.transformations.euler_from_quaternion(quaternion)	由四元数和特定的轴得到欧拉角

示例代码：

#!/usr/bin/env python  
# -*- coding:utf-8 -*-  
import rospy  
import math  
import tf  
if __name__ == '__main__':  
    rospy.init_node('py_coordinate_transformation')
#第1部分，定义空间点和空间向量
    print '第1部分，定义空间点和空间向量'
#1.1 返回均匀随机单位四元数
    q=tf.transformations.random_quaternion(rand=None)
    print '定义均匀随机四元数：'
    print q
#1.2 返回均匀随机单位旋转矩阵
    m=tf.transformations.random_rotation_matrix(rand=None)
    print '定义均匀随机单位旋转矩阵：'
    print m
#1.3 返回均匀随机单位向量
    v=tf.transformations.random_vector(3)
    print '定义均匀随机单位向量：'
    print v
#1.4 通过向量来求旋转矩阵
    v_m=tf.transformations.translation_matrix(v)
    print '通过向量来求旋转矩阵：'
    print v_m
#1.5 通过旋转矩阵来求向量
    m_v=tf.transformations.translation_from_matrix(m)
    print '通过旋转矩阵来求向量：'
    print  m_v
#第2部分，定义四元数
    print '第2部分，定义四元数'
#2.1 通过旋转轴和旋转角返回四元数
    axis_q=tf.transformations.quaternion_about_axis(0.123, (1, 0, 0))
    print '通过旋转轴和旋转角返回四元数：'
    print  axis_q
#2.2 返回四元数的共轭
    n_q=tf.transformations.quaternion_conjugate(q)
    print '返回四元数q的共轭：'
    print  n_q
#2.3 从欧拉角和旋转轴，求四元数
    o_q=tf.transformations.quaternion_from_euler(1, 2, 3, 'ryxz')
    print '从欧拉角和旋转轴，求四元数：'
    print  o_q    
#2.4 从旋转矩阵中，返回四元数
    m_q=tf.transformations.quaternion_from_matrix(m)
    print '从旋转矩阵中，返回四元数：'
    print  m_q 
#2.5 两个四元数相乘
    qxq=tf.transformations.quaternion_multiply(q,n_q)
    print '两个四元数相乘'
    print  qxq   
`

5.4. TF类

5.4.1. tf.TransformListener类

方法	作用
canTransform(self,target_frame,source_frame,time)	frame是否相通
waitForTransform(self,target_frame,source_frame,time,timeout)	阻塞直到frame相通
lookup Transform(self,target_frame,source_frame,time)	查看相对的tf，返回（trans，quat）

tf.TransformListener类中主要包含以上三种方法，它的构造函数不需要填值。注意这里的time参数，依然是使用rospy.Time(0)而不是rospy.Time.now().具体原因上节已经介绍，这里不再赘述。除了上述三种重要的方法，这个类中还有一些辅助用的方法如下：

方法	作用
chain(target_frame,target_time,source_frame,source_time,fixed_frame)	frame的连接关系
frameExists(self,frame_id)	frame是否存在
getFrameStrings(self)	返回所有tf的名称
fromTranslationRotation(translation,rotation)	根据平移和旋转返回4X4矩阵
transformPoint(target_frame,point_msg)	将PointStamped消息转换到新frame下
transformPose(target_frame,pose_msg)	将PoseStamped消息转换到新frame下
transformQuaternion(target_frame,quat_msg)	将QuaternionStamped…返回相同类型
…	…

在tf_demo教学包当中的scripts/py_tf_listerner.py给出了示例程序，详见如下。

py_tf_listerner.py

#!/usr/bin/env python  
# -*- coding:utf-8 -*-  
import rospy  
import math  
import tf   
if __name__ == '__main__':  
    rospy.init_node('py_tf_turtle')
    listener = tf.TransformListener() #TransformListener创建后就开始接受tf广播信息，最多可以缓存10s  目前存在的问题，是四个数值的顺序我还有点问题
    rate = rospy.Rate(1.0)  
    #1. 阻塞直到frame相通
    print '1. 阻塞直到frame相通'  
    listener.waitForTransform("/base_link", "/link1", rospy.Time(), rospy.Duration(4.0))
    while not rospy.is_shutdown():  
        try:  
        #2. 监听对应的tf,返回平移和旋转
            print '2. 监听对应的tf,返回平移和旋转'  
            (trans,rot) = listener.lookupTransform('/base_link', '/link1', rospy.Time(0)) #rospy.Time(0)不表示0时刻的tf，而是指最近一帧tf 
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):  
            continue    
        rospy.loginfo('距离原点的位置: x=%f ,y= %f，z=%f \n 旋转四元数: w=%f ,x= %f，y=%f z=%f ',trans[0],trans[1],trans[2],rot[0],rot[1],rot[2],rot[3])
        #3. 判断两个frame是否相通
        print '3. 判断两个frame是否相通'
        if listener.canTransform('/link1','/base_link',rospy.Time(0)) :
            print 'true'
        else :
            print 'false'
        rate.sleep()

5.4.2.

5.4.3. tf.TransformBroadcaster类

类似的，我们介绍的是发布方，tf.TransformBroadcaster类。该类的构造函数也是不需要填值，成员函数有两个如下：

• sendTransform(translation,rotation,time,child,parent)#向/tf发布消息
• sendTransformMessage(transform)#向/tf发布消息

第一个sendTransform()把transform的平移和旋转填好，打上时间戳，然后表示出从父到子的frame流，然后发向/tf的topic。第二种是发送transform已经封装好的Message给/tf，这两种不同的发送方式，功能是一致的。在tf_demo教学包当中的scripts/py_tf_broadcaster.py和scripts/py_tf_broadcaster02.py给出了示例程序，详见如下。

py_tf_broadcaster.py

#!/usr/bin/env python  
# -*- coding:utf-8 -*-  

import rospy  
import math  
import tf    
if __name__ == '__main__':  
    rospy.init_node('py_tf_broadcaster')
    print '讲解tf.transformBroadcaster类'
    print '第1种发布方式：sendTransform(translation,rotation,time,child,parent)'
#第一部分，发布sendTransform(translation,rotation,time,child,parent)
    br = tf.TransformBroadcaster()
#输入相对原点的值和欧拉角
    x=1.0 
    y=2.0
    z=3.0  
    roll=0 
    pitch=0
    yaw=1.57 
    rate = rospy.Rate(1)
    while not rospy.is_shutdown(): 
        yaw=yaw+0.1   
        br.sendTransform((x,y,z),  
                     tf.transformations.quaternion_from_euler(roll,pitch,yaw),  
                     rospy.Time.now(),  
                     "base_link",  
                     "link1")  #发布base_link到link1的平移和翻转   
        rate.sleep()

py_tf_broadcaster02.py

#!/usr/bin/env python  
# -*- coding:utf-8 -*-  

import rospy 
import geometry_msgs.msg
import tf2_ros.transform_broadcaster
import math  
import tf   

if __name__ == '__main__':  
    rospy.init_node('py_tf_broadcaster')
    print '讲解tf.transformBroadcaster类'
    print '第2种发布方式：sendTransformMessage(transform)'
#第二部分，发布sendTransformMessage(transform)
    m=tf.TransformBroadcaster()
    t = geometry_msgs.msg.TransformStamped()
    t.header.frame_id = 'base_link'
    t.header.stamp = rospy.Time(0)
    t.child_frame_id = 'link1'
    t.transform.translation.x = 1
    t.transform.translation.y = 2
    t.transform.translation.z = 3
    t.transform.rotation.w=1
    t.transform.rotation.x=0
    t.transform.rotation.y=0
    t.transform.rotation.z=0
#输入相对原点的值和欧拉角
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        m.sendTransformMessage(t)
        rate.sleep()

5.5. TF相关工具命令

1. 根据当前的tf树创建一个pdf图：

   $ rosrun tf view_frames

   这个工具首先订阅/tf，订阅5秒钟，根据这段时间接受到的tf信息，绘制成一张tf tree，然后创建成一个pdf图。

2. 查看当前的tf树：

   $ rosrun rqt_tf_tree rqt_tf_tree

   该命令同样是查询tf tree的，但是与第一个命令的区别是该命令是动态的查询当前的tf tree,当前的任何变化都能当即看到，例如何时断开何时连接，捕捉到这些然后通过rqt插件显示出来。

3. 查看两个frame之间的变换关系：

   $ rosrun tf tf_echo[reference_frame][target_frame]