TF靜態坐標轉換

第五章：ROS常用組件

5.1 TF坐標變換

概念性的東西趙老師的講義寫的非常的清楚，這里就不再重復了。講義鏈接如下：

趙老師的講義

下面用一個例子來演示 TF 的使用方法。

例程需求：
創建一個發布者節點，發布兩個坐標系以及他們之間的空間變換關系
創建一個接收者節點，用來接收並打印變換后的坐標數據
兩個節點之間的話題消息使用 geometry_msgs

重點學習內容：
了解節點消息的內容
學習創建節點過程中使用的庫

5.1.1 坐標 msg 消息

通過命令行輸入:

rosmsg list

我們可以從返回值中看到 ROS 系統中封裝的消息都有哪些。

返回的消息類型非常的多，但是大概被分成了下面的幾類：

• actionlib
• bond
• control_msgs
• control_manager_msgs
• dynamic_reconfigure
• gazebo_msgs
• geometry_msgs
• map_msgs
• nac_msgs
• sensor_msgs
• shape_msgs
• std_msgs
• tf2_msgs
• trajectory_msgs
• turtle
• visualization_msgs
  ...

英文比較好的同學應該大體能知道這些消息類型分別是用來做什么的。我們這里所需要的是傳遞兩個坐標系之間的關系和坐標與點的關系，所以要用的 msgs 應該屬於 geometry_msgs。具體來說應該是geometry_msgs/TransformStamped和geometry_msgs/PointStamped。

然后通過rosmsg info geometry_msgs/TransformStamped查看消息的中包含的內容：

std_msgs/Header header                     #頭信息
  uint32 seq                                #|-- 序列號
  time stamp                                #|-- 時間戳
  string frame_id                            #|-- 坐標 ID
string child_frame_id                    #子坐標系的 id
geometry_msgs/Transform transform        #坐標信息
  geometry_msgs/Vector3 translation        #偏移量
    float64 x                                #|-- X 方向的偏移量
    float64 y                                #|-- Y 方向的偏移量
    float64 z                                #|-- Z 方向上的偏移量
  geometry_msgs/Quaternion rotation        #四元數
    float64 x                                
    float64 y                                
    float64 z                                
    float64 w

從上面的消息內容中我們可以看到，消息中可以包含其他的消息內容，並且將其重命名。

然后我們再通過：rosmsg info geometry_msgs/PointStamped查看另外一個消息的內容：

命令行鍵入:rosmsg info geometry_msgs/PointStamped

std_msgs/Header header                    #頭
  uint32 seq                                #|-- 序號
  time stamp                                #|-- 時間戳
  string frame_id                            #|-- 所屬坐標系的 id
geometry_msgs/Point point                #點坐標
  float64 x                                    #|-- x y z 坐標
  float64 y
  float64 z

5.1.2 靜態坐標變換的發布者節點

需求說明：

現有一機器人模型，核心構成包含主體與雷達，各對應一坐標系，坐標系的原點分別位於主體與雷達的物理中心，已知雷達原點相對於主體原點位移關系如下: x 0.2 y0.0 z0.5。當前雷達檢測到一障礙物，在雷達坐標系中障礙物的坐標為 (2.0 3.0 5.0),請問，該障礙物相對於主體的坐標是多少？

在RVIZ看起來就是這樣：

那么要實現這個效果，我們需要發布相關消息。首先創建一個新的功能包，包的依賴為： tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

然后配置編譯文件，這里就不再演示了。

代碼如下：

#include "ros/ros.h"
// 用於廣播靜態變換的庫
#include "tf2_ros/static_transform_broadcaster.h"
// 用於儲存空間位置消息內容的庫
#include "geometry_msgs/TransformStamped.h"
// 用於計算坐標變換的庫
#include "tf2/LinearMath/Quaternion.h"

/*
    需求：發布坐標系之間的相對關系

    流程：
        1. 包含頭文件；
        2. 初始化（設置編碼，節點初始化，創建節點句柄）；
        3. 創建發布對象；
        4. 組織被發布的消息；
        5. 發布數據；
        6. spin();
*/

