Multi coordinate transformation

Issued by

establish launch file

<launch>
    <!--  Release  son1  be relative to  world  as well as  son2  be relative to  world  The relationship between  -->
    <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> 

roscore
cd demo02_ws/
source ./devel/setup.bash
roslaunch tf03_tfs tfs_c.launch( Start... First )
rosrun tf03_tfs demo01_tfs_sub( Open a new window )

son1 be relative to son2 Information about : Parent : son2, Sub level : son1 Offset (2.00,0.00,0.00)
son1 be relative to son2 Information about : Parent : son2, Sub level : son1

Open a new window ：rviz

Subscriber

C++ Realization

#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  Some coordinate point of is  son2  The coordinate values in   technological process : 1.  Include header file  2.  code , initialization  NodeHandle 3.  Create subscription object  4.  Write parsing logic  5. spinOnce() */

int main(int argc, char *argv[])
{
    
    // 2.  code , initialization  NodeHandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tfs_sub");
    ros::NodeHandle nh;

    // 3.  Create subscription object   The header file 2/3
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

    // 4.  Write parsing logic 

    //  Create coordinate points   The header file 4/5
    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())
    {
    
        //  The core 
        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 relation frames with the shortest interval to calculate the relative relation   Return value : geometry_msgs::TransformStamped  The relative relationship between the source and the target coordinate system  */
            geometry_msgs::TransformStamped son1ToSon2 =  buffer.lookupTransform("son2","son1",ros::Time(0)); 
            //  Target coordinate system , Coordinate system source , Time 0 Means to find the message of the last two coordinate systems 
            ROS_INFO("son1  be relative to  son2  Information about :  Parent : %s, Sub level : %s  Offset (%.2f,%.2f,%.2f)",
                    son1ToSon2.header.frame_id.c_str(), //  Parent node  son2
                    son1ToSon2.child_frame_id.c_str(), //  Child node  son1
                    son1ToSon2.transform.translation.x,
                    son1ToSon2.transform.translation.y,
                    son1ToSon2.transform.translation.z
                    ); 
            // 2.  Calculation  son1  Some coordinate point of is  son2  The coordinate values in 
            geometry_msgs::PointStamped psAtSon2 = buffer.transform(psAtSon1,"son2"); 
            //  The former is the coordinate point to be converted , The latter is the transformed coordinate system 
            ROS_INFO(" Coordinate point at  Son2  The value 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();
    }
    
    // 5. spinOnce()

    return 0;
}

Python Realization

Converted coordinates :(3.00,2.00,3.00), Reference coordinate system : son2 Parent coordinate system : son2, Sub coordinate system : son1, Offset (2.00,0.00,0.00)

#! /usr/bin/env python


import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped

if __name__ == "__main__":

    # 2.  initialization 
    rospy.init_node("tfs_sub_p")

    # 3.  Create subscription object 
    # 3.1  Create a cache object   The header file 2
    buffer = tf2_ros.Buffer()
    # 3.2  Create subscription object ( Pass the cache into )
    sub = tf2_ros.TransformListener(buffer)

    # 4.  Organize the coordinate points to be converted   The header file 3
    ps = tf2_geometry_msgs.PointStamped()
    #  Time stamp --0
    # ps.header.stamp = rospy.Time() 
    ps.header.stamp = rospy.Time.now() # launch Static coordinate transformation is used 

    #  Coordinate system 
    ps.header.frame_id = "son1"
    ps.point.x = 1.0
    ps.point.y = 2.0
    ps.point.z = 3.0

    # 5.  Transformation logic implementation , call tf Encapsulated algorithm 
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        #  Use try prevent 
        try:
            # ------  Need to compute  son1  be relative to  son2  Coordinate relationship of   The header file 4
            """  Parameters 1:  Target coordinate system   Parameters 2:  Coordinate system source   Parameters 3: rospy.Time(0) ---  Take the frames of the two coordinate systems with the nearest time interval (son1 relative world, And son2 relative world) Calculation   Return value : son1  And  son2 Coordinate system relationship  """
            ts = buffer.lookup_transform("son2","son1",rospy.Time(0))
            rospy.loginfo(" Parent coordinate system : %s, Sub coordinate system : %s,  Offset (%.2f,%.2f,%.2f)",
                        ts.header.frame_id,
                        ts.child_frame_id,
                        ts.transform.translation.x,
                        ts.transform.translation.y,
                        ts.transform.translation.z
                        )

            #  Transformation implementation 
            ps_out = buffer.transform(ps,"son2") #  Coordinate points to be converted , Name of the target coordinate system 
            # 6.  Output results 
            rospy.loginfo(" Converted coordinates :(%.2f,%.2f,%.2f), Reference coordinate system : %s",
                        ps_out.point.x,
                        ps_out.point.y,
                        ps_out.point.z,
                        ps_out.header.frame_id)
        except Exception as e:
            rospy.logwarn(" Error message :%s",e) 
        
        rate.sleep()

Coordinate system relationship

stay vscode Open in the terminal ：ctrl+`
source ./devel/setup.bash
roslaunch tf03_tfs tfs_c.launch
rosrun tf2_tools view_frames.py
evince frames.pdf（ see pdf）