ROS.tf2.編寫一個tf廣播器（C++）

宣告：本教程來自於ROS.WIKI，本人在整理過程中可能出現一些錯誤和問題，有問題可以去檢視官網版本也可以諮詢本人

1.1在接下來的兩篇教程中，我們將編寫程式碼來重現tf入門教程中的演示。之後，以下教程將重點介紹使用更高階的tf功能擴充套件演示。在我們開始之前，您需要為此專案建立一個新的ros包。在沙箱資料夾中，建立一個名為learning_tf的包，該包依賴於tf，roscpp，rospy和turtlesim：

 $ cd %YOUR_CATKIN_WORKSPACE_HOME%/src

 $ catkin_create_pkg learning_tf tf roscpp rospy turtlesim

1.2建立新的軟體包，然後才能使用roscd：

$ cd %YOUR_CATKIN_WORKSPACE_HOME%/

$ catkin_make

$ source./devel/setup.bash

2.1如何廣播轉換本教程將告訴您如何向tf廣播座標幀。在這種情況下，我們希望在移動時播放海龜變化的座標系。我們先建立原始檔。轉到我們剛剛建立的包：

轉到src/資料夾並啟動您的編輯器，將以下程式碼貼上到名為src/ 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)
{
  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];
  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose",10, &poseCallback);
  ros::spin();
  return 0;
}

2.2程式碼解釋

現在，我們對程式碼的關鍵部分進行解釋。

(1).#include
<tf/transform_broadcaster.h>
tf軟體包提供了TransformBroadcaster的實現，以幫助簡化釋出轉換的任務。要使用TransformBroadcaster，我們需要包含tf
/ transform_broadcaster.h標頭檔案。

(2).static
tf::TransformBroadcaster br;
在這裡，我們建立一個TransformBroadcaster物件，稍後我們將使用它來通過電線傳送變換。

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

在這裡，我們建立一個Transform物件，並將2D海龜姿勢的資訊複製到3D變換中。

(4).transform.setRotation（q）;
這裡我們設定旋轉。

(5).br.sendTransform（tf:: StampedTransform（transform，ros:: Time :: now（），“world”，turtle_name））;

這是真正的工作完成的地方。使用TransformBroadcaster傳送變換需要四個引數。

首先，我們通過轉換本身。

現在我們需要給釋出的變換一個時間戳，我們會用當前時間戳ros:: Time :: now（）

然後，我們需要傳遞我們建立的連結的父框架的名稱，“世界”

最後，我們需要傳遞我們建立的連結的子框架的名稱，這是烏龜本身的名稱

注意：sendTransform和StampedTransform與父母和子女的順序相反。

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

add_executable(turtle_tf_broadcastersrc/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster${catkin_LIBRARIES}

進入你的的工作包

$ catkin_make

如果一切順利，您的devel/ lib /learning_tf資料夾中應該有一個名為turtle_tf_broadcaster的二進位制檔案。如果是這樣，我們準備為這個演示建立一個啟動檔案。

進入learning_tf建立一個launch資料夾

$ mkdir launch

使用文字編輯器建立一個名為start_demo.launch的新檔案

$ vim start_demo.launch

然後新增以下行：

  <launch>

  <!-- Turtlesim Node-->

  <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
  <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
  <!-- Axes -->
  <param name="scale_linear" value="2" type="double"/>
  <param name="scale_angular" value="2" type="double"/>
  <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" />
  </launch>

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

 $ roslaunch learning_tf start_demo.launch

你應該看到一隻烏龜。

檢查結果

現在，使用tf_echo工具來檢查烏龜姿勢是否實際上正在廣播到tf：

$ rosrun tf tf_echo /world /turtle1

這應該告訴你第一隻烏龜的姿勢。使用箭頭鍵在烏龜周圍驅動（確保您的終端視窗處於活動狀態，而不是您的模擬器視窗）。如果你執行tf_echo來進行世界和海龜2之間的轉換，你不應該看到轉換，因為第二隻海龜還沒有。但是，只要我們在下一個教程中新增第二隻烏龜，烏龜2的姿勢就會廣播給tf。要實際使用廣播到tf的轉換，您應該轉到下一個關於建立tf偵聽器的教程（C+