ROS学习笔记—— TF & URDF
所有资料均来自于 https://www.icourse163.org/learn/ISCAS-1002580008#/learn/announce 和https://github.com/DroidAITech/ROS-Academy-for-Beginners 和 https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/
TF：坐标系变换(TransForm)，坐标系数据维护的工具（位置+姿态）。
URDF：统一机器人描述格式，是ROS指定的描述机器人的规范，定义机器人的具体模型。
ROS中的tf （https://wiki.ros.org/tf/Tutorials/Introduction%20to%20tf）
坐标转换的标准(tf tree)、话题(/tf 这个topic里的msg保存的就是tf tree的数据结构)、工具(pkg)、接口(roscpp rospy的API)
ROS中机器人模型包含大量的部件，这些部件统称之为link,每一个link上面对应着一个frame, 即一个坐标系．link和frame概念是绑定在一起的．robot_sim_demo运行的tf tree
TransformStamped.msg
ROS中两个frame之间的数据格式，用来表示两个坐标系之间的关系
包括：
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
tf/tfMessage.msg
tf2_msgs/TFMessage.msg
tf tree的数据结构 描述一个tf tree之间的关系
这里TF的数据类型有两个，主要的原因是版本的迭代,如何查看自己使用的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 wtf in C++
（https://docs.ros.org/api/tf/html/c++/namespacetf.html）
TF相关数据类型
| 名称 | 数据类型 |
|---|---|
| 向量 | tf::Vector3 |
| 点 | tf::Point |
| 四元数 | tf::Quaternion |
| 3*3矩阵（旋转矩阵） | tf::Matrix3x3 |
| 位姿 | tf::pose |
| 变换 | tf::Transform |
| 带时间戳的以上类型 | tf::Stamped |
| 带时间戳的变换 | tf::StampedTransform |
关于数据相关转换的代码在tf_demo中的coordinate_transformation.cpp当中：
#include
#include
//退出用：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部分，定义空间点和空间向量"< TF类
tf::TransformBroadcaster类 （将publisher的函数进行封装）
使用方法：在某个node中构建这个类，然后调用sendTransform（），就可以把一个transform发布到一个/tf上transform上
transformBroadcaster()
void sendTransform(const StampedTransform &transform) #发送单个的tf::StampedTransform
void sendTransform(const std::vector &transforms)
#发送一个StampedTransform数组
void sendTransform(const geometry_msgs::TransformStamped &transform)
#把TransformStamped的消息发送出去
void sendTransform(const std::vector &transforms) tf::TransformListener类
void lookupTranform(const std::string &target_frame,
const std::string &source_frame,
const ros::Time &time,
StampedTransform &transform) const
#第一个参数 目标坐标系 第二个参数 源坐标系 （得到从源坐标系到目标坐标系之间的转换关系）将这个转换关系放到第四个参数中 第三个参数 定义查询时刻 （常用ros::Time(0)表示最新的坐标转换关系）
bool canTransform() #判断两个Transform之间是否联通
bool waitForTransform() const #等待某两个frame之间的Transform联通tf in python
(https://docs.ros.org/api/tf/html/python/tf_python.html#)
TF相关数据类型
向量、点、四元数、矩阵都表示为类似数组的形式，Tuple，List，Numpy Array通用
rospy里有tf的库（import 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) | 矩阵到四元数 |
代码示例：
#!/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.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） |
PS：函数里的 time 要用 rospy.Time(0) 而不能用 rospy.Time.now() 因为 tf 的传输有延迟
除了上述三种重要的方法，这个类中还有一些辅助用的方法如下：
| 方法 | 作用 |
|---|---|
| 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.TransformBroadcaster类
类似的，我们介绍的是发布方，tf.TransformBroadcaster类。该类的构造函数也是不需要填值，成员函数有两个如下：
- sendTransform(translation,rotation,time,child,parent) #向/tf发布消息
把Transform的平移(translation)和旋转(rotation)填好，打上时间戳(time),表示从父farme(parent frame)到子frame(child frame)，发送到 /tf 的topic上。
- sendTransformMessage(transform) #向/tf发布消息
把transform massage发送出去，把已经封装好的massage发送到 /tf 上。
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]URDF( Unified Robot Description Format ) 统一机器人描述格式 （http://wiki.ros.org/urdf/XML）
为了描述机器人中的零部件以及零部件之间的关系，在URDF格式中定义了两部分，link 和 joint 。link指的是机器人的零部件，每个link都有三个轴（xyz），joint是连接两个link之间的关节，joint 和 tf 是对应的。
具体的构建方法见https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/chapter8/8.5.html 中的8.5.2章节
例程序见tf_follower
建模见 ROS-Academy-for-Beginners/tf_follower/mybot_description/urdf/mybot.xacro
link和joint相关属性
模型见urdf_demo/urdf 中的urdf 文件。