One 、 introduction

In the parent coordinate system world Next , There are two sub coordinate systems son1,son2, The relationship between the two child coordinate systems and the parent coordinate system is known , Find out son1 be relative to son2 The relationship between the coordinate origin of , as well as son1 A point in is relative to son2 Relationship in .
At this time , We can release it first son1,son2 be relative to world Coordinate relationship of , Then create TF Subscription object , adopt tf2 The function package completes coordinate conversion

Two 、C++ Realization

• Function package dependency ：tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

1. Publisher implementation

<launch>
    <!--  Issue through command parameters  -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.3 0.4 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0.7 0.7 0 0 0 /world /son2" output="screen" />
</launch>

2. The subscriber implements

// 1. Include header file 
#include"ros/ros.h"
#include"tf2_ros/transform_listener.h"
#include"tf2_geometry_msgs/tf2_geometry_msgs.h"
#include"geometry_msgs/TransformStamped.h"
#include"geometry_msgs/PointStamped.h"
int main(int argc, char *argv[]){
    
    setlocale(LC_ALL,"");
    // 2. initialization ros node 
    ros::init(argc,argv,"sub_frames");
    // 3. establish ros Handle 
    ros::NodeHandle nh;
    // 4. establish TF Subscription object 
    tf2_ros::Buffer buffer;// Store subscription messages 
    tf2_ros::TransformListener listener(buffer);
    // 5. Parse the subscription information to get son1 The origin of the coordinates is son2 The coordinates of 
    ros::Rate r(1);
    while (ros::ok())
    {
    
        try
        {
    
            // ros::Time(0)： It means take son1,son2 Recent relationship as a reference  son2: Parent coordinate system , Target coordinate system  son1： Sub coordinate system 
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));
            ROS_INFO(" Parent coordinate system :%s\n, Sub coordinate system ：%s\n, Offset ：x=%.2f,y=%.2f,z=%.2f\n",
                tfs.header.frame_id.c_str(),
                tfs.child_frame_id.c_str(),
                tfs.transform.translation.x,
                tfs.transform.translation.y,
                tfs.transform.translation.z
            );
            //  Coordinate point analysis 
            geometry_msgs::PointStamped ps;
            ps.header.frame_id = "son1";
            ps.header.stamp = ros::Time::now();
            ps.point.x = 1.0;
            ps.point.y = 2.0;
            ps.point.z = 3.0;

            geometry_msgs::PointStamped ps_at_son2;
            ps_at_son2 = buffer.transform(ps,"son2");
            ROS_INFO(" stay son2 The coordinates of ：x=%.2f,y=%.2f,z=%.2f\n",
            ps_at_son2.point.x,
            ps_at_son2.point.y,
            ps_at_son2.point.z
            );
        }
        catch(const std::exception& e)
        {
    
            // std::cerr << e.what() << '\n';
            ROS_INFO(" Abnormal information ：%s",e.what());
        }
        
        r.sleep();
        // 6.spin
        ros::spinOnce();
    }
    return 0;
}

• Running results

  [ INFO] [1658377161.418557965]:  Parent coordinate system :son2
  , Sub coordinate system ：son1
  , Offset ：x=-0.30,y=-0.40,z=-0.30
  
  [ INFO] [1658377161.418658173]:  stay son2 The coordinates of ：x=0.70,y=1.60,z=2.70
  

3、 ... and 、Python Realization

1. Publisher implementation

<launch>
    <!--  Issue through command parameters  -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.3 0.4 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0.7 0.7 0 0 0 /world /son2" output="screen" />
</launch>

2. The subscriber implements

#!/usr/bin/env python
# 1. Guide pack 
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
from tf2_geometry_msgs import PointStamped

if __name__ == "__main__":
    # 2. initialization ROS node 
    rospy.init_node("frames_sub_p")
    # 3. establish TF Subscription object 
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)
    rate = rospy.Rate(1)

    while not rospy.is_shutdown():
        try:
            # 4. call API Find out son1 be relative to son2 Relative coordinate relation of 
            # lookup_transform(target_frame,source_frame,time)
            # rospy.Time(0) finger son1,son2 be relative to world The latest relationship of the coordinate system 
            tfs = buffer.lookup_transform("son2","son1",rospy.Time(0))
            rospy.loginfo("son1 And son2 The relative relationship between ：")
            rospy.loginfo(" Parent coordinate system ：%s, Sub coordinate system ：%s",tfs.header.frame_id,tfs.child_frame_id)
            rospy.loginfo(" Relative coordinates ：x=%.2f,y=%.2f,z=%.2f",
                            tfs.transform.translation.x,
                            tfs.transform.translation.y,
                            tfs.transform.translation.z
                            )
            # 5. Create a son1 Coordinate points in , Find the point at son2 The coordinates of 
            ps = PointStamped()
            ps.header.frame_id = "son1"
            ps.header.stamp = rospy.Time.now()
            ps.point.x = 1
            ps.point.y = 1
            ps.point.z = 1
            ps_target = buffer.transform(ps,"son2",rospy.Duration(0.5))
            rospy.loginfo("ps_target Coordinate system ：%s",ps_target.header.frame_id)
            rospy.loginfo(" be relative to son2 The coordinate point of :(%.2f,%.2f,%.2f)",
            ps_target.point.x,
            ps_target.point.y,
            ps_target.point.z
            )
        except Exception as e:
            rospy.logerr(" Error message ：%s",e)
        rate.sleep()
    # 6.spin
    # rospy.spin()

• Running results

  [INFO] [1658395990.515741]: son1 And son2 The relative relationship between ：
  [INFO] [1658395990.517491]:  Parent coordinate system ：son2, Sub coordinate system ：son1
  [INFO] [1658395990.519920]:  Relative coordinates ：x=-0.30,y=-0.40,z=-0.30
  [INFO] [1658395990.522142]: ps_target Coordinate system ：son2
  [INFO] [1658395990.524224]:  be relative to son2 The coordinate point of :(0.70,0.60,0.70)
  

Four 、 Coordinate system relationship view

stay ros In the system , The coordinate system Atlas of tree structure can be generated through the corresponding function package

1. Get ready
   Check whether the corresponding function package is installed ：rospack find tf2_tools
   If not installed ：sudo apt install ros-noetic-tf2-tools
2. Use
   rosrun tf2_tools view_frames.py
   A PDF file , And output the following information ：

   rosrun tf2_tools view_fram
   es.py 
   Unable to register with master node [http://localhost:11311]: master may not be running yet. Will keep trying.
   [INFO] [1658400722.910123]: Listening to tf data during 5 seconds...
   [INFO] [1658400727.919585]: Generating graph in frames.pdf file...