1、變換設定 設計一個感測器在機器人上的場景

許多ROS 功能包 通過利用TF2 軟體 庫 去 釋出 機器人的 座標變換樹

在抽象層面上，座標變換樹 定義了 每個 不同的座標系間的 偏移和旋轉。

為了更加具體一些，舉個例子

例如一個簡單的機器人，是一個可移動的小車底盤在頂部安裝著一個單線鐳射測距儀。

在這個簡單機器人中，定義兩個座標系 ： 一個小車底盤的中心（base_link），另一個鐳射測距儀的中心(base_laser)

假設鐳射測距有一些資料，是目標到鐳射測距儀中心的距離。即 有一些資料在base_laser座標系下。

現在我們想用這些資料幫助小車移動，來避免撞到東西。

我們需要將 在 base_laser 下的資料轉換到 base_link下。換句話說就是我們需要base_laser座標系和base_link座標系的轉換關係。

現在假設有這種關係， 雷達安裝在距離小車中心點前10cm，高20cm的位置。

這樣就有了兩個座標系的偏移轉換關係。（兩個座標系之間沒有旋轉關係）

由 base_link 的 資料 轉換到 base_laser 的座標系下 需要 (x: 0.1m, y: 0.0m, z: 0.2m) 這樣的轉換

相反 base_laser 的 資料 轉換到 base_link 的座標系下 需要 (x:- 0.1m, y: 0.0m, z:- 0.2m) 這樣的轉換

我們可以選擇自己管理這種關係，這意味著在必要時在座標系之間儲存和應用適當的轉換，但是隨著座標框架數量的增加，這變得非常困難。 幸運的是，我們不必自己做這項工作。 相反，我們將使用TF一次定義“ base_link”和“ base_laser”之間的關係，並讓它為我們管理兩個座標系之間的轉換。

要使用tf定義和儲存“ base_link”和“ base_laser”座標系之間的關係，我們需要將它們新增到座標變換樹中。 座標變換樹中的每個節點都對應於一個座標系，每個座標變換是從當前節點移動到其子節點的變換。 TF使用樹結構來確保所有的座標系都在這個樹結構中。

對上面的示例 建立一個 座標變換樹 ，需要建立兩個節點，一個用於“ base_link”座標系，一個用於“ base_laser”座標系。

需要確定哪個是父座標系，哪個是子座標系。這種區別很重要，因為tf假定所有轉換都從父級移動到子級。

選擇“ base_link”座標系作為父座標系，因為隨著其他零件/感測器被新增到機器人中，通過遍歷“ base_link”框架將它們與“ base_laser”框架聯絡起來是最有意義的

連線“ base_link”和“ base_laser”相關的變換應為（x：0.1m，y：0.0m，z：0.2m）。

通過設定 這樣的轉換樹 ，把 base_laser 座標系下的資料 轉換為 base_link 座標系下的資料 將變的像呼叫tf庫一樣簡單。

2、釋出座標變化（TF） 程式碼 (釋出感測器座標系與機器人座標系關係)

TF分成兩個版本 TF和TF2
下面這個是TF

需要建立一個節點 完成廣播 base_laser到base_link TF 通過ROS.

建立 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();
  }
}

程式碼解釋

#include <tf/transform_broadcaster.h>

TF功能包 實現了 tf::TransformBroadcaster 來完成 釋出 座標變換
使用 tf::TransformBroadcaster 必須包含 該 標頭檔案 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"));

這是真正工作的地方，
傳送一個座標變換需要5個引數

• 第一個是旋轉變換 通過四元數
• 第二個是偏移向量 Vector3 這個例子中設定雷達 的 x偏移 0.1m 和 z的0.2m 相對與 base_link的
• 第三個是釋出的時間戳 用ros::Time::now() 即可
• 第四個兩個座標變換中的父座標系的名稱 即 “base_link”
• 第五個兩個座標變換中的子座標系的名稱 即 “base_laser”

這個 和 之前 部落格

基本一至 其實做的事情就是一樣的 。

對比一下

這篇部落格 就是 通過 tf::StampedTransform() 去構造的 geometry_msgs::TransformStamped

之前部落格 就是 手動去 配置的 geometry_msgs::TransformStamped

下面介紹下 tf::StampedTransform() 函式
有五個引數

1. 座標的變換關係 旋轉
2. 座標的變換關係 平移
3. 時間戳
4. 父座標系
5. 子座標系

3、使用座標變換 程式碼 （將感測器資料轉換為機器人座標系下）

上面，建立了一個節點，釋出 base_laser 和 base_link 的座標變換（TF）通過ROS。

現在寫一個節點，使用上面的座標變換（TF），來將base_laser座標系下的點，轉換到base_link座標系下。

建立 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));//等待10s，如果10s之後都還沒收到訊息，那麼之前的訊息就被丟棄掉

  //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();

}

這個 main 函式裡有個 呼叫ROS定時器的功能 就是 這

這個不是 本 篇 的重點 回來再介紹 ，功能就是 間隔 1 秒 （第一個引數），然後執行 後面的 函式

還有一個 知識點 boost::bind
boost::bind
是標準庫函式std::bind1st和std::bind2nd的一種泛化形式。
其可以支援函式物件、函式、函式指標、成員函式指標，並且繫結任意引數到某個指定值上或者將輸入引數傳入任意位置。

另一個知識點 boost::ref
boost庫中ref用於包裝一個物件，使其看起來像別名一樣。（不是很理解，先不管它）

boost::bind(&transformPoint, boost::ref(listener))

那麼就相當於

transformPoint(listener);

  ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

這行程式碼也就是，1秒鐘，執行transformPoint(listener) 1次

程式碼解釋

main函式裡的程式碼基本解釋清楚了

下面主要看下回撥函式

 void transformPoint(const tf::TransformListener& listener)

裡面的

========================================================================

#include <tf/transform_listener.h>

包含tf/transform_listener.h 標頭檔案

因為需要建立一個 tf::TransformListener 類 例項

TransformListener 物件會自動訂閱 座標變換 資訊 通過ROS 並且管理 資料轉換

========================================================================

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

建立一個回撥函式， 一個形參TransformListener

在 base_laser 座標系中 設定一個點， 轉換到 base_link 座標系中。

這個函式 由 main()函式中的定時器，一秒中回撥一次。

========================================================================

  //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，y ，z 的 具體的 值

========================================================================

  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() 函式

有三個引數

• 第一個：想要轉換到的座標系下的 座標系名稱 這個例子中就是（base_link）
• 第二個：需要轉換的點
• 第三個：轉換後的點

所以經過 listener.transformPoint(“base_link”, laser_point, base_point); 之後

base_point 就有了在 base_link 座標系下的 對應的點

========================================================================

  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可能會引發異常。 為了確保能正常處理，將捕獲異常並列印錯誤。

========================================================================

4、 編譯

在 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})

編譯

cd 工作空間 路徑
catkin_make

5、執行程式碼

執行 roscore

roscore

執行 tf廣播者

rosrun robot_setup_tf tf_broadcaster

執行tf監聽者

rosrun robot_setup_tf tf_listener

6、結果

7、Done