ROS中的多坐标变换详解（示例+代码）

肥肥胖胖是太阳

已于 2022-02-19 10:44:40 修改

阅读量3.3k

点赞数 1

分类专栏： ROS 文章标签：多坐标变换自动驾驶 ROS 坐标转换线性代数

于 2022-02-14 12:39:47 首次发布

本文链接：https://blog.csdn.net/weixin_45590473/article/details/122922092

版权

ROS 专栏收录该内容

36 篇文章 65 订阅

订阅专栏

命令行形式：another_static_pub.launch

发布方发布

坐标转换：another_static_sub.cpp

项目组织结构

运行结果

坐标变换的数学基础

四元数：

彻底搞懂“旋转矩阵/欧拉角/四元数”，让你体会三维旋转之美_超级霸霸强的博客-CSDN博客https://blog.csdn.net/weixin_45590473/article/details/122884112旋转矩阵：

坐标变换最通俗易懂的解释（推到+图解）_超级霸霸强的博客-CSDN博客https://blog.csdn.net/weixin_45590473/article/details/122848202

多坐标变换与静态/动态坐标变换的不同之处

tf坐标转换的实质

你也许会被静态/动态/多坐标转换弄晕了头，但是你只要认清坐标转换的实质就会恍然大悟。无论是哪种坐标转换，其实质就是利用tf2_ros::Buffer类对象中缓存的坐标间的相互关系将坐标点基于X坐标系的坐标转化至基于Y坐标系的坐标。

多坐标变换

我们以坐标系A、坐标系B、坐标系C、坐标系D三个坐标系相互转化为例进行说明：

准确的来说lookupTransfrom函数的作用是取得树状图中两个从未有联系的坐标系得相对关系（补齐树状图，使树状图中的每对元素都可以两两建立联系）：坐标系C与坐标系D的相对关系。可以取得树状图中已经建立联系的坐标系间的相对关系吗？可以，但是没有什么必要，如果我们仅仅使用已知的坐标系关系那么lookupTransfrom函数有无均可。

静态/动态坐标变换

我们会发现：两者都有的地方就是“调用transfrom函数进行坐标转换”。此外，两者不同之处在于坐标系间的转换关系获取，多坐标系转换中需要调用lookupTransfrom函数对buffer缓冲区内的数据进行复杂计算得到任意两个坐标系的转换关系，而静态/动态坐标系转换关系只需订阅static_tf话题即可得到。

坐标系相对关系的发布

坐标系相对关系的发布有两种形式：命令行形式和编写发布方源码。

命令行形式：another_static_pub.launch

<launch>  
    <node pkg="tf2_ros" name="son1" type="static_transform_publisher" args="1 2 3 10 30 20 /world /son1" output="screen"/>  
    <node pkg="tf2_ros" name="son2" type="static_transform_publisher" args="-1 -5 2 40 10 50 /world /son2" output="screen"/>  
    <node pkg="tf2_ros" name="son3" type="static_transform_publisher" args="-1 -5 2 40 10 50 /son1 /son3" output="screen"/>  
</launch>

上述形成的坐标系关系树状图如下所示：

注意：

当我们使用命令行工具向static_tf话题传送坐标系相对位置信息时，全部的坐标系不管如何相互联系最终都必须与world世界坐标系（世界坐标系是tf2中系统内定的全局参考坐标系）建立相互关系！

发布方发布

一个发布方只能发布使用tf2_ros:: StaticTransformBroadcaster::sendTransfrom函数发送一对坐标系的相对关系：

#include "ros/ros.h"  
#include "tf2_ros/static_transform_broadcaster.h"  
#include "tf2/LinearMath/Quaternion.h"  
#include "geometry_msgs/TransformStamped.h"  
  
int main(int argc,char* argv[])  
{  
    setlocale(LC_ALL,"");  
    // initial the node  
    ros::init(argc,argv,"static_pub");  
    // create publisher  
    tf2_ros::StaticTransformBroadcaster pub;  
    // organize the data  
    geometry_msgs::TransformStamped data;  
    data.header.frame_id = "base_link";  
    data.header.stamp = ros::Time::now();  
    data.child_frame_id = "laser";  
    // set translation  
    data.transform.translation.x = 0.1;  
    data.transform.translation.y = 0.5;  
    data.transform.translation.z = 0.3;  
    // set rotation  
    tf2::Quaternion qtn;  
    qtn.setRPY(30,10,5);  
    data.transform.rotation.w = qtn.getW();  
    data.transform.rotation.x = qtn.getX();  
    data.transform.rotation.y = qtn.getY();  
    data.transform.rotation.z = qtn.getZ();  
    // publish data  
    ros::Rate rate(1);  
    while(ros::ok()){  
        pub.sendTransform(data);  
        rate.sleep();  
    }  
    return 0;  
}

坐标转换：another_static_sub.cpp

#include "ros/ros.h"  
#include "tf2_ros/buffer.h"  
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"  
#include "tf2_ros/transform_listener.h"  
#include "geometry_msgs/TransformStamped.h"  
#include "geometry_msgs/PointStamped.h"  
  
int main(int argc,char* argv[])  
{  
    setlocale(LC_ALL,"");  
    ros::init(argc,argv,"another_static_sub");  
    tf2_ros::Buffer buffer;  
    tf2_ros::TransformListener listener(buffer);  
  
    ros::Rate rate(1);  
    while(ros::ok()){  
        try  
        {  
            // 对buffer中坐标系两两之间的关系全部计算出来存入buffer和变量Son12Son2中  
            geometry_msgs::TransformStamped Son12Son2 = buffer.lookupTransform("son2","son1",ros::Time(0));  
            geometry_msgs::PointStamped PointInSon1;  
            PointInSon1.header.frame_id = "son3";  
            PointInSon1.header.stamp = ros::Time(); // ros::Time(0)或者ros::Time()均可  
            PointInSon1.point.x = 1;  
            PointInSon1.point.y = 2;  
            PointInSon1.point.z = 3;  
            // 调用buffer中坐标系son2和坐标系son1之间的转换关系计算坐标点基于son2坐标系的坐标  
            geometry_msgs::PointStamped PointInSon2 = buffer.transform(PointInSon1,"son2");  
            ROS_INFO("PointInSon2:%f,%f,%f",PointInSon2.point.x,PointInSon2.point.y,PointInSon2.point.z);  
        }  
        catch(const std::exception& e)  
        {  
            std::cerr << e.what() << '\n';  
        }  
        rate.sleep();  
    }  
    return 0;  
}

注意：

该句程序中的Son12Son2变量虽然没被使用到，但是该句必须有。因为lookupTransfrom函数的作用不仅是输出一个表示坐标系转换关系的数据类型，同时也将表征着坐标系转换关系的数据类型存入buffer中。也就是说没有这一句，那么buffer就没有坐标系son1到son2的转换关系，进而transfrom函数也就无法调用坐标系转换关系完成坐标点坐标的转换。