第八章 TF与URDF

本章简介

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

首先我们从认识TF开始，然后学习TF消息和TF树，在后面我们还介绍了TF的数据类型和在C++以及Python中的一些函数和类。也简单介绍了统一机器人描述格式URDF．学习了TF和URDF，我们才开始真正的深入认识ROS。

8.1 认识TF

8.1.1 简介

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

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

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

8.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就会发布相关消息．　

8.2 TF消息

8.2.1 TransformStamped.msg

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

8.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树。

8.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即可。

8.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。

8.3 tf in c++

8.3.1 简介

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

8.3.2 数据类型

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

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

8.3.3 数据转换

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

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

示例代码：

#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;
}

第2部分定义四元数

示例代码：

#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部分定义旋转矩阵

示例代码：

#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;
};    

8.3.4 TF类

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;
};

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;
};

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

基本数学运算函数

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

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

第2部分，定义四元数

第3部分，定义四元数

示例代码：

#!/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类

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

在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]

8.5 统一机器人描述格式URDF

8.5.1 URDF基础

URDF（Unified Robot Description Format）统一机器人描述格式，URDF使用XML格式描述机器人文件。URDF语法规范，参考链接：http://wiki.ros.org/urdf/XML，URDF组件，是由不同的功能包和组件组成：

其中urdf_parser和urder_interface已经在hydro之后的版本中去除了。urdf_paser_plugin是URDF基础的插件，衍生出了urdfdom（面向URDF文件）和collar_parser（面向相互文件）。在URDF当中，当你想要描述一个机器人的时候，例如小车的base_link和右轮right，两个link之间需要joint来连接。参考下图：

8.5.2 制作URDF模型

（1）添加基本模型

我们以构建一个小车为例子，为大家讲解这部分的内容:（相关的示例代码可以从我们的tf_demo中找到），我们的想法是，首先构建base_link作为小车的父坐标系，然后在base_link基础上，再构建左轮，右轮 和雷达 link. 最后不同的link之间通过joint来连接。参考代码如下：

小技巧： sudo apt-get install liburdfdom-tools，安装完毕后，执行检查check_urdf my_car.urdf如果一切正常，就会有如下显示：

随后打开新的终端，输入roslaunch urdf_demo display_urdf_link_joint.launch,回车之后,发现所有的link和joint都在一起了，详见urdf本章的demo。

（2）添加机器人link之间的相对位置关系

在基础模型之上，我们需要为机器人之间link来设相对位置和朝向的关系，URDF中通过<origin>来描述这种关系。

随后打开新的终端，输入roslaunch urdf_demo display_urdf_link_position.launch,回车之后,发现所有的link和joint已经有在固定的位置上了，详见urdf本章的demo。

（3）添加模型的尺寸，形状和颜色等

在已经设置好模型的link基础上，添加模型的形状（例如圆柱或长方体），相对于link的位置，颜色等。其中形状用<geometry>来描述，颜色用<color>来描述。

（4）显示URDF模型

想要在rviz中显示出我们制作好的小车的URDF模型，可以写一个launch文件，参考如下：

除了launch文件中的前3句，导入我们制作小车的URDF模型外，还需要添加joint_state_publisher和robot_state_publisjer这两个节点。效果如下：

另外，我们可以输入rosrun rqt_tf_tree rqt_tf_tree,可以看到以下tf 树：

小技巧：你可以将launch文件中的param name="use_gui"的值由false改成true会弹出一个窗口，同一移动进度条，可以临时改变joint的朝向。

8.5.3 制作xacro模型

什么是Xacro? 我们可以把它理解成为针对URDF的扩展性和配置性而设计的宏语言(macro language)。有了Xacro，我们就可以像编程一样来写URDF文件。XACRO格式提供了一些更高级的方式来组织编辑机器人描述. 主要提供了三种方式来使得整个描述文件变得简单。

（1）Constants

Usage：<xacro:property name="WIDTH" value="2.0"/>

