# 8.4 tf in python
### 8.4.1 简介
我们知道tf中不仅有C++的接口，也有Python的接口。相比C++，tf在Python中的具体实现相对简单好用。
### 8.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\)来表示都是可以的。这些数据类型没有特殊对应，全部是通用的，所以这里也就没有了各种数据类型的转换的麻烦。
### 8.4.3 TF库
##### 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和示例代码，具详见下表。
###### 第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\) | 通过旋转矩阵来求向量 |
###### 第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\) | 两个四元数相乘 |
###### 第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
`
### 8.4.4 TF类
##### 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()
```
#####
##### 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()
```
### 8.4.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]
```
- 前言
- 第一章 ROS简介
- 机器人时代的到来
- ROS发展历程
- 什么是ROS
- 安装ROS
- 安装ROS-Academy-for-Beginners教学包
- 二进制与源码包
- 安装RoboWare Studio
- 单元测试一
- 第二章 ROS文件系统
- Catkin编译系统
- Catkin工作空间
- Package软件包
- CMakeLists.txt
- package.xml
- Metapacakge软件元包
- 其他常见文件类型
- 单元测试二
- 第三章 ROS通信架构（一）
- Node & Master
- Launch文件
- Topic
- Msg
- 常见msg类型
- 单元测试三
- 第四章 ROS通信架构（二）
- Service
- Srv
- Parameter server
- Action
- 常见srv类型
- 常见action类型
- 单元测试四
- 第五章 常用工具
- Gazebo
- RViz
- Rqt
- Rosbag
- Rosbridge
- moveit!
- 单元测试五
- 第六章 roscpp
- Client Library与roscpp
- 节点初始、关闭与NodeHandle
- Topic in roscpp
- Service in roscpp
- Param in roscpp
- 时钟
- 日志与异常
- 第七章 rospy
- Rospy与主要接口
- Topic in rospy
- Service in rospy
- Param与Time
- 第八章 TF与URDF
- 认识TF
- TF消息
- tf in c++
- tf in python
- 统一机器人描述格式
- 附录：TF数学基础
- 三维空间刚体运动---旋转矩阵
- 三维空间刚体运动---欧拉角
- 三维空间刚体运动---四元数
- 第九章 SLAM
- 地图
- Gmapping
- Karto
- Hector
- 第十章 Navigation
- Navigation Stack
- move_base
- costmap
- Map_server & Amcl
- 附录：Navigation工具包说明
- amcl
- local_base_planner
- carrot_planner
- clear_costmap_recovery
- costmap_2d
- dwa_local_planner
- fake_localization
- global_planner
- map_server
- move_base_msg
- move_base
- move_slow_and_clear
- navfn
- nav_core
- robot_pose_ekf
- rotate_recovery