ROS(九)：座標系統tf

阿新 • • 發佈：2019-02-05

1.安裝ｔｕｒｔｌｅ包

rosdep install turtle_tf rviz

2.執行demo

roslaunch turtle_tf turtle_tf_demo.launch

可以看到畫面上有兩個小烏龜，可以在終端啟用的情況下用鍵盤控制小烏龜一號，黃色的那隻，綠色的小烏龜就會緊緊的跟在黃色小烏龜後面．

這裡寫圖片描述

３．ｄｅｍｏ的分析

這個例程使用tf建立了三個參考系：a world frame, a turtle1 frame, and a turtle2 frame。然後使用tf broadcaster釋出烏龜的參考系，並且使用tf listener計算烏龜參考系之間的差異，使得第二隻烏龜跟隨第一隻烏龜。

我們可以使用tf工具來具體研究。

rosrun tf view_frames

這裡寫圖片描述

該檔案描述了參考系之間的聯絡。三個節點分別是三個參考系，而/world是其他兩個烏龜參考系的父參考系。還包含一些除錯需要的傳送頻率、最近時間等資訊。

tf還提供了一個tf_echo工具來檢視兩個廣播參考系之間的關係。我們可以看一下第二隻得烏龜座標是怎麼根據第一隻烏龜得出來的。

rosrun tf tf_echo turtle1 turtle2

這裡寫圖片描述

$T_{t u r t l e 1_t u r t l e 2} = T_{t u r t l e 1_w o r l d} * T_{w o r l d_t u r t l e 2}$

world∗Tworld_turtle2

這個公式的解釋是２號小烏龜到１號小烏龜的座標轉換等於世界座標系到１號小烏龜的Ｔ乘以２號小烏龜到世界座標系轉換的Ｔ．

４．寫一個ｔｆ的廣播

建立一個功能包,並新增對應的以來

 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

編寫對應的原始碼

cd /home/felaim/catkin_ws/src/learning_tf/src

新增對應的原始碼turtle_tf_broadcaster.cpp

//
// Created by felaim on 4/11/18.
//

#include <ros/ros.h>
//tf軟體包提供了TransformBroadcaster的實現，以幫助簡化釋出轉換的任務。
// 要使用TransformBroadcaster，我們需要包含tf/transform_broadcaster.h標頭檔案
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr &msg) {
    //建立一個TransformBroadcaster物件,用來發布變換矩陣
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    //設定２D的小烏龜的位姿，所以Z設定為零
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);
    //這是真正的工作完成的地方,使用TransformBroadcaster傳送變換需要四個引數
    //第一個引數是變換矩陣
    //給釋出的變換一個時間戳,地擁護髮就是當前時間ros::Time::now()
    //設定父親框架連結的名稱為"world"
    //設定子框架連結的名稱為,就是小烏龜的名字
    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;

}

在CMakeLists.txt檔案中新增下面的語句:

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

重新catkin_make一下.

建立一個launch資料夾,新增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>

執行launch檔案

roslaunch learning_tf start_demo.launch

檢查結果

rosrun tf tf_echo /world /turtle1

輸出的結果是1號小烏龜到世界座標系的tf.

這裡寫圖片描述

在tf中還有幾個問題注意的:

最常用的就是map，odom，base_link，base_laser座標系，這也是開始接觸gmapping的一些座標系。

map:地圖座標系，顧名思義，一般設該座標系為固定座標系（fixed frame），一般與機器人所在的世界座標系一致。

base_link:機器人本體座標系，與機器人中心重合，當然有些機器人(PR2)是base_footprint,其實是一個意思。

