在之前的教程中，我們建立了一個tf釋出者，用於將龜的姿勢釋出到tf。

在本教程中，我們將建立一個tf監聽器以開始使用tf。

1 如何建立一個tf監聽者

我們先建立原始檔。

轉到我們在上一個教程中建立的包：

$ roscd learning_tf

1.1 程式

啟動您喜歡的編輯器並將以下程式碼貼上到名為src / turtle_tf_listener.cpp的新檔案中。

https://raw.github.com/ros/geometry_tutorials/groovy-devel/turtle_tf/src/turtle_tf_listener.cpp

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <turtlesim/Velocity.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<turtlesim::Velocity>("turtle2/command_velocity", 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();
    }

    turtlesim::Velocity vel_msg;
    vel_msg.angular = 4.0 * atan2(transform.getOrigin().y(),
                                transform.getOrigin().x());
    vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                pow(transform.getOrigin().y(), 2));
    turtle_vel.publish(vel_msg);

    rate.sleep();
  }
  return 0;
};
 

注意上面的程式碼對於完成tf教程是必不可少的，但由於某些訊息和主題的命名稍有變化，它不會在ROS Hydro上編譯。

所述turtlesim / Velocity.h頭不再使用，它已取代geometry_msgs / Twist.h。

此外，主題/ turtle / command_velocity現在稱為/ turtle / cmd_vel。鑑於此，需要進行一些更改才能使其工作：

https://raw.github.com/ros/geometry_tutorials/hydro-devel/turtle_tf/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 this alternative code to call the listener:

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.2 程式解釋

現在，讓我們來看看與將turtle 位姿釋出到tf相關的程式碼。

#include <tf/transform_listener.h>

tf包提供了TransformListener的實現，以幫助簡化接收轉換的任務。

要使用TransformListener，我們需要包含tf / transform_listener.h標頭檔案。

  tf::TransformListener listener;

在這裡，我們建立一個TransformListener物件

一旦建立了監聽器，它就開始接收座標轉換資訊，它們緩衝最多10秒鐘。

The TransformListener object should be scoped to persist otherwise it's cache will be unable to fill and almost every query will fail.一種常見的方法是使TransformListener物件成為類的成員變數。

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

在這裡，真正的工作已經完成，我們向偵聽器查詢特定的轉換。我們來看看這四個引數：

1. 我們想要從frame / turtle1到frame / turtle2的轉換。

2. 我們想要轉變的時間。提供ros :: Time（0）將為我們提供最新的變換。

3. 我們儲存結果轉換的物件。

補充函式：

可能的異常講被try catch捕獲。

    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) +

這裡，基於它與turtle1的距離和角度，變換用於計算turtle2的新線性和角速度。

新的速度釋出在主題“turtle2 / cmd_vel”中，sim將使用它來更新turtle2的移動。

2 執行監聽器

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

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

在catkin工作區的頂層資料夾中構建包：

  $ 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

你應該看到有兩隻烏龜的烏龜。

3 檢查結果

要檢視是否有效，只需使用箭頭鍵驅動第一隻烏龜（確保終端視窗處於活動狀態，而不是模擬器視窗），然後您將看到第二隻烏龜跟隨第一隻烏龜！

當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座標。

現在，您已準備好繼續學習下一個教程，在那裡您將學習如何新增框架（Python） （C ++）

放個圖片：