ROS.tf3編寫一個tf接收器（C++）

在之前的教程中，我們建立了一個tf廣播公司向tf釋出一隻烏龜的姿勢。
在本教程中，我們將建立一個tf接收器來開始使用tf。

1.1如何建立一個tf接收器我們先建立原始檔。
轉到我們在上一個教程中建立的包：

 $ roscd learning_tf

1.2啟動您的編輯器並將以下程式碼貼上到名為src
/ 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; 

 ros::service::waitForService("spawn");

 ros::ServiceClient add_turtle =
node.serviceClient<turtlesim::Spawn>("spawn"); 

 turtlesim::Spawn srv; 

 add_turtle.call(srv); 

 ros::Publisher turtle_vel =
node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel",10);

 tf::TransformListener listener; 

 ros::Rate rate(10.0); 

 while(node.ok()) 

 { 

  tf::StampedTransform transform; 

  try 

  { 

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

  } 

  catch(tf::TransformException &ex) 

  { 

   ROS_ERROR("%s",ex.what()); 

   ros::Duration(1.0).sleep(); 

   continue; 

  } 

  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; 

}

如果在執行時出現錯誤“Lookup
would require extrapolation into the past”，則可以嘗試使用此替代程式碼來呼叫偵聽器：

try {

   
listener.waitForTransform(destination_frame, original_frame,
ros::Time(0), ros::Duration(10.0) );

   
listener.lookupTransform(destination_frame, original_frame,
ros::Time(0), transform);

} catch
(tf::TransformException ex) {

   
ROS_ERROR("%s",ex.what());

}

1.3程式碼解析

（1）
 #include <tf/transform_listener.h>

tf軟體包提供了一個TransformListener的實現來幫助簡化接收轉換的任務。
要使用TransformListener，我們需要包含tf
/ transform_listener.h標頭檔案。
（2）
tf::TransformListener listener;

在這裡，我們建立一個TransformListener物件。一旦建立了監聽器，它就開始接收通過線路進行的tf轉換，並緩衝它們長達10秒。TransformListener物件的範圍應該保持不變，否則它的快取將無法填充，幾乎每個查詢都會失敗。一種常見的方法是使TransformListener物件成為類的成員變數。

（3）try

     {

      listener.lookupTransform("/turtle2", "/turtle1",

      ros::Time(0), transform);

     }

在這裡，真正的工作完成了，我們向偵聽器查詢特定的轉換。我們來看看這四個引數：我們希望從frame/ turtle1到frame/ turtle2的轉換。我們想要改變的時間。提供ros ::Time（0）只會為我們提供最新的可用轉換。我們儲存結果變換的物件。

（4）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));

在這裡，變換用於計算turtle2的新的線性和角速度，基於它與turtle1的距離和角度。新速度釋出在主題“turtle2/ cmd_vel”中，sim將使用它來更新turtle2的移動。

2.1執行偵聽器

現在我們建立了程式碼，讓我們先編譯它。開啟CMakeLists.txt檔案，並在底部新增以下行：

add_executable(turtle_tf_listener
src/turtle_tf_listener.cpp)

target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

進入您的工作空間編譯

 $ catkin_make

如果一切順利，你的devel/ lib /learning_tf資料夾中應該有一個名為turtle_tf_listener的二進位制檔案。如果是這樣，我們已經準備好新增這個演示的啟動檔案。在您的文字編輯器中，開啟名為start_demo.launch的啟動檔案，然後在<launch>塊內合併下面的節點塊：

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

首先，確保你停止了前一篇教程中的啟動檔案（使用ctrl-c）。現在您已準備好開始您的完整海龜演示：

 $ roslaunch learning_tf start_demo.launch

要想看看事情是否有效，只需使用箭頭鍵在第一隻烏龜周圍駕駛（確保您的終端視窗處於活動狀態，而不是您的模擬器視窗），並且您會在第一隻烏龜之後看到第一隻烏龜！當turtlesim啟動時，您可能會看到：[ERROR]
1253915565.300572000: Frame id /turtle2 does not exist! When trying
to transform between /turtle1 and /turtle2.

[ERROR]
1253915565.401172000: Frame id /turtle2 does not exist! When trying
to transform between /turtle1 and /turtle2.

發生這種情況是因為我們的聽眾在收到關於龜2的訊息之前正在計算變換，因為它需要一點時間才能在turtlesim中產卵並開始廣播一個tf幀。現在您已準備好轉到下一個教程，您將在其中學習如何新增框架（C++）