odom：里程計座標系，這裡要區分開odom topic，這是兩個概念，一個是座標系，一個是根據編碼器（或者視覺等）計算的里程計。但是兩者也有關係，odom topic 轉化得位姿矩陣是odom–>base_link的tf關係。這時可有會有疑問，odom和map座標系是不是重合的？（這也是我寫這個部落格解決的主要問題）可以很肯定的告訴你，機器人運動開始是重合的。但是，隨著時間的推移是不重合的，而出現的偏差就是里程計的累積誤差。那map–>odom的tf怎麼得到?就是在一些校正感測器合作校正的package比如gmapping會給出一個位置估計（localization），這可以得到map–>base_link的tf，所以估計位置和里程計位置的偏差也就是odom與map的座標系偏差。所以，如果你的odom計算沒有錯誤，那麼map–>odom的tf就是0.

base_laser:鐳射雷達的座標系，與鐳射雷達的安裝點有關，其與base_link的tf為固定的。

這個一定要注意,下標的區別.如果是視覺的話,相機座標系和世界座標系, $T_{c w}$ 和 $T_{w c}$ 的區別.

5.tf的listener

在learning_tf/src中新增原始碼:

//
// Created by felaim on 4/11/18.
//

#include <ros/ros.h>
//tf軟體包提供了一個TransformListener的實現來幫助簡化接收轉換的任務。
// 要使用TransformListener，我們需要包含tf/transform_listener.h標頭檔案
#include <tf/transform_listener.h>

#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char **argv) {
    ros::init(argc, argv, "my_tf_listener");

    ros::NodeHandle node;

    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn");

    turtlesim::Spawn srv;
    add_turtle.call(srv);

    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

    //在這裡，我們建立一個TransformListener物件。 一旦建立了監聽器，它就開始接收通過線路進行的tf轉換，並緩衝它們長達10秒。
    // TransformListener物件的範圍應該保持不變，否則它的快取將無法填充，幾乎每個查詢都會失敗。
    // 一種常見的方法是使TransformListener物件成為類的成員變數
    tf::TransformListener listener;

    ros::Rate rate(10.0);
    while (node.ok()) {
        tf::StampedTransform transform;
        try {
            //在這裡,真正的工作完成了,我們向偵聽器查詢特定的轉換.
            //我們希望1號小烏龜到2號小烏龜的轉換
            //我們想要改變的時間.提供ros::Time(0)只會為我們提供最新的可用轉換
            //所有這些包裝在try-catch塊中以捕捉可能的異常
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) {
            ROS_ERROR("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        //在這裡,變換用於計算turtle2的新的線性和角速度,基於它魚turtle1的距離和角度.
        //新速度釋出在主題"turtle2/cmd_vel"中,sim將使用它來更新turtle2的運動
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));

        turtle_vel.publish(vel_msg);
        rate.sleep();
    }

    return 0;

};

在CMakeLists.txt中新增對應的可執行檔案和連結庫

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

重新catkin_make

在之前的start_demo.launch檔案中新增

  <launch>
    ...
    <node pkg="learning_tf" type="turtle_tf_listener"
          name="listener" />
  </launch>

執行launch檔案:

roslaunch learning_tf start_demo.launch

這裡寫圖片描述

剛開始會報出上圖的error
發生這種情況是因為listener在收到有關2號烏龜訊息之前計算轉換,在使用turtlesim產生第二個小烏龜並開始廣播一個tf幀是需要時間,所以找不到對應幀.

6新增一個frame

在很多應用中,新增一個參考系是很有必要的,比如在一個world參考系下,有一個RGBD相機節點,tf可以幫助我們將rgbd掃描的資訊座標換成全域性座標,然後可以在全域性中顯示對應的位置.

a.tf訊息結構
tf中的資訊是一個樹狀的結構，world參考系是最頂端的父參考系，其他的參考系都需要向下延伸。如果我們在上文的基礎上新增一個參考系，就需要讓這個新的參考系成為已有三個參考系中的一個的子參考系。

這裡寫圖片描述

按照之前的方法,在src/frame_tf_broadcaster.cpp

