譯：使用`tf`來配置你的機器人

使用tf來配置你的機器人

1. 座標變換的配置

---

許多ROS包都需要訂閱利用tf庫發佈的該機器人的變換樹。抽象來說，變換樹定義了不同的座標幀之間轉換和旋轉的偏移量。具體來說，考慮一個簡單的機器人，這個機器人有一個移動基座以及一個安裝在其上的雷達，我們來定義兩個座標幀：一個是基座的中心，一個是雷達的中心。將這兩個座標幀分別命名爲“base_link”（對navigation來說，將base_link放在機器人的旋轉中心點是很重要的）和“base_laser”。

接下來假設我們已經有了一些從雷達處獲取的關於雷達中心點到其他位置的距離信息，也就是說，我們有了base_laser座標幀的數據。如果我們需要利用這些數據來讓移動基座進行避障的話，我們就需要將base_laser幀的數據變換爲base_link幀的數據。本質上也就是需要定義一個base_laser和base_link之間的變換關係。

爲了定義這個變換關係，假設我們已經知道了雷達安裝比基座中心點前10cm以及高20cm的地方。這也就給了我們base_link 和base_laser之間的轉換偏移量。具體來說，如果我們需要將數據從base_laser幀變換到base_link幀的話，我們必須要將座標加上(x: 0.1m, y: 0.0m, z: 0.2m)，而要從base_link幀變換到base_laser幀的話，就需要將座標加上(x: -0.1m, y: 0.0m, z: -0.2m)。

看上去似乎我們完全可以自己來進行座標變換，只要把合適的變化關係存起來，在需要變換的時候再應用上就OK了，但當座標幀的數量很大時這項工作就不那麼輕鬆了。這時tf就派上用場了，我們不需要自己進行座標變換，只需要使用tf定義一次base_link和base_laser之間的關係，然後讓tf來爲我們處理座標之間的變換就行了。

要使用tf來定義並存儲base_link和base_laser之間的變換關係，我們需要將該變換關係添加一個變換樹中。從概念上來說，變換樹中的每個節點都對應了一個座標幀，每條邊都對應了從當前節點變換到子節點所需要的變換關係。tf使用了樹的結構來保證任意兩個座標幀之間的變換都是單向的，並假設所有的邊都是直接從父節點連接到子節點的。

爲了給我們的例子創建一個變換樹，我們將定義兩個節點，一個關於base_link的，一個關於base_laser的。爲了創建這兩個節點之間的邊，我們首先需要決定哪個節點將作爲父節點，哪個節點作爲子節點。這是非常重要的，因爲tf假設了所有變換都是從父節點變換到子節點。這裏我們選擇base_link作爲父節點，因爲其他的傳感器可能還會添加到機器人中。這也就意味着，連接base_link到base_laser之間的邊應該是(x: 0.1m, y: 0.0m, z: 0.2m)。建立起這個變換樹之後，從雷達處獲得的base_laser幀變換到base_link就很容易了，只需要調用tf就OK了。這樣我們的機器人也就能利用利用該信息來安全地避障了。

2. 編碼

---

上面這個例子應該能幫助我們從概念上來理解tf的作用。接下來，我們需要在代碼上實現上面那個變換樹。在繼續這個例子前，你應該對ROS比較熟悉了，如果碰到不熟悉的概念就去查ROS Documentation。

將base_laser變換到base_link幀的第一步是，創建一個負責在我們的系統中發佈變換關係的節點。接下來，我們需要創建一個節點訂閱該變換關係並進行變換。首先，創建一個包來存放我們這個例子的所有源碼，將其命名爲”robot_setup_tf”並依賴roscpp, tf和geometry_msgs包。

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

或者在fuerte, groovy和hidro中：有一個navigation_tutorials stack中有一個標準的robot_setupt_tf_tutorial包，安裝這個包就可以了：

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

3. 廣播一個變換

---

創建這個包之後，我們需要創建一個用於廣播base_laser -> base_link的變換的節點。將下面這段代碼copy到剛纔創建的包的src/tf_broadcaster.cpp文件中：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_publisher");
  ros::NodeHandle n;

  ros::Rate r(100);

  tf::TransformBroadcaster broadcaster;

  while(n.ok()){
    broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),"base_link", "base_laser"));
    r.sleep();
  }
}

我們來仔細看一下這段代碼是怎麼發佈base_link -> base_laser的變換的。

#include <tf/transform_broadcaster.h>

tf包爲我們提供了tf::TransformBroadcaster的實現來幫助完成發佈變換。爲了使用TransformBroadcaster，我們需要include tf/transform_broadcaster.h頭文件。

tf::TransformBroadcaster broadcaster;

然後我們創建了一個TransformBroadcaster對象，之後我們會使用該對象來發送base_link -> base_laser的變換。

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

