Multi coordinate transformation

Requirements describe :

Existing coordinate system , Parent coordinate system world, There are two subsystems son1,son2,son1 be relative to world, as well as son2 be relative to world The relationship of is known , seek son1 The origin is son2 The coordinates of , Also known in son1 The coordinates of the middle point , It is required to find the point at son2 The coordinates of

Implementation analysis :

1. First , Need to publish son1 be relative to world, as well as son2 be relative to world Coordinate message for
2. then , You need to subscribe to coordinate publishing messages , And get the subscribed messages , With the help of tf2 Realization son1 and son2 Transformation
3. Last , Also realize the transformation of coordinate points

Implementation process :C++ And Python Achieve process consistency

1. New function pack , Add dependency

2. Create a coordinate relative relationship publisher ( You need to publish the relative relationship between the two coordinates )

3. Create a coordinate relative relationship subscriber

4. perform

1. Create Feature Pack tf03_tfs

Creating a project Feature Pack depends on tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2. Issued by   tfs_c.launch

For convenience , Publish using static coordinate transformation   That is to say ROS The underlying program , Be similar to turtlesim turtlesim_node

<launch>
      <!--  Release son1 be relative to world as well as son2 be relative to world Coordinate relationship of  -->
      <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="5 0 0 0 0 0 /world /son1" output="screen" />
      <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="3 0 0 0 0 0 /world /son2" output="screen" />
</launch>

among ags yes son1 amount to world Euler angle and offset of

start-up ：   

roslaunch tf03_tfs tfs_c.launch

rviz

3. Subscriber  

#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"
#include "geometry_msgs/TransformStamped.h"

/*
      The subscriber implements ：1 Calculation son1 And son2 The relative relationship between  2 Calculation son1 And then son2 The coordinate values in 
*/
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tfs_sub");
    ros::NodeHandle nh;
    
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);
    
    //  Create coordinate points 
    geometry_msgs::PointStamped psAtSon1;
    
    psAtSon1.header.stamp = ros::Time::now();
    psAtSon1.header.frame_id = "son1";
    psAtSon1.point.x = 1.0;
    psAtSon1.point.y = 2.0;
    psAtSon1.point.z = 3.0;

    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {
            //1 Calculation son1 And son2 The relative relationship between 
            /*
            A be relative to B Coordinate system relationship of 

             Parameters 1： Target coordinate system   B
             Parameters 2： Coordinate system source     A
             Parameters 3：ros::Time(0)  Take the two coordinate key frames with the shortest interval to calculate the relative relationship 
             Return value ：geometry_msgs::TransformStamped  The relative relationship between the source and the target coordinate system 

            */
            geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("son1","son2",ros::Time(0));
            ROS_INFO("son1 be relative to son2 Information about : Parent :%s, Sub level :%s  Offset (%.2f,%.2f,%.2f) ",
                      son1ToSon2.header.frame_id.c_str(),
                      son1ToSon2.child_frame_id.c_str(),
                      son1ToSon2.transform.translation.x,
                      son1ToSon2.transform.translation.y,
                      son1ToSon2.transform.translation.z
                      );
            //2 Calculation son1 And then son2 The coordinate values in 
            geometry_msgs::PointStamped psAtSon2 = buffer.transform(psAtSon1,"son2");
            ROS_INFO(" Coordinate point re son2 The coordinate values in (%.2f,%.2f,%.2f)",
                      psAtSon2.point.x,
                      psAtSon2.point.y,
                      psAtSon2.point.z
                      );
        }
        catch(const std::exception& e)
        {
            ROS_INFO(" Error message ：%s",e.what());
        }
        
        rate.sleep();
        ros::spinOnce();
    }
    
    return 0;
}

  Coordinate system relationship view

In robot systems , There are multiple coordinate systems involved , For easy viewing ,ros Special tools are provided , It can be used to generate pdf file , This file contains the coordinate system Atlas of tree structure .

First call rospack find tf2_tools Check whether the feature pack is included , without , Please use the following command to install :

sudo apt install ros-noetic-tf2-tools

Generate pdf file

After starting the coordinate system broadcast program , Run the following command :

rosrun tf2_tools view_frames.py 

It will generate log information similar to the following :

[INFO] [1592920556.827549]: Listening to tf data during 5 seconds...
[INFO] [1592920561.841536]: Generating graph in frames.pdf file... 

Viewing the current directory will generate a frames.pdf file

see file

You can directly enter the directory to open the file , Or call the command to view the file :evince frames.pdf