//
// Created by felaim on 4/11/18.
//

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

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

    tf::TransformBroadcaster br;
    tf::Transform transform;

    ros::Rate rate(10.0);
    while (node.ok()) {
        //在這裡，我們建立一個新的轉換，從父turtle1到新的小胡蘿蔔1
        // carrot1框架距烏龜1框左側2米
        transform.setOrigin(tf::Vector3(0.0, 2.0, 0.0));
        transform.setRotation(tf::Quaternion(0, 0, 0, 1));
//        transform.setOrigin(tf::Vector3(2.0 * sin(ros::Time::now().toSec()), 2.0 * cos(ros::Time::now().toSec()), 0.0));
//        transform.setRotation(tf::Quaternion(0, 0, 0, 1));

        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1") );

        rate.sleep();
    }

    return 0;
}

在CMakeLists.txt中新增對應的程式碼

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

重新carkin_make.

在launch文件中新增對應的節點

  <launch>
    ...
    <node pkg="learning_tf" type="frame_tf_broadcaster"
          name="broadcaster_frame" />
  </launch>

執行對應的程式碼:

roslaunch learning_tf start_demo.launch

這時運行了還沒有變化,在src/turtle_tf_listener.cpp修改一部分程式碼:

 listener.lookupTransform("/turtle2", "/carrot1",
                           ros::Time(0), transform);

重新編譯並執行

 catkin_make
 roslaunch learning_tf start_demo.launch

這裡寫圖片描述

將frame_tf_broadcaster.cpp檔案進行修改，可以使turtle2在turlte1旁旋轉。

    transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );

重新編譯並執行

 catkin_make
 roslaunch learning_tf start_demo.launch

這裡寫圖片描述

7. tf and Time

