創建TF廣播和監聽

#include <ros/ros.h> #include <tf/transform_broadcaster.h> #include <turtlesim/Pose.h> 需要調用pose話題std::string turtle_name; 全局變量用于保存命令行或節點參數，注意小寫，這里是C++定義字符串，之前話題是消息類型。void poseCallback(const turtlesim::PoseConstPtr& msg) 對應頭文件pose消息可以不加&。 PoseConstPtr烏龜程序專用。不加const則&也不能加。 {// tf廣播器static tf::TransformBroadcaster br;// 根據烏龜當前的位姿，設置相對于世界坐標系的坐標變換tf::Transform transform; 用于儲存相對位置transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); Vector3是TF中 定義的數據類型xyztf::Quaternion q;q.setRPY(0, 0, msg->theta);transform.setRotation(q);

注意修改格式，原程序中平移和旋轉采用了兩種格式。上圖格式互換。control+點擊看類的定義，針對定義的格式來編寫，roboware自帶查看成員方便。除去源代碼的方式還有：

方法1 tf::Vector3 a； 方法2 tf::Vector3 a(msg->x,msg->y,0.0);；

a.setX(msg->x); transform.setOrigin( a );

transform.setOrigin( a );

C++小括號賦值只能在定義的時候（類的對象，構造函數，只能在聲明的時候對對象進行初始化賦值,如果不在聲明的時候賦值，則只能像方法1，對象調用方法賦值），不能在調用的時候。比如上面transform后面a就是調用。transform.setOrigin( a.setX(msg->x) );是錯誤的

方法3 transform.setRotation(tf::Quaternion(0, 0, msg->theta));

方法4 tf::Quaternion q(0, 0, msg->theta);

transform.setRotation(q);

同理，都是在聲明的時候初始化，如果沒有初始化，則只能對q調用setRPY時候賦值。方法3如同平移源代碼是tf::Quaternion類省略定義對象構造函數直接對類賦值（C++可以這樣寫），然后前面setRotation方法調用剛才類中輸出的變量。省略RPY是因為看到Quaternion中自動識別輸入參數個數，3為RPY,4為四元數。

例如，以下函數具有一個接收 Rectangle 對象的形參： void displayRectangle(Rectangle r) {cout << "Length = " << r.getLength() << endl;cout << "Width = " << r.getWidth() << endl;cout << "Area = " << r.getArea() << endl; ' }

以下代碼創建了一個長度為 15 和寬度為 10 的 Rectangle 對象 box，然后將 box 對象作為參數傳遞給了 displayRectangle 函數：
Rectangle box(15, 10);displayRectangle(box);

則該函數將輸出以下信息：Length = 15 。。。。。

類 class A

方式1 A a（賦值） 構造函數，a傳給函數參數

方式2 A（賦值），省略對象，無法直接傳給函數做參數，需要如下圖定義個中

間變量a或者直接在函數中使用這種方式類名非對象傳參，如下圖和程序中的

vector3等。

方式3 A b b.c(賦值) 不使用構造函數賦值，則只能通過對象調用方法賦值

// 發布坐標變換，

br.sendTransform

(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

}

br.sendTransform，廣播把當前now()時刻坐標變化插入TF樹并進行發布，發布消息類型為tf::StampedTransform，其中包括坐標變換，時間戳，源坐標，目標坐標turtle_name同時定義了目標坐標系名字。

注意前面是這個類StampedTransform儲存著誰到誰的變換信息，相對數據儲存在基類transform（在前面定義tf::Transform transform;）中。然后通過tf::TransformBroadcaster類中的函數sendTransform（）發送到tf中。

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; 等效1};turtle_name = argv[1]; 終端或launch args獲取變量名// 訂閱烏龜的pose信息ros::NodeHandle node;ros::Subscriber sub = node.subscribe(turtle_name+"/pose",10, &poseCallback); 訂閱話題名，調用回 調函數，&可不加函數作為參數，turtle_name+"/pose"相當于/turtle1/pose與前面話題訂閱類似，注意，所有訂閱沒有指明消息類型，發布有。因為訂閱在回掉函數中定義了與發布話題消息有關的消息類型。ros::spin();return 0; };TF監聽器，TF消息廣播后其他節點就可以監聽TF消息，從而獲取需要的坐標變換。獲取turtle2相對于turtle1的變換從而控制turtle2移動。#include <ros/ros.h> #include <tf/transform_listener.h> #include <geometry_msgs/Twist.h> 需要調用twist話題 #include <turtlesim/Spawn.h> 需要調用spawn服務int main(int argc, char** argv) {// 初始化節點ros::init(argc, argv, "my_tf_listener");ros::NodeHandle node;// 通過服務調用，產生第二只烏龜turtle2 ros::service::waitForService("spawn"); 等待，直到服務"spawn"出現，在launch中下面啟動烏龜節點。

