TF學習筆記2(建立一個釋出者)
在接下來的兩個教程中,我們將編寫程式碼以重現tf介紹教程的Demo。之後,以下教程將重點放在使用更高階的tf功能擴充套件演示。
在開始之前,您需要為此專案建立一個新的ros包。
建立一個名為learning_tf的包,它依賴於tf,roscpp,rospy和turtlesim:
$ cd%YOUR_CATKIN_WORKSPACE_HOME%/ src
$ catkin_create_pkg learning_tf tf roscpp rospy turtlesim
編譯:
$ cd%YOUR_CATKIN_WORKSPACE_HOME%/ $ catkin_make $ source ./devel/setup.bash
1. 如何釋出座標變換
本教程將教您如何將座標釋出到tf。
我們希望釋出海龜四處移動時的變化座標。
我們先建立原始檔。轉到我們剛剛建立的包:
$ roscd learning_tf
1.1 程式碼
轉到src /資料夾,並啟動您喜歡的編輯器,將以下程式碼貼上到名為src / turtle_tf_broadcaster.cpp的新檔案中。
https://raw.github.com/ros/geometry_tutorials/hydro-devel/turtle_tf/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; };
1.2 程式解釋
現在,讓我們來看看與將turtle pose釋出到tf相關的程式。
#include <tf/transform_broadcaster.h>
tf包提供了TransformBroadcaster,以幫助簡化釋出轉換的任務。
要使用TransformBroadcaster,我們需要包含tf / transform_broadcaster.h標頭檔案。
static tf::TransformBroadcaster br;
我們建立一個TransformBroadcaster物件,稍後我們將使用該物件傳送轉換。
補充下接收到資訊的型別:
setOrigin應為平移座標;setRPY應為尤拉角;Quaternion應為四元數;
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變換中。
transform.setRotation(q);
在這裡我們設定旋轉。
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
使用TransformBroadcaster傳送變換需要四個引數。
- 首先,我們傳遞變換本身。
-
現在我們需要為變換髮佈一個時間戳,我們只用當前時間標記它,ros :: Time :: now()。
-
然後,我們需要傳遞我們正在建立連結的父座標系的名稱,在本例中為“world”
- 最後,我們需要傳遞我們正在建立連結的子座標系的名稱,在這種情況下,這是烏龜本身的名稱。
注意:sendTransform和StampedTransform have opposite ordering of parent and child.
2. 執行釋出者
現在我們建立了程式碼,讓我們先編譯它。開啟CMakeLists.txt檔案,並在底部新增以下行:
add_executable(turtle_tf_broadcaster src / turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster $ {catkin_LIBRARIES})
建立你的包裹; 在catkin工作區的頂部資料夾中:
$ catkin_make
如果一切順利,你的devel / lib / learning_tf資料夾中應該有一個名為turtle_tf_broadcaster的二進位制檔案。
如果是這樣,我們已準備好為此演示建立一個啟動檔案。使用文字編輯器,建立一個名為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
3. 檢查結果
現在,使用tf_echo工具檢查烏龜姿勢是否實際上正在廣播到tf:
$ rosrun tf tf_echo / world / turtle1
這應該會告訴你第一隻烏龜的姿勢。
使用箭頭鍵驅動烏龜(確保終端視窗處於活動狀態,而不是模擬器視窗)。
如果你為世界和烏龜2之間的轉換執行tf_echo,你不應該看到變換,因為第二隻烏龜還沒有。
但是,只要我們在下一個教程中新增第二隻烏龜,烏龜2的姿勢就會被廣播到tf。