ROS 導航功能包 爲你的機器人配置 tf

• cn/navigation/Tutorials/RobotSetup/TF

 

Note: This tutorial assumes that you have completed the ROS Tutorials and the tf basic tutorials. The code for this tutorial is available in the robot_setup_tf_tutorial package..

Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

爲你的機器人配置 tf

Description: This tutorial provides a guide to set up your robot to start using tf.

Tutorial Level: BEGINNER

Next Tutorial: Learn how to have the robot state publisher do all the tf publishing for you: using the robot state publisher on your own robot

目錄

1. Transform Configuration（變換配置）
2. Writing Code（代碼編寫）
3. Broadcasting a Transform（廣播變換）
4. Using a Transform（調用變換）
5. Building the Code代碼構建
6. Running the Code（代碼運行）

 

Transform Configuration（變換配置）

許多ROS功能包，都要求利用tf軟件庫，以機器人識別的變換樹的形式進行發佈。抽象層面上，變換樹其實就是一種“偏移”，代表了不同座標系之間的變換和旋轉。更具體點來說，設想一個簡單的機器人，只有一個基本的移動機體和掛在機體上方的掃描儀。基於此，我們定義了兩個座標系：一個對應於機體中心點的座標系，一個對應於掃描儀中心的座標系。分別取名爲“base_link”和“baser_laser”。關於座標系的命名習慣，參考REP 105.

此時，可以假設，我們已經從傳感器獲取了一些數據，以一種代表了物體到掃描儀中心點的距離的形式給出。換句話說，我們已經有了一些“base_laser”座標系的數據。現在，我們期望通過這些數據，來幫助機體避開物理世界的障礙物。成功的關鍵是，我們需要一種方式，把傳感器掃描的數據，從“base_laser”座標系轉換到“base_link”座標系中去。本質上，就是定義一種兩個座標系的“關係”。

爲了定義這種關係，假設我們知道，傳感器是掛在機體中心的前方10cm，高度20cm處。這就等於給了我們一種轉換的偏移關係。具體來說，就是，從傳感器到機體的座標轉換關係應該爲(x:0.1m,y:0.0m, z:0.2m),相反的轉換即是(x:-0.1m,y:0.0m,z:0.2m)。

我們可以選擇去自己管理這種變換關係，意味着需要自己去保存，以及在需要的時候調用。但是，這種做法的缺陷即是隨着座標轉換關係數量的增加，而愈加麻煩。幸運的是，我們也沒有必要這麼幹。相反，我們利用tf定義了這麼一種轉換關係，那麼就讓它來幫我們管理這種轉換關係吧。

利用tf來管理這種關係，我們需要把他們添加到轉換樹（transform tree）中。一方面來說，轉換樹中的每一個節點都對應着一類座標系，節點之間的連線即是兩個座標相互轉換的一種表示，一種從當前節點到子節點的轉換表示。Tf利用樹結構的方式，保證了兩個座標系之間的只存在單一的轉換，同時假設節點之間的連線指向是從parent到child。

基於我們簡單的例子，我們需要創建兩個節點，一個“base_link”，一個是“base_laser”。爲了定義兩者的關係，首先，我們需要決定誰是parent，誰是child。時刻記得，由於tf假設所有的轉換都是從parent到child的，因此誰是parent是有差別的。我們選擇“base_link”座標系作爲parent，其他的傳感器等，都是作爲“器件”被添加進robot的，對於“base_link”和“base_laser”他們來說，是最適合的。這就意味着轉換關係的表達式應該是(x:0.1m,y0.0m,z:0.2m)。關係的建立，在收到“base_laser”的數據到“base_link”的轉換過程，就可以是簡單的調用tf庫即可完成。我們的機器人，就可以利用這些信息，在“base_link”座標系中，就可以推理出傳感器掃描出的數據，並可安全的規劃路徑和避障等工作。

 

Writing Code（代碼編寫）

希望上面的例子，一定程度上可以幫助大家理解tf。現在，我們可以建立通過代碼來實現轉換樹。對於這個例子來說，前提是熟悉ROS，所以如果不熟悉，先預習下ROS Documentation吧。

假定，我們以上層來描述“base_laser”座標系的點，來轉換到"base_link"座標系。首先，我們需要創建節點，來發布轉換關係到ROS系統中。下一步，我們必須創建一個節點，來監聽需要轉換的數據，同時獲取並轉換。在某個目錄創建一個源碼包，同時命名“robot_setup_tf”。添加依賴包roscpp,tf,geometry_msgs。