int main(int argc, char *argv[])
{
        // 2. 初始化（設置編碼，節點初始化，創建節點句柄）；
        setlocale(LC_ALL,"");
        ros::init(argc,argv,"static_pub");
        ros::NodeHandle nh;
        // 3. 創建發布對象；
        tf2_ros::StaticTransformBroadcaster pub;
        // 4. 組織被發布的消息；
        geometry_msgs::TransformStamped tfs;
        tfs.header.stamp = ros::Time::now();
        tfs.header.frame_id = "base_link";    // 相對坐標系關系中被參考的那一個
        tfs.child_frame_id = "laser";
        tfs.transform.translation.x = 0.2;
        tfs.transform.translation.y = 0.0;
        tfs.transform.translation.z = 0.5;

        // 需要根據歐拉角轉換
        tf2::Quaternion qtn;        // 創建四元數對象
        // 向該對象設置歐拉角，這個對象可以將歐拉角轉化成四元數
        qtn.setRPY(0.0, 0.0, 0.0);  // 歐拉角的單位是弧度
        tfs.transform.rotation.x = qtn.getX();
        tfs.transform.rotation.y = qtn.getY();
        tfs.transform.rotation.z = qtn.getZ();
        tfs.transform.rotation.w = qtn.getW();
        
        // 5. 發布數據；
        pub.sendTransform(tfs);
        // 6. spin();
        ros::spin();
    
    return 0;
}

代碼中的注釋非常的清楚，這里就不多介紹了。展示一些結果：

想要生成RIVZ的的圖像，在命令行中輸入 rviz 然后設置Fixed Frame 為 base_link，點擊左下的 add 按鈕，在彈出的窗口中選擇 TF 組件，即可顯示坐標關系。

5.1.3 靜態坐標變換的接收者節點

代碼中的注釋非常明確，所以直接看代碼。

代碼如下：

#include "ros/ros.h"
// 坐標接收者庫
#include "tf2_ros/transform_listener.h"
// 緩存庫
#include "tf2_ros/buffer.h"
// 坐標里點的消息庫
#include "geometry_msgs/PointStamped.h"
// 坐標轉換的庫
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
/*
    訂閱方：訂閱發布的坐標系相對關系，傳入一個坐標點，調用 tf 實現轉換

    流程：
        1. 包含頭文件；
        2. 初始化（編碼，節點，NodeHandle）
        3. 創建訂閱對象； --> 訂閱坐標系相對關系
        4. 組織一個座標點數據；
        5. 轉化算法，調用tf內置實現；
        6. 最后輸出。
*/

int main(int argc, char *argv[])
{
    // 2. 初始化（編碼，節點，NodeHandle）
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"Static_sub");
    ros::NodeHandle nh;
    // 3. 創建訂閱對象； --> 訂閱坐標系相對關系
    // 3-1. 創建一個 buffer 緩存
    tf2_ros::Buffer buffer;
    // 3-2. 創建一個監聽對象（將訂閱的數據緩存到 buffer）
    tf2_ros::TransformListener listener(buffer);
    // 4. 組織一個座標點數據；
    geometry_msgs::PointStamped ps;
    ps.header.frame_id = "laser";
    ps.header.stamp = ros::Time::now();
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;

    // 休眠
    // ros::Duration(2).sleep();
    // 5. 轉化算法，調用tf內置實現；
    ros::Rate rate(10);
    while(ros::ok())
    {
        // 核心代碼 -- 將 ps 轉換成相對於 base_link 的坐標點
        geometry_msgs::PointStamped ps_out;
        /*
            調用了 buffer 的轉換函數 transform
            參數1：被轉化的座標點
            參數2：目標坐標系
            返回值：輸出的坐標點

            PS1：調用時必須包含頭文件 tf2_geometry_msgs/tf2_geometry_msgs.h
            PS2: 運行時存在的問題，拋出異常 base_link 不存在
                 原因： 訂閱數據是一個耗時操作，可能在調用 transform 轉換函數時，
                        坐標系的相對關系還沒訂閱到，因此出現異常
                 解決：
                       方案1：在調用轉換函數前，執行休眠；
                       方案2： 進行異常處理(建議使用)。


        */

       try
       {
           
        ps_out =  buffer.transform(ps,"base_link");
        // 6. 最后輸出
        ROS_INFO("轉換后的坐標值:(%.2f,%.2f,%.2f),參考坐標系: %s",
                        ps_out.point.x,
                        ps_out.point.y,
                        ps_out.point.z,
                        ps_out.header.frame_id.c_str()
                        );
       }
       catch(const std::exception& e)
       {
        //    std::cerr << e.what() << '\n';
        ROS_INFO("異常消息：%s",e.what());
       }
       
       
        rate.sleep();
        ros::spinOnce();
    }

    return 0;
}

最終的效果為：

值得注意的點是在剛剛啟動接收者節點時發出的異常警告： base_link 不存在
原因： 訂閱數據是一個耗時操作，可能在調用 transform 轉換函數時，坐標系的相對關系還沒訂閱到，因此出現異常

解決：
方案1：在調用轉換函數前，執行休眠；
方案2： 進行異常處理(建議使用)。