~歡迎關注~
微信公衆号：古月居
新浪微網誌：古月春旭
知乎專欄：古月居
原文連結：ROS技術點滴 —— 海龜例程中的tf
先學知識： ROS技術點滴 —— tf坐标變換庫
代碼例程： https:// github.com/huchunxu/ros _exploring
本篇我們在海龜仿真器中，通過一個例程（turtle_tf）來了解TF的作用，并且熟悉之前學到的TF工具。該例程的功能包turtle_tf可以使用如下指令進行安裝：
$ sudo apt-get install ros-kinetic-turtle-tf
安裝完成後，就可以使用如下指令運作例程了：
$ roslaunch turtle_tf turtle_tf_demo.launch
海龜仿真器打開後會出現兩隻小海龜，并且
下方的小海龜自動向中心位置的小海龜移動。
打開鍵盤控制節點，控制中心位置的小海龜運作：
$ rosrun turtlesim turtle_teleop_key
另外一隻海龜總是會跟随我們控制的那隻海龜運作。在這個例程中，TF是如何運用的呢？我們首先使用TF工具來看一下這個例程中的TF樹是什麼樣的：
$ rosrun tf view_frames
在目前系統中存在三個坐标系：world、turtle1、turtle2。world是世界坐标系，作為系統的基礎坐标系，其他坐标系都相對該坐标系建立，是以world是TF樹的根節點。相對于world坐标系，又分别針對兩隻海龜建立了兩個海龜坐标系，這兩個坐标系的原點就是海龜在世界坐标系下的坐标位置。
現在要讓turtle2跟随turtle1運動，相當于
turtle2坐标系向turtle1坐标系移動，這就需要知道turtle2與turtle1之間的坐标變換。三個坐标系之間的變換關系可以使用如下公式描述：
使用tf_echo工具在TF樹中查找海龜坐标系之間的變換關系：
$ rosrun tf tf_echoturtle1 turtle2
也可以通過rviz的圖形界面更加形象的看到這三者之間的坐标關系：
$ rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz
得到turtle2與turtle1之間的坐标變換後，就可以計算兩隻海龜間的
距離和角度，即可控制turtle2向turtle1移動了。
接下來，我們以這個例程為目标，學習如何實作TF的廣播和監聽功能。
一、建立TF廣播器
首先，我們需要建立一個釋出海龜坐标系與世界坐标系之間TF變換的節點，實作源碼turtle_tf_broadcaster.cpp的具體内容如下：
#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;
// 根據海龜目前的位姿，設定相對于世界坐标系的坐标變換
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);
// 釋出坐标變換
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv)
{
// 初始化節點
ros::init(argc, argv, "my_tf_broadcaster");
if (argc != 2)
{
ROS_ERROR("need turtle name as argument");
return -1;
};
turtle_name = argv[1];
// 訂閱海龜的pose資訊
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
};
以上代碼的關鍵部分是處理海龜pose消息的回調函數poseCallback。在廣播TF消息之前需要定義tf::TransformBroadcaster廣播器，然後根據海龜目前的位姿，設定tf::Transform類型的坐标變換，包含setOrigin設定的
平移變換以及setRotation設定的
旋轉變換。
然後使用廣播器将坐标變換插入TF樹并且釋出，這裡釋出的TF消息類型是tf::StampedTransform，不僅包含tf::Transform類型的
坐标變換、時間戳，而且還需要指定坐标變換的
源坐标系（parent）和
目标坐标系（child）。
二、建立TF監聽器
TF消息廣播之後，其他節點就可以監聽該TF消息，進而擷取需要的坐标變換了。
目前我們已經将海龜相對于world坐标系的TF變換廣播，接下來需要監聽TF消息，并從中擷取turtle2相對于turtle1坐标系的變換，進而控制turtle2移動。實作源碼turtle_tf_listener.cpp的詳細内容如下：
#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 node;
// 通過服務調用，産生第二隻烏龜turtle2
ros::service::waitForService("spawn");
ros::ServiceClient add_turtle =
node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
// 定義turtle2的速度控制釋出器
ros::Publisher turtle_vel =
node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
// tf監聽器
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.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;
}
// 根據turtle1和turtle2之間的坐标變換，計算turtle2需要運動的線速度和角速度
// 并釋出速度控制指令，使turtle2向turtle1移動
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;
};
該節點首先通過
服務調用産生海龜turtle2，然後聲明控制turtle2速度的Publisher。在監聽TF消息之前，需要建立一個tf::TransformListener類型的監聽器，建立成功後監聽器會自動接收TF樹的消息，并且緩存10秒。
然後在循環中就可以實時查找TF樹中的坐标變換了，這裡需要調用的是tf::TransformListener中的兩個接口：
waitForTransform(const std::string &target_frame, const std::string&source_frame, const ros::Time&time, const ros::Duration &timeout)
給定的源坐标系（source_frame）和目标坐标系（target_frame），等待兩個坐标系之間指定時間（time）的變換關系，該函數會
阻塞程式運作，是以需要設定逾時時間（timeout）。
lookupTransform(const std::string & target_frame,const std::string &source_frame, const ros::Time & time,StampedTransform & transform）
給定的源坐标系（source_frame）和目标坐标系（target_frame），得到兩個坐标系之間指定時間（time）的坐标變換（transform），ros::Time(0) 表示我們想要的是最新一次的坐标變換。
通過以上兩個接口的調用，就可以擷取turtle2相對于turtle1的坐标變換了。然後根據坐标系之間的位置關系，計算得到turtle2需要運動的
線速度和角速度，并釋出速度控制指令，使turtle2向turtle1移動。
三、實作海龜跟随運動
現在小海龜跟随例程的所有代碼都已經完成，我們來編寫一個launch檔案，将所有節點運作起來，start_demo_with_listener.launch：
<launch>
<!-- 海龜仿真器 -->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- 鍵盤控制 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- 兩隻海龜的tf廣播 -->
<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" />
<!-- 監聽tf廣播，并且控制turtle2移動 -->
<node pkg="learning_tf" type="turtle_tf_listener"
name="listener" />
</launch>
運作該launch檔案，就可以看到與之前例程類似的兩隻海龜的界面了，在終端中通過鍵盤控制turtle1移動，turtle2也跟随移動。
通過這個例程的實作，我們學習了TF廣播與監聽的實作方法，在實際應用中會産生更多坐标系，TF樹的結構也會更加複雜，但是基本的使用方法依然相同。
更多内容歡迎關注：
微信公衆号：
古月居(guyue_home)
新浪微網誌：古月春旭
知乎專欄：古月居
或通路
古月居網站：
古月居 - 怕什麼真理無窮，進一寸有一寸的歡喜www.guyuehome.com