這裏就是實際完成工作的代碼了。通過TransformBroadcaster對象發送變換需要5個參數。

1. 旋轉變換，由btQuaternion指定的座標幀之間的旋轉變換。這裏我們不需要進行變換，將pitch，roll和yaw設爲0就可以了；
2. 座標變換，由btVector3指定的變換關係，從上文的分析我們知道從我們想設置的變換關係爲(0.1, 0.0, 0.2)；
3. 時間戳，我們需要給發送的變換附上一個時間戳，這裏直接用ros::Time::now()附上當前時間就可以了；
4. 變換樹中的父節點名，這裏就是base_link了；
5. 變換樹中的子節點名，這裏就是base_laser。

4. 使用變換

---

上文中我們已經創建了一個發佈base_link -> base_laser變換的節點了。接下來我們將編寫一個節點使用這個變換並將一個base_laser幀變換爲base_link幀。將下面這段代碼copy到src/tf_listener.cpp文件中：

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>

void transformPoint(const tf::TransformListener& listener){
  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
  geometry_msgs::PointStamped laser_point;
  laser_point.header.frame_id = "base_laser";

  //we'll just use the most recent transform available for our simple example
  laser_point.header.stamp = ros::Time();

  //just an arbitrary point in space
  laser_point.point.x = 1.0;
  laser_point.point.y = 0.2;
  laser_point.point.z = 0.0;

  try{
    geometry_msgs::PointStamped base_point;
    listener.transformPoint("base_link", laser_point, base_point);

    ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
        laser_point.point.x, laser_point.point.y, laser_point.point.z,
        base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
  }
  catch(tf::TransformException& ex){
    ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
  }
}

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_listener");
  ros::NodeHandle n;

  tf::TransformListener listener(ros::Duration(10));

  //we'll transform a point once every second
  ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

  ros::spin();

}

接下來介紹一下代碼的關鍵部分。

#include <tf/transform_listener.h>

這裏我們include了tf/transform_listener.h頭文件，因爲我們需要創建tf::TransformListener對象，該對象可以自動訂閱發佈的變換信息topic，並管理接受的所有變換數據。

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

這裏我們創建了一個函數，給定一個TransformListener，該函數能將base_laser轉換爲base_link，該函數也將作爲ros::Timer的回調函數，每秒調用一次。

  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
  geometry_msgs::PointStamped laser_point;
  laser_point.header.frame_id = "base_laser";

  //we'll just use the most recent transform available for our simple example
  laser_point.header.stamp = ros::Time();

  //just an arbitrary point in space
  laser_point.point.x = 1.0;
  laser_point.point.y = 0.2;
  laser_point.point.z = 0.0;

這裏我們創建了geometry_msgs::PointStamped點數據，消息名末尾的‘Stamped’表明該消息包含了一個header，可以存儲消息的timestamp和frame_id。這裏我們將timestamp設爲ros::Time()，這是一個特殊的時間值，可以允許我們想TransformListener請求最新的可用變換。frame_id則設置爲了“base_laser”，因爲我們創建的是一個base_laser幀的點數據。最後我們隨意設置了該點的座標，(x: 1.0, y: 0.2, z: 0.0)。

  try{
    geometry_msgs::PointStamped base_point;
    listener.transformPoint("base_link", laser_point, base_point);

    ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
        laser_point.point.x, laser_point.point.y, laser_point.point.z,
        base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
  }

現在我們已經有了base_laser的點，要轉換爲base_link的點，我們需要使用TransformListener對象，並調用transformPoint()方法，該方法有三個參數：

1. 變換後的frame名，這裏就是”base_link”；
2. 需要變換的點；
3. 以及變換後的點的存儲位置。

調用之後base_point中就存儲了laser_point變換後的數據了。

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

如果由於某些原因base_laser -> base_link的轉換不可用（如tf_broadcaster未運行）,在調用transformPoint（）時TransformListener可能會拋出異常。因此我們需要捕捉該異常。

5. build代碼

---

打開CMakeLists.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})

保存，build這個包。

$ cd %TOP_DIR_YOUR_CATKIN_WS%
$ catkin_make

6. 運行代碼

---

構建之後，打開terminal，運行一個core

roscore

再打開一個terminal運行tf_broadcaster節點

rosrun robot_setup_tf tf_broadcaster

打開第三個terminal運行tf_listener

rosrun tobot_setup_tf tf_listener

如果順利的話，終端會每秒輸出以下信息：

[ 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

OK，到這裏你已經基本成功了，接下來你就需要將例子中的PointStamped替換爲ROS中接受的實際的傳感器流數據了。關於這一部分，你可以閱讀這份文檔。

參考資料

1. navigation/Tutorials/RobotSetup/TF