TF介绍（三）

tf in python

tf中有C++接口，也有Python接口，tf在Python中的具体实现相对比较简单。

数据类型：

TF的相关数据类型，向量、点、四元数。矩阵的=都可以表示成类似于数组的形式（Tuple、List、Numpy Array表示）。
如：

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

t平移数据用Tuple表示的，也可以用List表示（t = [1.0,1.5,1.0]），也能用numpy.array(1.0,1.5,0)表示。数据类型没有特殊的对应，都是通用的，也就没有数据类型的转换。

TF库

tf.trabsformations

基本数学运算函数:

函数

注释

euler_matrix(ai,aj,ak,axes=‘sxyz’)

欧拉角到矩阵

euler_from_matrix(matrix,axes=‘sxyz’)

矩阵到欧拉角

euler_quaternion(quaternion,axes=‘sxyz’)

四元数到欧拉角

quaternion_from_euler(ai,aj,ak,axes=‘sxyz’)

欧拉角到四元数

quaternion_matrix(quaternion)

四元数到矩阵

quaternion_from_matrix()

矩阵到四元数

…

…

在使用函数库的时候，首先要import tf，tf.transformations给我们提供了基本的数学运算函数，使用较方便。

1. 定义空间点和空间向量

函数名称

函数功能

tf.transformations.random_quaternion(rand=None)

返回均匀随机单位四元数

tf.transformations.random_rotation_matrix(rand=None)

返回均匀随机单位旋转矩阵

tf.transformations.random_vector(size)

返回均匀随机单位向量

tf.transformations.translation_matrix(v)

通过向量来求旋转矩阵

tf.transformations.translation_from_matrix(m)

通过旋转矩阵来求向量

2. 定义四元数

函数名称

函数功能

tf.transformations.quaternion_about_axis(angle axis)

通过旋转轴和旋转角返回四元数

tf.transformations.quaternion_conjugate(quaternion)

返回四元数的共轭

tf.transformations.quaternion_from_euler()

从欧拉角和旋转轴，求四元数

tf.transformations_from_matrix(matrix)

从旋转矩阵中，返回四元数

tf.transformations.quaternion_multiply(quaternion1,quaternion2)

两个四元数相乘

文件位置：test/scripts/py_coordinate_transformation.py

#!/usr/bin/env python
#coding=utf-8

import rospy
import tf
import math

if __name__ == '__main__':
	rospy.init_node("py_coordinate_transformation")

	print '第1部分，定义空间点和空间向量'
	q = tf.transformations.random_quaternion(rand=None)
    print '定义均匀随机四元数：'
    print q

	m = tf.transformations.random_rotation_matrix(rand=None)
	print '定义均匀随机单位旋转矩阵：'
	print m
	
	v = tf.transformations.rand_vector(3)
	print '定义均匀随机单位向量：'
	print v
	
	v_m = tf.transformation.translation_matrix(v)
	print '通过向量来求旋转矩阵：'
	print v_m

	m_v = tf.transformation.translation_from_matrix(m)
	print '通过旋转矩阵来求向量：'
	print m_v
	
	print '第2部分，定义四元数'
	axis_q = tf.transformations.quaternion_about_axis(0.123,(1,0,0))
	print '通过旋转轴和旋转角返回四元数：'
	print axis_q
	
	n_q = tf.transformations.quaternion_conjugate(q)
	print '返回四元数q的共轭：'
	print n_q
	
	m_q = tf.transformations.quaternion_from_matrix(m)
	print '从旋转矩阵中，返回四元数：'
	print m_q

	qxq = tf.transformations.quaternion_multiply(q,n_q)
	print '两个四元数相乘'
	print  qxq

TF类

tf.transformListener类

方法

作用

canTransform(self,target_frame,source_frame,time)

frame是否相通

waitForTransform(self,target_frame,source_target,time,timeout)

阻塞直到frame相通

lookupTransform(self,target_frame,source_frame,time)

查看相对的tf，返回（trans，quat）

time参数使用rospy.Time(0)，不是rospy::time::now()

方法

作用

chain(target_frame,source_frame,source_time,fixed_frame)

frame的连接关系

frameExists(self,frame_id)

frame是否存在

getFrameString(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…返回相同类型

…

…

文件位置：test/scripts/py_tf_listerner.py

#!/usr/bin/env python
#coding=utf-8

import ros
import tf
import math

if __name__ == '__main__':
	rospy.init_node("py_tf_listener")
	listener = tf.transdormListener()	#TransformListener创建后就开始接收tf广播信息，最多可以缓存10s
	rate = rospy.Rate(1.0)
	
	print '1. 阻塞直到frame相通'  
	listener.waitForTransform('/base_link','/link1',rospy.Time(0)，rospy.Duration(4))	#rospy.Time(0)不表示0时刻的tf，是指最近的一帧
	while not rospy.is_shutdown():
		try:
			 print '2. 监听对应的tf,返回平移和旋转'  
			 (trans,rot) = listener.lookupTransform('/base_link','/link1',rospy.Time(0))
		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])
		print '3. 判断两个frame是否相通'
		if listener.canTransform('/link1','base_link',rospy.Time(0)):
			print 'true'
		else:
			print 'false'
		rate.sleep()

tf.TransformBroadcaster类

成员函数有两个：

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

sendTransform()参数为平移、旋转、时间戳，从父到子的frame流，再发向/tf的topic。
sendTransformMessage(),是发送transform已经封装好了的Message给/tf。
这两种不同的发送方式功能一致。

文件位置：test/scripts/py_tf_broadcaster.py

#!/usr/bin/env python
#coding=utf-8

import rospy
import tf
import math

if __name__ == '__main__':
	rospy.init_node('py_tf_broadcaster')
	 print 'tf.transformBroadcaster类'
	 print '第1种发布方式：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 += 0.1
     	br.sendTransform((x,y,z),tf.transformations.quaternion_from_euler(roll,pitch,yaw),rospy.Time(0),'base_link','link1')
     	rate.sleep()

文件位置：test/scripts/py_tf_broadcaster02.py

#!/usr/bin/env python
#coding=utf-8

import rospy
import geometry_msgs.msg
import tf
import math

if __name__ == '__main__':
	rospy.init_node('py_tf_broadcaster')
	print 'tf.transformBroadcaster类'
	print '第2种发布方式：sendTransformMessage(transform)'
	br = 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 rtospy.is_shutdown():
		br.sendTransformMessage(t)
		rate.sleep()

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]