。 只有這樣才能請求此服務再產生一個烏龜。如果刪waitForService,則還沒產生spawn 服務就可能直接進行下面的程序，無法獲取turtle2數據waitForService是靜態函數 只能::調用，不能對象調用。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樹信息tf::TransformListener listener;//循環頻率一秒10次循環。ros::Rate rate(10.0); while (node.ok()) 可以換成ros::ok(){tf::StampedTransform transform; 消息類型try try-catch結構，捕捉異常，當try里面語句異常時，執行catch{// 開始在TF樹中查找turtle2與turtle1的坐標變換listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0)); waitForTransform()函數,返回bool值，評估變換是否有效,基本API： bool tf::TransformListener::waitForTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const source_frame target_frame在指定時間time等待坐標變換關系，所以waitForTransform()實際上會阻塞直到兩個海龜之間的變換可用（這通常需要幾毫秒）或者如果變換不可用,直到達到超時。所以設置 ros::Duration &timeout。Duration(3.0)：為waitForTransform()函數的結束條件：最多等待3秒，如果提前得到了坐標的轉換信息，直接結束等待。 listener.lookupTransform("/turtle2", "/turtle1",ros::Time(0), transform); lookupTransform()是一個更底層的方法用于返回兩個坐標系的變換。 這個方法是 tf庫的核心方法。大部分transform的方法都是終端用戶使用，而這個方法設計在transform()方法內使用的。 void tf::TransformListener::lookupTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const 在時間time，ros::Time(0)當前時刻也就是最新一次bg的坐標變換，上使用從source_frame到target_frame的變換填充transform，為什么要講這個，這是因為 ROS 的 tf 在進行坐標之間的轉換變換不是實時的轉換，它有一個緩沖器來存放一段時間的坐標轉換數據，所以有時，可能沒有當前時間的坐標轉換數據，使用 lookupTransform() 函數就可以得到你想的某個時間的坐標數據，前提是緩沖區里要有這個時間的坐標數據，tf 的坐標轉換是自動計算的，所以有時，你想得到當前的時間的坐標轉換數據，你調用 lookupTransform() 函數去獲取，但是緩沖器里還沒有來得及去計算現在的坐標轉換數據，就是說：現在還沒有。如果你非要獲取，就會出錯，所以你要使用 waitForTransform() 函數來等待 tf 的坐標轉換線程得到你想要的時間點的坐標轉換數據。 簡單的說：waitForTransform() 就是一個安全程序。 Time(0):是tf緩存里的第一個tf信息。 now():是當前這個時間的tf信息。 如果把lookuptransfrom中的Time(0)換成now(),會報錯，因為每個監聽器有一個緩沖區，它存儲來自不同tf廣播者的所有坐標變換。 當廣播者發出變換時，變換進入緩沖區之前需要一些時間（通常是幾個毫秒）。 因此，當您在時間“now”請求坐標系變換時，您應該等待幾毫秒以獲得該信息}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)); pow(x,y)求的是x的y次方; 加上turtle_vel.publish(vel_msg);rate.sleep();}return 0; };

與話題服務一樣，也需要配置CMakeList.txt

注意沒有add_dependecies沒有依賴消息代碼。

配置packag.xml

下面我們看看如何為空間新增一個坐標系corrot1，并且計算出turtle2相對于corrot1的位置關系。

注意：

