2021.3.17| ROS靜態坐標變換和動態坐標變換

本文轉載自查看原文 2021-03-17 23:07 401

概念：ROS中是通過右手坐標系統來標定物體。

一、坐標msg消息

坐標變換實現中最常用的msg是geometry_msgs/TransformStamped和geometry_msgs/PointStamped

前者用於傳輸坐標系相關位置信息，后者用於傳輸某個坐標系內坐標點的信息。在坐標變換中，頻繁的需要使用到坐標系的相對關系以及坐標點信息。

1.geometry_msgs/TransformStamped

命令行鍵入 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

其中四元數表示坐標的相對姿態

2.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

參考：

http://docs.ros.org/en/api/geometry_msgs/html/msg/TransformStamped.html

http://docs.ros.org/en/api/geometry_msgs/html/msg/PointStamped.html

二、靜態坐標變換

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

分析：

坐標系相對關系，可以通過發布方發布
訂閱方，訂閱到發布的坐標系相對關系，再傳入坐標點信息(可以寫死)，然后借助於 tf 實現坐標變換，並將結果輸出

流程：

1.新建功能包，添加依賴。創建項目功能包依賴於 tf2、tf2_ros、tf2_geometry_msgs、roscpp std_msgs geometry_msgs

2.新建發布方的cpp文件，編寫發布方實現。編寫完畢還要配置CMakeLists.txt文件，三處，此略。

/* 
    靜態坐標變換發布方:
        發布關於 laser 坐標系的位置信息 

    實現流程:
        1.包含頭文件
        2.初始化 ROS 節點
        3.創建靜態坐標轉換廣播器
        4.創建坐標系信息
        5.廣播器發布坐標系信息
        6.spin()
*/


// 1.包含頭文件
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 節點
    ros::init(argc,argv,"static_brocast");
    // 3.創建靜態坐標轉換廣播器
    tf2_ros::StaticTransformBroadcaster broadcaster;
    // 4.創建坐標系信息
    geometry_msgs::TransformStamped ts;
    //----設置頭信息
    ts.header.seq = 100;
    ts.header.stamp = ros::Time::now();
    ts.header.frame_id = "base_link";
    //----設置子級坐標系
    ts.child_frame_id = "laser";
    //----設置子級相對於父級的偏移量
    ts.transform.translation.x = 0.2;
    ts.transform.translation.y = 0.0;
    ts.transform.translation.z = 0.5;
    //----設置四元數:將 歐拉角數據轉換成四元數
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,0);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();
    // 5.廣播器發布坐標系信息
    broadcaster.sendTransform(ts);
    ros::spin();
    return 0;
}

3.新建訂閱方的cpp文件，編寫訂閱方實現。編寫完畢還要配置CMakeLists.txt文件，三處，此略。

/*  
    訂閱坐標系信息，生成一個相對於 子級坐標系的坐標點數據，轉換成父級坐標系中的坐標點

    實現流程:
        1.包含頭文件
        2.初始化 ROS 節點
        3.創建 TF 訂閱節點
        4.生成一個坐標點(相對於子級坐標系)
        5.轉換坐標點(相對於父級坐標系)
        6.spin()
*/
//1.包含頭文件
#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" //注意: 調用 transform 必須包含該頭文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 節點
    ros::init(argc,argv,"tf_sub");
    ros::NodeHandle nh;
    // 3.創建 TF 訂閱節點
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    {
    // 4.生成一個坐標點(相對於子級坐標系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "laser";
        point_laser.header.stamp = ros::Time::now();
        point_laser.point.x = 1;
        point_laser.point.y = 2;
        point_laser.point.z = 7.3;
    // 5.轉換坐標點(相對於父級坐標系)
        //新建一個坐標點，用於接收轉換結果  
        //--------------使用 try 語句或休眠，否則可能由於緩存接收延遲而導致坐標轉換失敗------------------------
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"base_link");
            ROS_INFO("轉換后的數據:(%.2f,%.2f,%.2f),參考的坐標系是:",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());

        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序異常.....");
        }


        r.sleep();  
        ros::spinOnce();
    }


    return 0;
}

4.編譯執行。

三、動態坐標變換

需求：啟動 turtlesim_node,該節點中窗體有一個世界坐標系(左下角為坐標系原點)，烏龜是另一個坐標系，鍵盤控制烏龜運動，將兩個坐標系的相對位置動態發布。

