Detailed explanation of multi coordinate transformation in ROS (example + code)

Catalog

Mathematical basis of coordinate transformation

Multi coordinate transformation and static / The difference of dynamic coordinate transformation

tf Essence of coordinate transformation

Multi coordinate transformation

static state / Dynamic coordinate transformation

Release of relative relations of coordinate system

Command line form ：another_static_pub.launch

The publisher publishes

Coordinate transformation ：another_static_sub.cpp

Project organization structure

  Running results

---

Mathematical basis of coordinate transformation

Four yuan number ：

Understand thoroughly “ Rotation matrix / Euler Angle / Four yuan number ”, Let you experience the beauty of three-dimensional rotation _ Super powerful blog -CSDN Blog https://blog.csdn.net/weixin_45590473/article/details/122884112 Rotation matrix ：

The most easy to understand explanation of coordinate transformation （ push to + The illustration ）_ Super powerful blog -CSDN Blog https://blog.csdn.net/weixin_45590473/article/details/122848202

Multi coordinate transformation and static / The difference of dynamic coordinate transformation

tf Essence of coordinate transformation

You may be static / dynamic / The multi coordinate transformation made me dizzy , But as long as you recognize the essence of coordinate transformation, you will suddenly realize . No matter what kind of coordinate transformation , Its essence is to make use of tf2_ros::Buffer The relationship between the coordinates cached in the class object bases the coordinate points on X The coordinates of the coordinate system are converted to be based on Y Coordinates of the coordinate system .

Multi coordinate transformation

We use the coordinate system A、 Coordinate system B、 Coordinate system C、 Coordinate system D The mutual transformation of three coordinate systems is illustrated as an example ：

To be exact lookupTransfrom The function is used to obtain the relative relationship between two coordinate systems that have never been connected in the tree view （ Complement tree view , Each pair of elements in the tree view can be connected in pairs ）： Coordinate system C And coordinate system D The relative relationship between . Can I get the relative relationship between the coordinate systems in the tree view ？ Sure , But there's no need , If we only use known coordinate system relations then lookupTransfrom Whether the function is present or not .

static state / Dynamic coordinate transformation

We will find that ： What both have is “ call transfrom Function to convert coordinates ”. Besides , The difference between the two lies in the transformation relationship between coordinate systems , In multi coordinate system transformation, you need to call lookupTransfrom Function pair buffer The data in the buffer is calculated to obtain the conversion relationship between any two coordinate systems , And static / Dynamic coordinate system transformation relations only need to subscribe to static_tf Topic can be .

Release of relative relations of coordinate system

There are two forms of publishing the relative relations of coordinate systems ： Command line form and write the source code of the publisher .

Command line form ：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> 

The tree view of the coordinate system relationship formed above is as follows ：

Be careful ：

When we use the command line tool to static_tf When the topic transmits the relative position information of the coordinate system , All coordinate systems, no matter how interconnected, must eventually be associated with world World coordinate system （ The world coordinate system is tf2 The global reference coordinate system defined in the system ） Build relationships ！

The publisher publishes

A publisher can only publish and use tf2_ros:: StaticTransformBroadcaster::sendTransfrom Function to send the relative relation of a pair of coordinate systems ：

#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;  
}  

Coordinate transformation ：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  
        {  
            //  Yes buffer The relationship between two pairs in the coordinate system is calculated and stored in buffer And variables Son12Son2 in   
            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) perhaps ros::Time() All possible   
            PointInSon1.point.x = 1;  
            PointInSon1.point.y = 2;  
            PointInSon1.point.z = 3;  
            //  call buffer The central coordinate system son2 And the coordinate system son1 The conversion relationship between the calculated coordinate points is based on son2 Coordinates of the coordinate system   
            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;  
}  

Be careful ：

In the program Son12Son2 Variables are not used , But the sentence must have . because lookupTransfrom The function is not only used to output a data type that represents the transformation relationship of the coordinate system , At the same time, the data type representing the transformation relationship of the coordinate system is also stored in buffer in . That is to say, there is no such sentence , that buffer There is no coordinate system son1 To son2 The transformation of , , in turn, transfrom The function cannot call the coordinate system transformation relationship to complete the transformation of coordinate points .

Why not use a CPP Source files to complete coordinate transformation ？ 

Why not use a CPP Source file to complete coordinate transformation ？_ Super powerful blog -CSDN Blog https://blog.csdn.net/weixin_45590473/article/details/123015364

Project organization structure

  Running results