其實我們要增加傳感器或者改變原來傳感器的位置，就要新增坐標系。tf會處理所引入的額外坐標系，且不會更改原來的坐標系統，tf坐標系樹種不允許出現環，只能靠樹的關系來建立，所以當你引入新坐標系一定要說明這個坐標的父系是誰和相對父系的坐標數據，然后tf才會處理它，最后可以通過tf庫計算出它與任何坐標系的相對位置。這就是tf庫的偉大。

最開始的那張圖，tf樹包含三個坐標系：world，turtle1和turtle2。 兩只烏龜是世界的子系。

如果我們要向tf添加一個新坐標系，三個現有坐標系中的一個需要是父系，新坐標系將成為子系。我們將turtle1作為corrot1的父系。

加入新坐標代碼： #include <ros/ros.h> #include <tf/transform_broadcaster.h>int main(int argc, char** argv){ros::init(argc, argv, "my_tf_broadcaster");ros::NodeHandle node;tf::TransformBroadcaster br;//用于廣播的一個對象tf::Transform transform;//創建一個儲存相對位置數據的對象ros::Rate rate(10.0);while (node.ok()){transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );//初始化 相距2米transform.setRotation( tf::Quaternion(0, 0, 0, 1) );//廣播，指明相對關系和數據br.sendTransform(tf::StampedTransform(transform,ros::Time::now(), "turtle1", "carrot1"));rate.sleep();}return 0; };

是不是發現跟前面的廣播代碼是一樣的，沒錯就是一樣的。這里先創建一個變換對象transform，并且初始化。然后廣播出去，指定carrot1的相對于父系turtle1的相對位置是tranform。這樣就在坐標空間增加了新坐標carrot1。同時注意，carrot1所有速度位置都是基于turtule1了，后面會有小車例子。

編譯，launch文件，然后CMakeList.txt添加文件

然后運行。你會發現烏龜什么都沒改變，你只是廣播了新增的點，什么也沒干。但是在rviz中可以看到新增的坐標，移動turtle1，carrot1相對于turlte1的位置不變，相對世界點原點一直在變，而turtle2也一直追著turtle1在跑。

現在我們讓turtle2跟著corrot1跑 ，下面的duration時間大小自定義。

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

在之前的監聽中加入ROS_INFO不打印信息，需要在roslaunch中加入output=screen，在終端就不存在這個問題。

這樣運行起來之后，turtle1移動，carrot1相對于turlte1不動，然后turtle2追著corrot1跑。

會出現報錯

• 這是因為turtle2需要非零時間來生成并開始發布tf幀。 因此，第一次請求現在時間的/turtle2坐標系可能不存在，當請求轉換時，轉換可能不存在，并且第一次失敗。 在第一次變換之后，所有的變換都存在，并且烏龜的行為如預期的那樣。

檢查結果

• 現在，你應該能夠使用箭頭鍵（確保你的終端窗口是活躍的，而不是你的模擬器窗口），你會看到第二只烏龜跟隨第一只烏龜！
• 所以，你注意到烏龜的行為沒有明顯的區別。 這是因為實際的時間差只有幾個毫秒。 但是為什么我們從Time(0)到now()進行這種改變？ 只是教你關于tf緩沖區和與它相關的時間延遲。對于真實的tf用例，使用Time(0)通常是完全正常的。

下面再說一個隨著時間的移動，turtle2繞著turtle1繞圈。很簡單，只要改變turlte1與carrot1的相對距離，這個相對距離隨時間變化,carrot1繞著turtle1,turtle2追著carrot1，直觀上自然就繞著turtle1繞圈了。

于是我們更改carrot1跟turtle1的距離：

transform.setOrigin(tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );

transform.setRotation( tf::Quaternion(0, 0, 0, 1) );

看到沒，對時間用正余弦然后當成距離。c1繞著t1轉圈，t2追著c1，于是就形成了左邊的烏龜軌跡，烏龜1不動，烏龜2繞著烏龜1轉圈圈。此時我們還可以移動t1，軌跡就跟復雜了。

總結

以上是生活随笔為你收集整理的判断frame是否已创建_大白菜的ROS笔记（8）（创建TF广播和监听，内容很多，细节满满）...的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。