ROS學習之tf.2.編寫一個TF廣播器(C++)
阿新 • • 發佈:2019-01-30
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+