ros 学习（3）
参考/转载：https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/chapter8/
1 TF与URDF
1.1简介
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来处理，维护着各个坐标系之间的联通．如下图：
每一个圆圈代表一个frame,对应着机器人上的一个link，任意的两个frame之间都必须是联通的，如果出现某一环节的断裂，就会引发error系统报错．所以完整的tf tree不能有任何断层的地方，这样我们才能查清楚任意两个frame之间的关系．仔细观察上图，我们发现每两个frame之间都有一个broadcaster,这就是为了使得两个frame之间能够正确连通，中间都会有一个Node来发布消息来broadcaster.如果缺少Node来发布消息维护连通，那么这两个frame之间的连接就会断掉．broadcaster就是一个publisher,如果两个frame之间发生了相对运动，broadcaster就会发布相关消息．
1.2 TF消息
每个frame之间都会有broadcaster来发布消息维系坐标转换，这个消息TransformStampde.msg,它就是处理两个frame之间一小段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 wTF tree是由很多的frame之间TF拼接而成，那么TF tree是什么类型呢？如下：
- tf/tfMessage.msg
- tf2_msgs/TFMessage.msg
（官网建议新工作直接使用tf2，因为它有一个更清洁的界面，和更好的使用体验。如何查看自己使用的TF是哪一个版本，使用命令rostopic info /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。
1.3 TF的Python接口
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
`TF类
tf.TransformListener类
TransformListener extends TransformerROS, adding a handler for ROS topic /tf_message, so that the messages update the transformer.
| 方法 | 作用 |
|---|---|
| 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） |
* canTransform(target_frame, source_frame, time)
Returns True if the Transformer can determine the transform from source_frame to target_frame at the time time. time is a rospy.Time instance.
* canTransformFull(target_frame, target_time, source_frame, source_time, fixed_frame)
It's an extended version of canTransform.
* chain(target_frame, target_time, source_frame, source_time, fixed_frame) -> list
returns the chain of frames connecting source_frame to target_frame.
* frameExists(frame_id) -> Bool
returns True if frame frame_id exists in the Transformer`.
* getFrameStrings -> list
returns all frame names in the Transformer as a list.
* getLatestCommonTime(source_frame, target_frame) -> time
Determines that most recent time for which Transformer can compute the transform between the two given frames, that is, between source_frame and target_frame. Returns a rospy.Time.
* lookupTransform(target_frame, source_frame, time) -> position, quaternion
Returns the transform from source_frame to target_frame at the time time. time is a rospy.Time instance. The transform is returned as position (x, y, z) and an orientation quaternion (x, y, z, w).
* lookupTransformFull(target_frame, target_time, source_frame, source_time, fixed_frame)
It is the full version of lookupTransform.
TransformerROS extends the base class Transformer, adding methods for handling ROS messages.
* asMatrix(target_frame, hdr) -> matrix
Looks up the transform for ROS message header hdr to frame target_frame, and returns the transform as a NumPy 4x4 matrix.
* fromTranslationRotation(translation, rotation) -> matrix
Returns a NumPy 4x4 matrix for a transform.
* transformPoint(target_frame, point_msg) -> point_msg
Transforms a geometry_msgs's PointStamped message, i.e. point_msg, to the frame target_frame, returns the resulting PointStamped.
* transformPose(target_frame, pose_msg) -> pose_msg
Transforms a geometry_msgs's PoseStamped message, i.e. point_msg, to the frame target_frame, returns the resulting PoseStamped.
* transformQuaternion(target_frame, quat_msg) -> quat_msg
Transforms a geometry_msgs's QuaternionStamped message, i.e. quat_msg, to the frame target_frame, returns the resulting QuaternionStamped.
This example uses a TransformListener to find the current base_link position in the map：
import rospy
import tf
from tf import TransformListener
import time
class myNode:
def __init__(self, *args):
rospy.init_node('py_tf')
self.tf = TransformListener()
def some_method(self):
time.sleep(1) # 必须要给一定的反冲时间，否则为all=self.tf.getFrameStrings()空
All_Frame = TransformListener().getFrameStrings()
print(All_Frame)
if self.tf.frameExists("thorvald_001/corner0_1") and self.tf.frameExists("thorvald_001/pipe1_1"):
print 'ok'
t = self.tf.getLatestCommonTime("thorvald_001/corner0_1", "thorvald_001/pipe1_1")
position, quaternion = self.tf.lookupTransform("thorvald_001/corner0_1", "thorvald_001/pipe1_1", t)
print position, quaternion
if __name__ == '__main__':
try:
L = myNode()
L.some_method()
except rospy.ROSInterruptException:
pass
A simple example to transform a pose from one frame to another（根据上面的程序进行补充和修改）：
import rospy
import tf
class myNode:
def __init__(self, *args):
self.tf_listener_ = TransformListener()
def example_function(self):
if self.tf.frameExists("/base_link") and self.tf.frameExists("/fingertip"):
t = self.tf_listener_.getLatestCommonTime("/base_link", "/fingertip")
p1 = geometry_msgs.msg.PoseStamped()
p1.header.frame_id = "fingertip"
p1.pose.orientation.w = 1.0 # Neutral orientation
p_in_base = self.tf_listener_.transformPose("/base_link", p1)
print "Position of the fingertip in the robot base:"
print p_in_base
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()tf包还包括流行的transformations.py模块。TransformROS使用transformations.py(geometry/transformations.py at hydro-devel · ros/geometry · GitHub)在四元数和矩阵之间执行转换。transformations.py对NumPy矩阵进行有用的转换；它可以在欧拉角、四元数和矩阵等变换之间进行转换。
1.4 TF相关工具命令
-
根据当前的tf树创建一个pdf图：
$ rosrun tf view_frames这个工具首先订阅
/tf，订阅5秒钟，根据这段时间接受到的tf信息，绘制成一张tf tree，然后创建成一个pdf图。 -
查看当前的tf树：
$ rosrun rqt_tf_tree rqt_tf_tree该命令同样是查询tf tree的，但是与第一个命令的区别是该命令是动态的查询当前的tf tree,当前的任何变化都能当即看到，例如何时断开何时连接，捕捉到这些然后通过rqt插件显示出来。
-
查看两个frame之间的变换关系：
$ rosrun tf tf_echo [reference_frame][target_frame]