9. ROS TF coordinate transformation (3): Coordinate system relationship viewing and a case

1 Check the current coordinate system changes

We first install the function package:
sudo apt install ros-melodic-tf2-tools
Successful installation!

We first start the node that published the coordinate transformation last time:
liuhongwei@liuhongwei-Legion-Y9000P-IRX8H:~/Desktop/final/my_catkin$ source devel/setup.bash 
liuhongwei@liuhongwei-Legion-Y9000P-IRX8H:~/Desktop/final/my_catkin$ roslaunch test tf_son1_son2.launch 
<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0 0 0 0 0 /world /son2" output="screen" />
</launch>
The current coordinate system relationship has been released.

We check the current coordinate relationship:
rosrun tf2_tools view_frames.py
liuhongwei@liuhongwei-Legion-Y9000P-IRX8H:~$ rosrun tf2_tools view_frames.py
[INFO] [1701501936.157621]: Listening to tf data during 5 seconds...
[INFO] [1701501941.163282]: Generating graph in frames.pdf file...
liuhongwei@liuhongwei-Legion-Y9000P-IRX8H:~$ 
A pdf file is generated.

out

2 Case: Spawn two turtles and let one follow the other

2.1 Requirements

At the beginning of the program startup: two turtles are generated, the middle turtle (A) and the lower left turtle (B). B will automatically run to the position of A, and when controlled by the keyboard, it only controls the movement of A, but B can follow A.

2.2 Implementation analysis

The core of the turtle following implementation is that turtles A and B both publish coordinate information relative to the world coordinate system. Then, subscribing to this information requires conversion to obtain the information of A relative to the coordinate system of B. Finally, the speed information is generated and controls B. sports.

Start the turtle display node

Generate a new turtle in the turtle display form (service required)

Write nodes for two turtles to publish coordinate information

Write a subscription node to subscribe to coordinate information and generate new relative relationships to generate speed information.

Implementation process:

Create a new function package and add dependencies

Write a service client to generate a new turtle

Write a publisher to publish the coordinate information of the two turtles

Write a subscriber, subscribe to two turtle information, generate speed information and publish it

run

2.3 Implementation

2.3.1 Implementation 1--Start turtle 1 and generate turtle 2: GUI

Create gen_turtle.cpp to create the second turtle:

//
// Created by liuhongwei on 23-12-2.
//
/*
    创建第二只小乌龟
 */
#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc, char *argv[])
{

    setlocale(LC_ALL,"");

    //执行初始化
    ros::init(argc,argv,"create_turtle");
    //创建节点
    ros::NodeHandle nh;
    //创建服务客户端
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");

    ros::service::waitForService("/spawn");
    turtlesim::Spawn spawn;
    spawn.request.name = "turtle2";
    spawn.request.x = 1.0;
    spawn.request.y = 2.0;
    spawn.request.theta = 3.12415926;
    bool flag = client.call(spawn);
    if (flag)
    {
        ROS_INFO("乌龟%s创建成功!",spawn.response.name.c_str());
    }
    else
    {
        ROS_INFO("乌龟2创建失败!");
    }

    ros::spin();

    return 0;
}

Put it in the launch file:

<launch>
    <!-- 启动乌龟节点与键盘控制节点 -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/>
    <!-- 启动创建第二只乌龟的节点 -->
    <node pkg="test" type="generate_turtle" name="turtle2" output="screen" />

</launch>

Start the node: roslaunch test generate_turtle.launch

All functions are implemented!

So how to control the movement of the second turtle? Let’s think about what to do!

2.3.2 Publish turtle coordinate system correlation

Publish the relationship between the two turtles relative to the world coordinate system:

        Basically the same as before, here we reuse the previous code:

1.Comparison between TF2 and TF_Introduction

TF2 has replaced TF. TF2 is a superset of TF. It is recommended to learn TF2 instead of TF.

The TF2 function package has enhanced cohesion. The function packages that TF and TF2 depend on are different. TF corresponds to the tf package, and TF2 corresponds to the tf2 and tf2_ros packages, different types of API implementations are subcontracted in TF2.

TF2 implementation is more efficient, such as: TF2 static coordinate implementation, Buffer implementation in TF2 coordinate transformation listener, etc.

2.Comparison between TF2 and TF_Static coordinate transformation demonstration

Next, we demonstrate the implementation efficiency of TF2 through static coordinate transformation.

2.1 Start the static coordinate transformation of TF2 and TF versions

TF2 version static coordinate transformation:rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 /base_link /laser

TF version static coordinate transformation:rosrun tf static_transform_publisher 0 0 0 0 0 0 /base_link /laser 100

You will find that there is one more parameter at the end of the startup of the TF version, which specifies the release frequency.

2.2 Comparison of running results

Userostopic to view topics, including /tf and /tf_static, the former is a topic published by TF, and the latter is TF2 Posted topics, call the commands to print the topic messages of the two topics

rostopic echo /tf: Currently, coordinate system information will be output in a loop

rostopic echo /tf_static: Coordinate system information only once

2.3 Conclusion

If it is a static coordinate conversion, then the relative status between different coordinate systems is fixed. Since it is fixed, there is no need to repeatedly publish the conversion message of the coordinate system. Obviously, the tf2 implementation is more efficient than tf.

        We write it into the launch file.