$ cd %TOP_DIR_YOUR_CATKIN_WS%/src
$ catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs

至此，你必須運行上面的命令，當然你必須有必要的權限（例如，~/ros目錄下，你可能在之前的文檔中操作過這個目錄）

另外，在 fuerte, groovy 和 hydro 中，有一個標準的 robot_setup_tf_tutorial 軟件包，在 navigation_tutorials 中。 你可以按如下方法安裝他們 (%YOUR_ROS_DISTRO% 可以是 { fuerte, groovy } den):

$ sudo apt-get install ros-%YOUR_ROS_DISTRO%-navigation-tutorials

Broadcasting a Transform（廣播變換）

至此，我們已經創建了package。我們需要創建對於的節點，來實現廣播任務base_laser->base_link。在robot_setup_tf包中，用你最喜歡的編輯器打開，然後將下面的代碼粘貼到src/tf_broadcaster.cpp文件中去。

   1 #include <ros/ros.h>
   2 #include <tf/transform_broadcaster.h>
   3 
   4 int main(int argc, char** argv){
   5   ros::init(argc, argv, "robot_tf_publisher");
   6   ros::NodeHandle n;
   7 
   8   ros::Rate r(100);
   9 
  10   tf::TransformBroadcaster broadcaster;
  11 
  12   while(n.ok()){
  13     broadcaster.sendTransform(
  14       tf::StampedTransform(
  15         tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
  16         ros::Time::now(),"base_link", "base_laser"));
  17     r.sleep();
  18   }
  19 }

現在，讓我們來針對上面的代碼，作更細節的解釋。

   2 #include <tf/transform_broadcaster.h>
   3 

Tf功能包提供了一種實現tf::TransformBroadcaster ，使任務發佈變換更容易。爲了調用TransformBroadcaster, 我們需要包含 tf/transform_broadcaster.h 頭文件.

  10   tf::TransformBroadcaster broadcaster;

我們創建一個TransformBroadcaster對象，之後我們可以利用他來發送變換關係，即base_link → base_laser。

  13     broadcaster.sendTransform(
  14       tf::StampedTransform(
  15         tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
  16         ros::Time::now(),"base_link", "base_laser"));

這部分是關鍵部分。通過TransformBroadcaster來發送轉換關係，需要附帶5個參數。第1個參數，我們傳遞了旋轉變換，在兩個座標系的發送的任意旋轉，都必須通過調用btQuaternion.現在情況下，我們不想旋轉，所以我們在調用btQauternion的時候，將pitch,roll,yaw的參數都置0.第2個參數，btVector3，任何變換過程都需要調用它。無論怎樣，我們確實需要做一個變換，所以我們調用了btVector3，相應的傳感器的x方向距離機體基準偏移10cm，z方向20cm。第3個參數，我們需要給定轉換關係攜帶一個時間戳，我們標記爲ros::Time::now()。第4個參數，我們需要傳遞parent節點的名字。第5個參數，傳遞的是child節點的名字。

 

Using a Transform（調用變換）

上面的代碼，我們創建了一個節點來發布轉換關係，baser_laser->base_link。現在，我們需要利用轉換關係，將從傳感器獲取的數據轉換到機體對應的數據，即是“base_laser”->到“base_link”座標系的轉換。下面的代碼，後邊會緊根更詳細的解析。在robot_setup_if功能包中，在src目錄下創建tf_listener.cpp，並將下面的代碼粘貼到裏面：

   1 #include <ros/ros.h>
   2 #include <geometry_msgs/PointStamped.h>
   3 #include <tf/transform_listener.h>
   4 
   5 void transformPoint(const tf::TransformListener& listener){
   6   //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
   7   geometry_msgs::PointStamped laser_point;
   8   laser_point.header.frame_id = "base_laser";
   9 
  10   //we'll just use the most recent transform available for our simple example
  11   laser_point.header.stamp = ros::Time();
  12 
  13   //just an arbitrary point in space
  14   laser_point.point.x = 1.0;
  15   laser_point.point.y = 0.2;
  16   laser_point.point.z = 0.0;
  17 
  18   try{
  19     geometry_msgs::PointStamped base_point;
  20     listener.transformPoint("base_link", laser_point, base_point);
  21 
  22     ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
  23         laser_point.point.x, laser_point.point.y, laser_point.point.z,
  24         base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
  25   }
  26   catch(tf::TransformException& ex){
  27     ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
  28   }
  29 }
  30 
  31 int main(int argc, char** argv){
  32   ros::init(argc, argv, "robot_tf_listener");
  33   ros::NodeHandle n;
  34 
  35   tf::TransformListener listener(ros::Duration(10));
  36 
  37   //we'll transform a point once every second
  38   ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));
  39 
  40   ros::spin();
  41 
  42 }

