ROS学习笔记：TF坐标变换及编程详解

1. 前言及TF简介

TF是什么呢？

tf是一个让用户随时间跟踪多个参考系的功能包，它使用一种树型数据结构，根据时间缓冲并维护多个参考系之间的坐标变换关系，可以帮助用户在任意时间，将点、向量等数据的坐标，在两个参考系中完成坐标变换。

简单来说，我们知道在机器人工作过程中，机器人自身处于世界坐标系下，同时其自身又具备了一个局部坐标系，这两个坐标系的存在确保了我们能够计算得出机器人的位姿。

但是对于机器人本体来说，其内部同样存在着很多需要确定位姿的部分，而它们在不同时刻所处的状态或许也是不同的，如果以底部框架作为基座标，那么我们又该如何确定不同时刻机器人内部如激光雷达、抓手、关节、头部的位姿呢？最好的方法就是再在这些东西上建立各自的坐标系，并且通过坐标变换得到它们的位姿。

而TF的作用就是帮助你实现系统中任一个点在所有坐标系之间的坐标变换，任意给定系统中一个点的坐标，TF能够帮你换算得出该点在其他坐标系下的坐标。
在这里插入图片描述

2. TF编程实例详解

那么我们该怎么应用TF呢？这里先以ROS官方给出的实例作为演示。

官方TF教程

首先我们先安装TF功能包

sudo apt-get install ros-melodic-turtle-tf

执行以下命令

roslaunch turtle_tf turtle_tf_demo.launch
rosrun turtlesim turtle_teleop_key

通过键盘控制初始化在中心的海龟进行运动，可以看到另一只海龟自动跟随。
在这里插入图片描述
该过程主要通过turtle1和turtle2之间的坐标变换实现。我们可以通过

rosrun tf view_frames	//创建一个TF监听器监听5s，得到TF结构关系图
rqt_graph				//查看节点信息

得到系统中所运行的所有坐标系的关系结构图以及节点信息图
在这里插入图片描述

2.1 常用数据类型

参考ROS Wiki上面给出的TF数据类型，主要有以下几种基本类型，分别对应四元数，向量，点坐标，位姿和转换模板。
在这里插入图片描述
除此之外，还包括 tf::Stamped，官方源码如下，意思就是这个数据类型是在上述所有基本类型（除了tf::Transform）的基础上具有元素frame_id_和stamp_模板化

同时还有tf::StampedTransform，该类型是tf::Transform的一个特例，同时要求具有frame_id，stamp 和 child_frame_id。
在这里插入图片描述
总共6种数据类型。

2.2 TF使用方法

在这里插入图片描述
当我们使用TF包时，需要编写两个程序，分别用来执行监听TF变换和广播TF变换的功能，我们称它们为TF监听器和TF广播器。

TF监听器：监听TF变换，接收并缓存系统中发布的所有参考系变换，并从中查询所需要的参考系变换。
TF广播器：广播TF变换，向系统中广播参考系之间的坐标变换关系。系统中可能会存在多个不同部分的tf变换广播，但每个广播都可以直接将参考系变换关系直接插入tf树中，不需要再进行同步。

下面我们以官方的教程为例，该程序实现了上面给出的海龟跟随的效果。

2.3 TF广播器

实现功能：创建TF广播器，创建坐标变换值并实时发布坐标变换

编程思路：

初始化ROS节点，并订阅 turtle 的位置消息；
循环等待话题消息，接收到之后进入回调函数，该回调函数用以处理并发布坐标变换；
在该回调函数内部定义一个广播器；
根据接收到的小海龟的位置消息，创建坐标变换值
通过定义的广播器发布坐标变换

以下是具体实现的代码部分

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void PoseCallBack(const turtlesim::PoseConstPtr& msg)
{
    
    
	//定义一个tf广播器
	static tf::TransformBroadcaster br;
	
	//根据turtle的位置信息，得到其相对于世界坐标系的变换
	tf::Transform transform;
	transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
	tf::Quaternion q;
	q.setRPY(0,0,msg->theta);
	transform.setRotation(q);
	
	//tf广播器发布2只海龟相对于世界坐标系的坐标变换
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv)
{
    
    
	//初始化节点
	ros::init(argc, argv, "my_tf_broadcaster");
	
	//判断并且获取turtle_name
	if(argc != 2)
	{
    
    
		ROS_ERROR("Need a turtle name!");
		return -1;
	}
	turtle_name = argv[1];
	
	//订阅turtle的pose信息
	ros::NodeHandle n;
	ros::Subscriber sub = n.subscribe(turtle_name + "/pose", 10, &PoseCallBack);
	
	ros::spin();
	
	return 0;
}

代码解释：

已经学到tf包这里的同学应该对ROS内部消息的发布和订阅已经不陌生了，我这里就主要说一下回调函数内跟tf变换相关的代码内容。对于一些对C++还不是很熟悉的同学，关于argc和argv的意思详情看这里：main函数的参数argc和argv

在具体分析之前我们先来看一下 “transform_broadcaster.h” 里面的内容

#ifndef TF_TRANSFORMBROADCASTER_H
#define TF_TRANSFORMBROADCASTER_H
 
#include "tf/tf.h"
#include "tf/tfMessage.h"
 
#include <tf2_ros/transform_broadcaster.h>
 
namespace tf
{
    
    
class TransformBroadcaster{
    
    
public:
  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);
 
private:
 
  tf2_ros::TransformBroadcaster tf2_broadcaster_;
 
};
 
}
 
#endif //TF_TRANSFORMBROADCASTER_H

注意到在命名空间tf内部定义了一个TransformBroadcaster类，这个类的内部的内容也很简单：