类似于C语言中的宏定义, 在头部定义后就可以${body_width}进行引用其数值，有了这个，至少我们可以把需要配置的变量进行统一管理和使用。

（2）Macros

Usage：<xacro:macro name="default_origin">
    <origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:macro>
<xacro:default_origin />

Macros是xacro文件中最重要的部分. 就像宏函数一样, 完成一些最小模块的定义, 方便重用, 以及可以使用参数来标识不同的部分.

（3）Include

很多模型都是已宏的形式进行定义, 并以最小集团分成很多个文件. 而最终的机器人描述就变得非常简单了. 下面摘录一个ur5的描述文件. 从中可以看出来xacro的强大优势. 在最后的示例中我们还能够看到, urdf文件也是能够直接导入进来的.

Usage：<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="ur5" >

  <!-- common stuff -->
  <xacro:include filename="$(find ur_description)/urdf/ur5/common.gazebo.xacro" />

  <!-- ur5 -->
  <xacro:include filename="$(find ur_description)/urdf/ur5/ur5.urdf.xacro" />

  <!-- arm -->
  <xacro:ur5_robot prefix="" joint_limited="false"/>

  <link name="world" />

  <joint name="world_joint" type="fixed">
    <parent link="world" />
    <child link = "base_link" />
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
  </joint>

</robot>

include类似于C语言中的include, 先将该文件扩展到包含的位置. 但包含进来的文件很有可能只是一个参数宏的定义. 并没有被调用.

举例说明打开新的终端，输入roslaunch urdf_demo display_xacro.launch,回车之后,发现所有的link和joint已经有在固定的位置上了，并且小车颜色和形状已经固定完成，详见urdf本章的demo。

8.5.4 制作gazebo模型

在已经制作好的xcaro模型的基础上，添加gazebo模型的组建，看起已经变得十分的具有可操作性。对于二轮差动模型通过添加libgazebo_ros_diff_drive.so插件对小车左右轮的控制。

<gazebo>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<robotNamespace>/</robotNamespace>
<alwaysOn>true</alwaysOn>
<legacyMode>false</legacyMode>
<updateRate>50</updateRate>
<leftJoint>mybot_left_wheel_hinge</leftJoint>
<rightJoint>mybot_right_wheel_hinge</rightJoint>
<wheelSeparation>${chassisWidth+wheelWidth}</wheelSeparation>
<wheelDiameter>${2*wheelRadius}</wheelDiameter>
<torque>20</torque>
<commandTopic>mybot_cmd_vel</commandTopic>
<odometryTopic>mybot_odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<robotBaseFrame>mybot_link</robotBaseFrame>
</plugin>
</gazebo>

通过添加libgazebo_ros_p3d.so来计算里程。

<gazebo>
<plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
<frameName>map</frameName>
<bodyName>mybot_chassis</bodyName>
<topicName>odom</topicName>
<updateRate>30.0</updateRate>
</plugin>
</gazebo>

最后，对gazebo模型中小车左右轮相关PID等参数进行设置

<gazebo reference="mybot_chassis">
  <material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="caster_wheel">
  <mu1>0.0</mu1>
  <mu2>0.0</mu2>
  <material>Gazebo/Red</material>
</gazebo>
<gazebo reference="right_wheel">
  <mu1 value="1.0"/>
  <mu2 value="1.0"/>
  <kp  value="10000000.0" />
  <kd  value="1.0" />
  <fdir1 value="1 0 0"/>
  <material>Gazebo/Black</material>
</gazebo>
<gazebo reference="left_wheel">
  <mu1 value="1.0"/>
  <mu2 value="1.0"/>
  <kp  value="10000000.0" />
  <kd  value="1.0" />
  <fdir1 value="1 0 0"/>
  <material>Gazebo/Black</material>
</gazebo>

</robot>

举例说明打开新的终端，输入roslaunch urdf_demo display_gazebo_rviz.launch,回车之后,发现所有的link和joint已经有在固定的位置上了，并且小车颜色和形状已经固定完成,gazebo界面同样显示正常，详见urdf本章的demo。