如何獲得特定時間的變換矩陣?
開啟src/turtle_tf_listener.cpp.
可以看到:

   try{
      listener.lookupTransform("/turtle2", "/carrot1",  
                               ros::Time(0), transform);

我們可以讓2號小烏龜跟著1號小烏龜,修改下列程式碼:

//在這裡,我們指定一個時間為0,
//對於tf而言,0表示在緩衝區中最新可獲得的變換矩陣
   try{
      listener.lookupTransform("/turtle2", "/turtle1",  
                               ros::Time(0), transform);

這裡寫圖片描述

可以看到2號小烏龜始終跟隨1號小烏龜

然後,我們改成下面的程式碼:

 try{
    listener.lookupTransform("/turtle2", "/turtle1",  
                             ros::Time::now(), transform);

結果編譯完進行執行會報如下的錯誤:

[ERROR] [1523497623.747414128]: Lookup would require extrapolation into the future.  Requested time 1523497623.747204825 but the latest data is at time 1523497623.743468028, when looking up transform from frame [turtle1] to frame [turtle2]

這是為什麼呢?我們可以看到報錯是時間有問題.每個listener都有一個緩衝區,用於儲存來自不同tf的broadcaster的座標變換.當broadcaster發出tf時,通常需要幾毫秒的時間.因此,當我們”現在”請求tf的時候,應該等幾毫秒在獲取資訊.

我們可以修改程式碼如下:

  try{
    ros::Time now = ros::Time::now();
    listener.waitForTransform("/turtle2", "/turtle1",
                              now, ros::Duration(3.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             now, transform);

上述程式碼設定了等待變換矩陣.waitForTransform()需要四個引數變數.1.目標幀, 2.源幀, 3.現在的時間, 4.最長等待時間間隔

注意：使用ros :: Time :: now（）就是這個例子。通常這將是希望被轉換的資料的時間戳.

所以waitForTransform（）實際上會阻止，直到兩個海龜之間的轉換變為可用（這通常需要幾毫秒），或者,如果轉換不可用,直到達到超時.

重新編譯並執行程式碼,還是可以看到如下的錯誤:

[ERROR] [1523500937.935210218]: Lookup would require extrapolation into the past.  Requested time 1523500934.925630513 but the earliest data is at time 1523500935.359631462, when looking up transform from frame [turtle1] to frame [turtle2]

發生這種情況是因為turtle2需要非零時間來產生並開始釋出tf幀。所以，第一次你問現在/turtle2幀可能不存在，當請求變換時，變換可能還不存在，並且第一次失敗。第一次轉換後，所有轉換都存在，烏龜的行為如預期。

8.Time travel with tf
現在,我們可以不要讓2號小烏龜直接去1號小烏龜的地方,讓2號小烏龜去5秒鐘前1號小烏龜的位置

  try{
    ros::Time past = ros::Time::now() - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", "/turtle1",
                              past, ros::Duration(1.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             past, transform);

編譯執行後,所以現在，如果你想執行這個，你期望看到什麼？肯定在第一個5秒內第二隻烏龜不知道該去哪裡，因為我們還沒有第一隻烏龜的5秒歷史。但是這5秒之後呢？讓我們試試看：

這裡寫圖片描述

2號小烏龜怕是被tf這個教程弄瘋了…根本無法控制…
我們問的問題是，“5秒前/turtle1 5秒前相對於/turtle2 5秒鐘前的姿勢是什麼？”。這意味著我們正在控制第二隻烏龜，基於它在5秒前的位置以及第一隻烏龜在5秒前的位置。

我們真正想要問的是：“相對於/turtle2的當前位置，5秒前/ turtle1的姿勢是什麼？”。

那如果要正確地問這樣的問題,獲得對應的答案呢?

  try{
    ros::Time now = ros::Time::now();
    ros::Time past = now - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", now,
                              "/turtle1", past,
                              "/world", ros::Duration(1.0));
    listener.lookupTransform("/turtle2", now,
                             "/turtle1", past,
                             "/world", transform);

使用進階版的lookupTransform(),需要6個引數,1.目標幀,2.時間,3.源幀,4.時間,5.指定一個不會隨時間變化的幀作為參考幀,這裡可以選擇/world,好像也沒其他的…6.選擇儲存的變數

這裡寫圖片描述
該圖顯示了後臺正在做什麼, 過去它計算從第一隻烏龜到世界的轉變, 在時間從過去到現在的世界框架中, 現在，我們計算從世界到第二隻烏龜的變化.

雖然引數變多了,好歹2號小烏龜能夠跟著1號小烏龜♪(^∇^*)

這裡寫圖片描述

ROS(九)：座標系統tf

ROS(九)：座標系統tf

Learn OpenGL （六）：座標系統

Android群英傳知識點回顧——第九章：Android系統信息與安全機制

linux基礎篇（九）：Redhat7系統中使用指令碼安裝虛擬機器

微服務實戰(九)：落地微服務架構到直銷系統(回顧總結)

C++系統學習之九：順序容器

ROS（一）：檔案系統介紹

在樹莓派上安裝ROS：Raspbian系統、ROS2GO系統、ROS Kinectic系統

ArcGIS教程：ArcGIS中的座標系統定義與投影轉換

ROS探索總結（十二）——座標系統

Android群英傳讀書筆記——第九章：Android系統資訊與安全機制

Linux學習筆記九：磁盤分區與文件系統

我是如何學習寫一個作業系統（九）：檔案系統

C#開發PACS醫學影像處理系統(九)：序列控制元件與拖拽

C#開發PACS醫學影像處理系統(十九)：Dicom影像放大鏡

九：給請求加斷言

Thinkphp5筆記九：路由設置，隱藏indx.php

DataGridView使用技巧九：DataGridView的右鍵菜單（ContextMenuStrip）

Oracle數據庫12cR2（項目實戰之二）：Linux系統安裝Oracle12.2

老男孩教育每日一題-2017年5月12日-磁盤知識點：linux系統中LVM配置實現方法?

ROS(九)：座標系統tf

相關推薦