分析：

烏龜本身不但可以看作坐標系，也是世界坐標系中的一個坐標點
訂閱 turtle1/pose,可以獲取烏龜在世界坐標系的 x坐標、y坐標、偏移量以及線速度和角速度
將 pose 信息轉換成坐標系相對信息並發布

流程：

新建功能包，添加依賴。創建項目功能包依賴於 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

創建坐標相對關系發布方(同時需要訂閱烏龜位姿信息)，新建發布方的cpp文件，編寫發布方實現。編寫完畢還要配置CMakeLists.txt文件，三處，此略。

/*  
    動態的坐標系相對姿態發布(一個坐標系相對於另一個坐標系的相對姿態是不斷變動的)

    需求: 啟動 turtlesim_node,該節點中窗體有一個世界坐標系(左下角為坐標系原點)，烏龜是另一個坐標系，鍵盤
    控制烏龜運動，將兩個坐標系的相對位置動態發布

    實現分析:
        1.烏龜本身不但可以看作坐標系，也是世界坐標系中的一個坐標點
        2.訂閱 turtle1/pose,可以獲取烏龜在世界坐標系的 x坐標、y坐標、偏移量以及線速度和角速度
        3.將 pose 信息轉換成 坐標系相對信息並發布

    實現流程:
        1.包含頭文件
        2.初始化 ROS 節點
        3.創建 ROS 句柄
        4.創建訂閱對象
        5.回調函數處理訂閱到的數據(實現TF廣播)
            5-1.創建 TF 廣播器
            5-2.創建 廣播的數據(通過 pose 設置)
            5-3.廣播器發布數據
        6.spin
*/
// 1.包含頭文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void doPose(const turtlesim::Pose::ConstPtr& pose){
    //  5-1.創建 TF 廣播器
    static tf2_ros::TransformBroadcaster broadcaster;
    //  5-2.創建 廣播的數據(通過 pose 設置)
    geometry_msgs::TransformStamped tfs;
    //  |----頭設置
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();

    //  |----坐標系 ID
    tfs.child_frame_id = "turtle1";

    //  |----坐標系相對信息設置
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0; // 二維實現，pose 中沒有z，z 是 0
    //  |--------- 四元數設置
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    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-3.廣播器發布數據
    broadcaster.sendTransform(tfs);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 節點
    ros::init(argc,argv,"dynamic_tf_pub");
    // 3.創建 ROS 句柄
    ros::NodeHandle nh;
    // 4.創建訂閱對象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
    //     5.回調函數處理訂閱到的數據(實現TF廣播)
    //        
    // 6.spin
    ros::spin();
    return 0;
}

創建坐標相對關系訂閱方。

//1.包含頭文件
#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" //注意: 調用 transform 必須包含該頭文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 節點
    ros::init(argc,argv,"dynamic_tf_sub");
    ros::NodeHandle nh;
    // 3.創建 TF 訂閱節點
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    {
    // 4.生成一個坐標點(相對於子級坐標系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "turtle1";
        point_laser.header.stamp = ros::Time();
        point_laser.point.x = 1;
        point_laser.point.y = 1;
        point_laser.point.z = 0;
    // 5.轉換坐標點(相對於父級坐標系)
        //新建一個坐標點，用於接收轉換結果  
        //--------------使用 try 語句或休眠，否則可能由於緩存接收延遲而導致坐標轉換失敗------------------------
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"world");
            ROS_INFO("坐標點相對於 world 的坐標為:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);

        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序異常:%s",e.what());
        }


        r.sleep();  
        ros::spinOnce();
    }


    return 0;
}

編譯執行。

參考：

免責聲明！

本站轉載的文章為個人學習借鑒使用，本站對版權不負任何法律責任。如果侵犯了您的隱私權益，請聯系本站郵箱yoyou2525@163.com刪除。

猜您在找 矩陣的坐標變換(轉) 坐標變換-tf::transform OpenGl從零開始之坐標變換（下）向量點的坐標變換——坐標系旋轉或坐標旋轉 TF靜態坐標轉換 ROS tf-增加坐標系 ROS探索總結（十二）——坐標系統 halcon學習筆記——傅里葉變換與極坐標變換三維坐標變換矩陣的推導過程 Halcon 位姿跟坐標變換的關系