好的，現在讓我們將上面代碼的重點步步分解：

   3 #include <tf/transform_listener.h>
   4 

這裏，我們包含tf/transform_listener.h頭文件，是爲了後邊創建tf::TransformListener。一個TransformListener目標會自動的訂閱ROS系統中的變換消息主題，同時管理所有的該通道上的變換數據。

   5 void transformPoint(const tf::TransformListener& listener){

創建一個函數，參數爲TransformListener,作用爲將“base_laser”座標系的點，變換到“base_link”座標系中。這個函數將會以ros::Timer定義的週期，作爲一個回調函數週期調用。目前週期是1s。

   6   //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
   7   geometry_msgs::PointStamped laser_point;
   8   laser_point.header.frame_id = "base_laser";
   9 
  10   //we'll just use the most recent transform available for our simple example
  11   laser_point.header.stamp = ros::Time();
  12 
  13   //just an arbitrary point in space
  14   laser_point.point.x = 1.0;
  15   laser_point.point.y = 0.2;
  16   laser_point.point.z = 0.0;

此處，我們創建一個虛擬點，作爲geometry_msgs::PointStamped。消息名字最後的“Stamped”的意義是，它包含了一個頭部，允許我們去把時間戳和消息的frame_id相關關聯起來。我們將會設置laser_point的時間戳爲ros::time(),即是允許我們請求TransformListener取得最新的變換數據。對於header裏的frame_id，我們設置爲“base_laser”,因爲我們是創建的是掃描儀座標系裏的虛擬點。最後，我們將會設置具體的虛擬點，比如x:1.0,y:0.2,z:0.0

  18   try{
  19     geometry_msgs::PointStamped base_point;
  20     listener.transformPoint("base_link", laser_point, base_point);
  21 
  22     ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
  23         laser_point.point.x, laser_point.point.y, laser_point.point.z,
  24         base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
  25   }

至此，我們已經有了從“base_laser”到“base_link”變換的點數據。進一步，我們通過TransformListener對象，調用transformPoint(),填充三個參數來進行數據變換。第1個參數，代表我們想要變換的目標座標系的名字。第2個參數填充需要變換的原始座標系的點對象，第3個參數填充，目標座標系的點對象。所以，在函數調用後，base_point裏存儲的信息就是變換後的點座標。

  26   catch(tf::TransformException& ex){
  27     ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
  28   }

如果某些其他的原因，變換不可得（可能是tf_broadcaster 掛了），調用transformPoint()時，TransformListener調用可能會返回異常。爲了體現代碼的優雅性，我們將會截獲異常並把異常信息呈現給用戶。

 

Building the Code代碼構建

至此，根據我們寫的例子，接下來我們需要構建編譯。打開CMakeList.txt，在文件末尾添加下面的幾行：

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

Next, make sure to save the file and build the package.

$ cd %TOP_DIR_YOUR_CATKIN_WS%
$ catkin_make

Running the Code（代碼運行）

好的，萬事俱備只欠東風！讓我恩試着實際運行下吧。這部分，你需要開三個終端窗口

第一個窗口，運行core

roscore

第二個，運行 tf_broadcaster

rosrun robot_setup_tf tf_broadcaster

好的。現在，我們會在第三個窗口運行tf_listener，將從傳感器座標系獲取的虛擬點，變換到機體座標系。

rosrun robot_setup_tf tf_listener

如果一切順利，應該會看到類似的結果。每次打印間隔1s。

[ INFO] 1248138528.200823000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138528.19
[ INFO] 1248138529.200820000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138529.19
[ INFO] 1248138530.200821000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138530.19
[ INFO] 1248138531.200835000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138531.19
[ INFO] 1248138532.200849000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138532.19

祝賀你，你已經成功的編寫了一個針對平面傳感器的座標變換。下一步就是替換PointStamped，來使用真正的傳感器進行操作。幸運的是，已經有相關的指導文檔了here。