ROS learning (22) TF transformation

---

Preface

   There are a large number of component elements in the robot's local and robot's working environment , The position and posture of different components will be involved in the design and application of robots , This requires the introduction of the concept of coordinate system and coordinate transformation .
   Coordinate transformation is a basic function commonly used in robot systems ,ROS The coordinate transformation system in is composed of TF Function pack maintenance .

One 、TF Function pack

  TF It is a function package that allows users to track multiple coordinate systems over time , Use a tree data structure , Buffer and maintain coordinate transformation relations among multiple coordinate systems according to time , Help developers at any time 、 Complete points between coordinate systems 、 Transformation of coordinates such as vectors .
  TF Can operate in a distributed system , All coordinate transformation relations in a robot system , All node components are available , All subscriptions TF The node of the message will buffer a transformation relationship data of all coordinate systems , So this structure does not need a central server to store any data .
   Want to use TF Function pack , In general, it takes two steps ：

1. monitor TF Transformation
   Receive and cache all coordinate transformation data published in the system , And query the required coordinate transformation relationship .
2. radio broadcast TF Transformation
   Broadcast the coordinate transformation relationship between coordinate systems in the system . There may be many different parts of the system TF Change broadcast , Each broadcast can directly insert the coordinate system transformation relationship TF In the tree , No more synchronization is needed .

Two 、TF Tools

The coordinate system involves the transformation between multiple spaces , Not easy to abstract , therefore TF It provides rich terminal tools to help developers debug and create TF Transformation .

1、tf_monitor

1） For printing TF Release status of all coordinate systems in the tree

rosrun tf tf_monitor

2） View the publishing status between the specified coordinate systems

rosrun tf tf_monitor <source_frame> <target_frame>

2、tf_echo

Used to view the transformation relationship between specified coordinate systems

rosrun tf tf_echo <source_frame> <target_frame>

3、static_transform_publisher

Used to publish static coordinate transformations between two coordinate systems , The relative position of these two coordinate systems does not change . The tool needs to set the offset parameters and rotation parameters of coordinates , The frequency of release is ms In units of .
There are two formats for commands , as follows ：
1） The rotation parameter uses yaw/pitch/roll angle

rosrun tf static_transform_publisher x y x yaw pitch roll frame_id child_frame_id period_in_ms

2） The rotation parameter uses quaternions

rosrun tf static_transform_publisher x y x qx qy qw frame_id child_frame_id period_in_ms

This command can also be used in launch Use... In the document , as follows ：

<launch>
  <node pkg="tf" type="static_transform_publisher" name="link_broadcaster" args="1 0 0 0 0 0 1 link_parent link 100" />
<launch>

4、view_frames

view_frames It is a visual debugging tool , Can generate pdf file , Show TF Tree information . The order is as follows ：

rosrun tf view_frames

see pdf file , You can use the following command ：

evince frames.pdf

3、 ... and 、 Turtle routine TF

Mainly used to understand TF The role of , And be familiar with the above TF Tool use , Feature Pack name is turtle_tf, The function pack installation commands are as follows ：

sudo apt-get install ros-kinetic-turtle-tf

function turtle_tf Function pack , The order is as follows ：

roslaunch turtle_tf turtle_tf_demo.launch

Open the keyboard control node , The order is as follows ：

rosrun turtlesim turtle_teleop_key

The effect is as follows ：

You can find , There are two turtles , Use the keyboard direction keys to control a turtle to move , You will find another turtle will follow the movement .
Its TF The trees are as follows ：

As shown above , There are three coordinate systems in the current system , Time coordinate system world、 Tortoise coordinate system turtle1 And tortoise coordinate system turtle2.
The world coordinate system is the basic coordinate system of the system , Other coordinate systems are established relative to this coordinate system , therefore world yes TF Root node of tree , The origin of the two tortoise coordinate systems is the coordinate position of the tortoise in the world coordinate system .
You can use the following command , Check the transformation relationship between the two tortoise coordinate systems ：

rosrun tf tf_monitor turtle1 turtle2

The effect is as follows ：

Four 、 Tortoise follows the routine code

Now let turtle2 Follow turtle1 motion , Equivalent to turtle2 The coordinate system is oriented to turtle1 Coordinate system movement , First new learning_tf Function pack .

1、 establish TF Broadcaster

Create a node , It is mainly used to publish the relationship between tortoise coordinate system and world coordinate system TF Transformation ,turtle_tf_broadcaster.cpp The contents are as follows ：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    
    // tf Broadcaster 
    static tf::TransformBroadcaster br;

    //  According to the turtle's current posture , Sets the coordinate transformation relative to the world coordinate system 
    tf::Transform transform;
    // Set translation transform 
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    // Set rotation transform 
    transform.setRotation(q);

    //  Insert coordinate transformation TF Tree and publish coordinate transformation 
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv)
{
    
    //  Initialize node 
    ros::init(argc, argv, "my_tf_broadcaster");
    if (argc != 2)
    {
    
        ROS_ERROR("need turtle name as argument"); 
        return -1;
    };
    turtle_name = argv[1];

    //  Subscribe to turtle's pose Information 
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

    ros::spin();

    return 0;
};

2、 establish TF Monitor

Create a node , Mainly used for monitoring TF news , Get from turtle2 be relative to turtle1 Transformation of coordinate system , To control turtle2 The movement of the .turtle_tf_listener.cpp The contents are as follows ：

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
    
    //  Initialize node 
    ros::init(argc, argv, "my_tf_listener");

    ros::NodeHandle node;

    //  Call through service , Produce a second turtle turtle2
    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle =
    node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    //  Definition turtle2 Speed control publisher 
    ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

    // tf Monitor 
    tf::TransformListener listener;
	// The listener will automatically receive TF Tree messages , And cache 10 second 
    ros::Rate rate(10.0);
    while (node.ok())
    {
    
        tf::StampedTransform transform;
        try
        {
    
            //  lookup turtle2 And turtle1 Coordinate transformation of 
            listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) 
        {
    
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        //  according to turtle1 and turtle2 Coordinate transformation between , Calculation turtle2 The linear velocity and angular velocity of motion are required 
        //  And issue speed control instructions , send turtle2 towards turtle1 Move 
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
                                        transform.getOrigin().x());
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                      pow(transform.getOrigin().y(), 2));
        turtle_vel.publish(vel_msg);

        rate.sleep();
    }
    return 0;
};

3、 Realize tortoise following movement

To write start_demo_with_listener.launch file , The contents are as follows ：

 <launch>
    <!--  Turtle simulator  -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <!--  Keyboard control  -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <!--  Two turtles tf radio broadcast  -->
    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle1" name="turtle1_tf_broadcaster" />
    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle2" name="turtle2_tf_broadcaster" />

    <!--  monitor tf radio broadcast , And control turtle2 Move  -->
    <node pkg="learning_tf" type="turtle_tf_listener"
          name="listener" />

 </launch>

Run the following command ：

roslaunch learning_tf start_demo_with_listener.launch 

The effect is as follows ：

Open the keyboard control node , The order is as follows ：

rosrun turtlesim turtle_teleop_key

Follow the effect as follows ：