<launch>
    
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/>
    
    <node pkg="test" type="generate_turtle" name="turtle2" output="screen" />
    
    <node pkg="test" type="generate_turtle_locate" name="caster1" output="screen" args="turtle1" />
    <node pkg="test" type="generate_turtle_locate" name="caster2" output="screen" args="turtle2" />

</launch>
Start the launch file: roslaunch test generate_turtle.launch

Let’s open RVIZ to visualize:

2.3.3 Subscribe to coordinate information and generate speed information

Subscription to the TF broadcast information of turtle1 and turtle2, find and convert the latest TF information
Convert turtle1 into coordinates relative to turtle2, calculate the linear velocity and angular velocity and publish them a>

        Implementation process:
        1. Include header file
        2. Initialize ros node
        3. Create ros handle
        4. Create TF subscription object
        5. Process the subscribed TF
        6.spin

First seeing how to get from t1 to t2:

Their straight-line distance is $\sqrt{x^2+y^2}$ , and the angle is $bind(y,x)$ . We multiply it by a parameter to get the speed and angular velocity.

            //5-1.先获取 turtle1 相对 turtle2 的坐标信息
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));

            //5-2.根据坐标信息生成速度信息 -- geometry_msgs/Twist.h
            geometry_msgs::Twist twist;
            twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));
            twist.angular.z = 4 * atan2(tfs.transform.translation.y,tfs.transform.translation.x);

            //5-3.发布速度信息 -- 需要提前创建 publish 对象
            pub.publish(twist);

So we get the angular velocity, linear velocity and publish them.

//
// Created by liuhongwei on 23-12-2.
//
/*
    订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息
    将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布

    实现流程:
        1.包含头文件
        2.初始化 ros 节点
        3.创建 ros 句柄
        4.创建 TF 订阅对象
        5.处理订阅到的 TF
        6.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"sub_TF");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 TF 订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
    // 5.处理订阅到的 TF

    // 需要创建发布 /turtle2/cmd_vel 的 publisher 对象

    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",1000);

    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {
            //5-1.先获取 turtle1 相对 turtle2 的坐标信息
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));

            //5-2.根据坐标信息生成速度信息 -- geometry_msgs/Twist.h
            geometry_msgs::Twist twist;
            twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));
            twist.angular.z = 4 * atan2(tfs.transform.translation.y,tfs.transform.translation.x);

            //5-3.发布速度信息 -- 需要提前创建 publish 对象
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("错误提示:%s",e.what());
        }



        rate.sleep();
        // 6.spin
        ros::spinOnce();
    }

    return 0;
}

We execute the launch file.

OK.

3 Summary

Coordinate transformation is an extremely important component module in the robot system. In ROS, the TF2 component is specially used to implement coordinate transformation. The specific content of TF2 implementation mainly introduces the following parts:

1. Static coordinate transformation broadcaster can be implemented by coding or by calling the built-in function package (the latter is recommended), and is suitable for relatively fixed coordinate system relationships.

2. Dynamic coordinate transformation broadcaster broadcasts the relative relationship between coordinate systems in a coded manner, which is suitable for variable coordinate system relationships.

3. The coordinate transformation listener is used to listen to the coordinate system messages broadcast by the broadcaster, and can realize the transformation between different coordinate systems or the same point between different coordinate systems.

4. The coordinate system relationship in the robot system is relatively complex. You can also use the tf2_tools toolkit to generate the coordinate system relationship diagram in ros.

5. Currently, TF2 has replaced TF. The official website recommends learning TF2 directly, and the usage process of TF and TF2 is similar to that of implementing API. As long as there is experience in using either party, the other party can also learn by analogy.

1.Comparison between TF2 and TF_Introduction

TF2 has replaced TF. TF2 is a superset of TF. It is recommended to learn TF2 instead of TF.

The TF2 function package has enhanced cohesion. The function packages that TF and TF2 depend on are different. TF corresponds to the tf package, and TF2 corresponds to the tf2 and tf2_ros packages, different types of API implementations are subcontracted in TF2.

TF2 implementation is more efficient, such as: TF2 static coordinate implementation, Buffer implementation in TF2 coordinate transformation listener, etc.

2.Comparison between TF2 and TF_Static coordinate transformation demonstration

Next, we demonstrate the implementation efficiency of TF2 through static coordinate transformation.

2.1 Start the static coordinate transformation of TF2 and TF versions

TF2 version static coordinate transformation:rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 /base_link /laser

TF version static coordinate transformation:rosrun tf static_transform_publisher 0 0 0 0 0 0 /base_link /laser 100

You will find that there is one more parameter at the end of the startup of the TF version, which specifies the release frequency.

2.2 Comparison of running results

Userostopic to view topics, including /tf and /tf_static, the former is a topic published by TF, and the latter is TF2 Posted topics, call the commands to print the topic messages of the two topics

rostopic echo /tf: Currently, coordinate system information will be output in a loop

rostopic echo /tf_static: Coordinate system information only once

2.3 Conclusion

If it is a static coordinate conversion, then the relative status between different coordinate systems is fixed. Since it is fixed, there is no need to repeatedly publish the conversion message of the coordinate system. Obviously, the tf2 implementation is more efficient than tf.