声明了一个无参构造函数TransformBroadcaster()；
使用了函数重载的方法定义了多个同名函数sendTransform；
声明了一个私有化的成员变量tf2_broadcaster_

基于此内容，我们再将回调函数代码的层次分为：

（1）定义tf广播器

static tf::TransformBroadcaster br;

tf::TransformBroadcaster有一个无参构造函数，因此初始化时直接调用默认构造函数声明一个tf广播器br。

（2）创建坐标变换

根据tf内部的数据类型，我们首先声明一个“Transform”数据结构，用来记录变换内容

tf::Transform transform;

针对该内容，我们需要确定变换的姿态，即位置和方向。我们查找Transform这个类里面的函数：http://docs.ros.org/melodic/api/tf/html/c++/classtf_1_1Transform.html

以下两个函数可以实现我们的需求。内部参数各自要求为“Vector3”和“Quaternion”
在这里插入图片描述
进一步进行查找（有兴趣的同学自己去官网细看哈，这里就不再赘述啦），发现Vector3类型直接声明即可使用，Quaternion类型需要先使用setRPY这个函数进行赋值。关于RPY，详情请看：无人机的偏航角，滚动角，俯仰角解释
在这里插入图片描述
由此，我们可以得到

transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
transform.setRotation(tf::Quaternion::setRPY(0, 0, msg->theta));

进一步简化得到

tf::Transform transform;
transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
tf::Quaternion q;
q.setRPY(0,0,msg->theta);
transform.setRotation(q);

（3）广播器发布坐标变换

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

根据前面所解释的 “transform_broadcaster.h” 里面的内容应该不难理解
在这里插入图片描述
内部参数类型为“StampedTransform”，下图为继承关系图，也就是说StampedTransform这个类里面的内容继承自Transform。
tf::StampedTransform的继承图
使用时需要在内部声明：输入（即所需坐标变换），时间戳，框架id和子框架id。
在这里插入图片描述

2.4 TF监听器

实现功能：创建TF监听器，创建第二只海龟，监听坐标变换并发布运动控制指令使第二只海龟向第一只海龟运动

编程思路：

初始化ROS节点，并向MASTER注册节点信息；
通过服务调用产生第二只海龟；
创建turtle2的速度控制发布器；
创建tf监听器并监听turtle2相对于turtle1的坐标变换；
根据坐标变换发布速度控制指令；

以下是具体实现的代码部分

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
    
    
	//初始化节点
	ros::init(argc, argv, "my_tf_listener");
	ros::NodeHandle n;
	
	//service调用产生第二只海龟
	ros::service::waitForService("spawn");
	ros::ServiceClient add_turtle = n.serviceClient<turtlesim::Spawn>("spawn");
	turtlesim::Spawn srv;
	add_turtle.call(srv);
	
	//定义速度控制指令的消息发布者
	ros::Publisher turtle_vel = n.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
	
	//定义一个tf监听器
	tf::TransformListener listener;
	
	ros::Rate rate(10.0);
	
	while(ros::ok())
	{
    
    
		//申明一个空的坐标变换
		tf::StampedTransform transform;
		try
		{
    
    
			//监听turtle2和turtle1的坐标变换
			listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
		}
		catch(tf::TransformException &ex)
		{
    
    
			ROS_ERROR("%s", ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}
		//根据坐标变换计算得出turtle2的角速度和线速度，并发布该消息
		geometry_msgs::Twist vel_msg;
		vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
		vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(),2) + pow(transform.getOrigin().y(),2));
		
		turtle_vel.publish(vel_msg);
		
		rate.sleep();
	}

	return 0;
}

这边还是主要讲一下tf的部分，这里就不放 “transform_listener.h” 的内容了，因为太多啦~大家可以自己去查一下官方文档。

相对的，大家也可以看看wiki上关于listener的使用方法：http://wiki.ros.org/tf/Overview/Using%20Published%20Transforms

（1）定义一个tf监听器

首先需要明确，TransformListener的内容继承自Transformer类（注意这里不是Transform类哦），因此在使用时需要同时查看两者的使用文档，当然最简单的还是看wiki上面关于listener的使用。
在这里插入图片描述
内部构造和析构函数如下（截取自头文件）：

在大多数情况下，使用以下命令声明即可

tf::TransformListener listener

（2）监听坐标变换

首先声明一个空的变换

tf::StampedTransform transform;

那么这里为什么使用 tf::StampedTransform 而不是 tf::Transform 来进行声明呢？我们仔细回想一下当初 tf广播器发布消息时所用的数据类型就知道啦~

尝试 try

listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);

Wiki解释如下：

① tf :: TransformListener :: waitForTransform函数，测试是否可以在时间time将source_frame转换为target_frame，并且返回一个bool类型值表明是否可以进行变换。
在这里插入图片描述
② tf :: TransformListener :: lookupTransform函数，返回两个坐标系之间的变换，返回的转换方向为从target_frame到source_frame。

（3）出错了怎么办？

所有tf异常的类继承自tf::TransformException，而它自身又继承自std::runtime_error。
在这里插入图片描述

那么以下代码的内容就是当catch到错误信息的时候，用what()提取这个错误的基本信息，并打印该错误信息，然后系统挂起一段时间之后，继续循环。

ROS_ERROR("%s", ex.what());
ros::Duration(1.0).sleep();
continue;

3 实现效果

完成程序的编写之后我们在CMakeLists文件添加
在这里插入图片描述
并配置Launch文件

<launch>
	<node pkg="turtlesim" type="turtlesim_node" name="sim"/>

	<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen" />

	<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
	<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />

	<node pkg="learning_tf" type="turtle_tf_listener" name="listener" />

</launch>