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。

使用時需要在內部聲明：輸入（即所需座標變換），時間戳，框架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>

在命令行中執行launch文件之後得到效果圖如下

此時我們再用rqt_graph查看節點信息，發現和使用tf功能包時基本一致

至此，我們完成了tf的基本編程學習內容。

參考

TF 在ROS基本作用和簡單介紹
 ROS探索總結（十八）——重讀tf
http://wiki.ros.org/tf
http://docs.ros.org/jade/api/tf/html/c++/namespacetf.html
http://wiki.ros.org/roscpp/Overview/Time
ROS-TF庫-TF和ros::Time(0)
ROS機器人的tf變換

轉載請註明來源信息

ROS學習筆記：TF座標變換及編程詳解