0.TF initial experience
http://wiki.ros.org/tf/Tutorials/Introduction%20to%20tf
1.Writing a tf broadcaster (C++)
In the next two tutorials, we will write code to reproduce the demonstration in the tf introductory tutorial. After that, the following tutorial will focus on extending the demo with more advanced tf features.
Before you start, you need to create a new ros package for this project. In the sandbox folder, create a file named learning_tf package, which depends on tf, roscpp, rospy and turnlsim:
$ cd %YOUR_CATKIN_WORKSPACE_HOME%/src $ catkin_create_pkg learning_tf tf roscpp rospy turtlesim
Before roscd, build your new package:
$ cd %YOUR_CATKIN_WORKSPACE_HOME%/ $ catkin_make $ source ./devel/setup.bash
2.How to broadcast transforms
This tutorial teaches you how to broadcast a coordinate system to tf. In this case, we want to broadcast the changing coordinate system when the turtle moves.
First create the source file. Go to the package we just created:
$ roscd learning_tf
2.1 code
Go to the src / folder and launch your favorite editor, and paste the following code into a file called src / Turbo_ Tf_ broadcaster.cpp In the new file.
1 #include <ros/ros.h> 2 #include <tf/transform_broadcaster.h> 3 #include <turtlesim/Pose.h> 4 5 std::string turtle_name; 6 7 8 9 void poseCallback(const turtlesim::PoseConstPtr& msg){ 10 static tf::TransformBroadcaster br; 11 tf::Transform transform; 12 transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); 13 tf::Quaternion q; 14 q.setRPY(0, 0, msg->theta); 15 transform.setRotation(q); 16 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name)); 17 } 18 19 int main(int argc, char** argv){ 20 ros::init(argc, argv, "my_tf_broadcaster"); 21 if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;}; 22 turtle_name = argv[1]; 23 24 ros::NodeHandle node; 25 ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback); 26 27 ros::spin(); 28 return 0; 29 };
2.2 code interpretation
#include <tf/transform_broadcaster.h>
The tf package provides an implementation of transform broadcaster to help simplify the task of publishing transformations. To use transform broadcast, we need to include tf / transform_ Broadcast. H header file.
static tf::TransformBroadcaster br;
Here, we create a transformbrroadcaster object that we will use later to wire the transformation.
tf::Transform transform; transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); tf::Quaternion q; q.setRPY(0, 0, msg->theta);
Here, we create a Transform object and copy the information from the 2D tortoise pose to the 3D Transform.
transform.setRotation(q);
Here, we set the rotation.
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
This is where the real work is done. Four parameters are required to send a transformation using transformrroadcaster.
First, we pass on the transformation itself.
Now, we need to timestamp the transformation to be published, we just need to mark it as the current time ROS:: time:: now().
Then we need to pass in the name of the parent frame of the link we created, in this case "world"
Finally, we need to pass the name of the subframe of the link we are creating, in this case the tortoise itself.
Note: sendTransform and StampedTransform have opposite order of parent and child.
2.3Running the broadcaster
Now that we've created the code, let's compile it first. open CMakeLists.txt File and add the following lines at the bottom:
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp) target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
Create your package; in the top folder of the catkin workspace:
$ catkin_make
If all goes well, on devel / lib / learning_ There should be one in the TF folder named turtle_ Tf_ The binary file for the broadcaster.
If so, we are ready to create a startup file for the presentation. Using your text editor, create a file named start_demo.launch And add the following lines:
<launch> <!-- Turtlesim Node--> <node pkg="turtlesim" type="turtlesim_node" name="sim"/> <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/> <!-- Axes --> <param name="scale_linear" value="2" type="double"/> <param name="scale_angular" value="2" type="double"/> <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" /> </launch>
First, make sure you have stopped the startup file from the previous tutorial (using ctrl-c). Now, you can start your own turtle broadcast demonstration:
$ roslaunch learning_tf start_demo.launch
2.4 Checking the results
Now, use tf_ The echo tool checks if the tortoise pose is actually broadcasting to tf:
$ rosrun tf tf_echo /world /turtle1
This should show you the position of the first turtle. Use the arrow keys to drive around the tortoise (make sure your terminal window is active, not the simulator window). If tf is running_ Echo will not see the transformation between the world and tortoise 2, because the second tortoise has not yet appeared. However, once we add a second tortoise in the next tutorial, tortoise 2's pose will be broadcast to tf.
3.Writing a tf listener (C++)
In the previous tutorial, we created a tf broadcaster to publish the turtle pose to tf. In this tutorial, we will create a tf listener to start using tf.
3.1 how to create a TF listener
First create the source file:
$ roscd learning_tf
3.2 code
Start your favorite editor and paste the following code into a file called Src / Turbo_ Tf_ listener.cpp In the new file.
1 #include <ros/ros.h> 2 #include <tf/transform_listener.h> 3 #include <geometry_msgs/Twist.h> 4 #include <turtlesim/Spawn.h> 5 6 int main(int argc, char** argv){ 7 ros::init(argc, argv, "my_tf_listener"); 8 9 ros::NodeHandle node; 10 11 ros::service::waitForService("spawn"); 12 ros::ServiceClient add_turtle = 13 node.serviceClient<turtlesim::Spawn>("spawn"); 14 turtlesim::Spawn srv; 15 add_turtle.call(srv); 16 17 ros::Publisher turtle_vel = 18 node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10); 19 20 tf::TransformListener listener; 21 22 ros::Rate rate(10.0); 23 while (node.ok()){ 24 tf::StampedTransform transform; 25 try{ 26 listener.lookupTransform("/turtle2", "/turtle1", 27 ros::Time(0), transform); 28 } 29 catch (tf::TransformException &ex) { 30 ROS_ERROR("%s",ex.what()); 31 ros::Duration(1.0).sleep(); 32 continue; 33 } 34 35 geometry_msgs::Twist vel_msg; 36 vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), 37 transform.getOrigin().x()); 38 vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + 39 pow(transform.getOrigin().y(), 2)); 40 turtle_vel.publish(vel_msg); 41 42 rate.sleep(); 43 } 44 return 0; 45 };
3.3 code interpretation
#include <tf/transform_listener.h>
tf package provides the implementation of TransformListener to help simplify the task of receiving transformation. To use the TransformListener, we need to include tf / transform_listener.h header file.
tf::TransformListener listener;
Here, we create a transformlistener object. After the listener is created, it begins to receive tf transforms over the network and buffers them for up to 10 seconds. The scope of the TransformListener object should be persistent, otherwise its cache cannot be populated, and almost every query fails. A common approach is to make the transformlistener object a member variable of a class.
try{ listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform); }
Here, the actual work is done, and we query the listener for a specific transformation. Let's look at four parameters:
1. We want to switch from frame / turnle1 to frame / turnle2.
2. The time we want to change. Providing ROS:: time (0) will only provide us with the latest available transformations.
3. We store the object of result transformation.
All of this is wrapped in a try catch block to catch possible exceptions.
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));
Here, the transformation is used to calculate the new linear and angular velocity according to the distance and angle between tortoise 2 and tortoise 1. The new speed will be in the theme "turnle2 / CMD"_ Vel "and the simulator will use this speed to update the motion of turnle2.
3.4Running the listener
Now that we've created the code, let's compile it first. open CMakeLists.txt File and add the following lines at the bottom:
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp) target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
Build the package in the top folder of the catkin workspace:
$ catkin_make
If all goes well, on devel / lib / learning_ There should be one in the TF folder named turtle_ Tf_ The binary of the listener.
If so, we are ready to add a startup file for the demo. Using a text editor, open the file named start_demo.launch And merge the following node blocks into the block:
<launch> ... <node pkg="learning_tf" type="turtle_tf_listener" name="listener" /> </launch>
First, make sure you have stopped the startup file from the previous tutorial (using ctrl-c). Now you can start the complete turtle demonstration:
$ roslaunch learning_tf start_demo.launch
As shown in the picture:
4.Adding a frame (C++)
In the previous tutorial, we recreated the turtle demo by adding tf broadcasters and tf listeners. This tutorial will teach you how to add additional frames to the tf tree. This is very similar to creating tf broadcasters and will show some of the tf's capabilities.
4.1 Why adding frames
For many tasks, it is easier to think in a local framework, such as in the framework of the laser scanner center, to infer the reason for laser scanning. tf allows you to define a local framework for each sensor, link, and so on in the system. Furthermore, the tf will handle all the extra frame conversions introduced.
4.2 Where to add frames
tf establishes the tree structure of the framework; it does not allow closed loops in the framework structure. This means that there is only one single parent in a frame, but there can be multiple children. Currently, our tf tree contains three frameworks: world, turnle1 and turnle2. Two tortoises are children of the world. If you want to add a new frame to tf, you need to parent one of the three existing frames and the new frame will become a subframe.
4.3 How to add a frame
In our tortoise example, we will add a new frame to the first tortoise. The frame will be the second tortoise's "carrot.". First create the source file. Go to the package we created for the previous tutorial:
$ roscd learning_tf
4.3.1 code
Start your favorite editor and paste the following code into a file called src / frame_tf_broadcaster.cpp In the new file.
1 #include <ros/ros.h> 2 #include <tf/transform_broadcaster.h> 3 4 int main(int argc, char** argv){ 5 ros::init(argc, argv, "my_tf_broadcaster"); 6 ros::NodeHandle node; 7 8 tf::TransformBroadcaster br; 9 tf::Transform transform; 10 11 ros::Rate rate(10.0); 12 while (node.ok()){ 13 transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) ); 14 transform.setRotation( tf::Quaternion(0, 0, 0, 1) ); 15 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1")); 16 rate.sleep(); 17 } 18 return 0; 19 };
4.3.2 code interpretation
transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) ); transform.setRotation( tf::Quaternion(0, 0, 0, 1) ); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
Here, we create a new transformation, from the parent turnle1 to the new child carrot1. The carrot1 frame is shifted 2 meters to the left from the turnle1 frame.
4.4 Running the frame broadcaster
Now that we've created the code, let's compile it first. open CMakeLists.txt File and add the following lines at the bottom:
add_executable(frame_tf_broadcaster src/frame_tf_broadcaster.cpp) target_link_libraries(frame_tf_broadcaster ${catkin_LIBRARIES})
Build the package in the top folder of the catkin workspace:
$ catkin_make
If all goes well, there should be a frame in the bin folder_ Tf_ The binary file for the broadcaster. If so, we are ready to edit start_demo.launch Start file. Simply merge the following node blocks into the startup block:
<launch> ... <node pkg="learning_tf" type="frame_tf_broadcaster" name="broadcaster_frame" /> </launch>
First, make sure that you have stopped the startup file from the previous tutorial (using Ctrl-c). Now you can start the tortoise broadcast demonstration:
$ roslaunch learning_tf start_demo.launch
5.Checking the results
So if you drive the first turtle, even if we add a new framework, you'll notice that its behavior doesn't change from the previous tutorial. This is because adding additional frames does not affect other frames, and our listener is still using the previously defined frames. So let's change the behavior of the listener. Open Src / Turbo_ Tf_ listener.cpp File and replace "/ turnle1" with "/ carrot1" on lines 26-27:
listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0), transform);
Now it's all right: rebuild and restart the tortoise demo, and you'll see the second tortoise after carrot1 instead of the first! Remember, carrot1 is 2 meters to the left of turnle1. There is no visual representation of the carrot, but you should see the second tortoise moving to that point.
$ catkin_make
$ roslaunch learning_tf start_demo.launch
6.Broadcasting a moving frame
The additional framework we released in this tutorial is a fixed framework, which does not change over time relative to the parent framework. However, if you want to publish the mobile framework, you can change the broadcaster to change over time. Let's modify the / carrot1 framework so that it changes over time relative to / turnle1.
transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) ); transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
Now it's ready: rebuild and restart the tortoise demo, and then you'll see the second tortoise following the carrot1 move.
$ catkin_make
$ roslaunch learning_tf start_demo.launch
5.Learning about tf and time (C++)
5.1 tf and Time
In the previous tutorial, we learned how tf tracks coordinate system trees. The tree changes over time, and tf stores time snapshots for each transformation (up to 10 seconds by default). So far, we have used the lookupTransform() function to access the latest available transformation in the tf tree without knowing when the transformation was recorded. This tutorial will teach you how to convert at a specific time.
$ roscd learning_tf
And open the file src / turtle_tf_listener.cpp . Take a look at lines 25-27:
try{ listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0), transform);
Let's let tutle2 follow tutle1, not carrot1. Change your code to the following:
try{ listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
You can also see that the time we specified is equal to 0. For tf, time 0 represents the latest available transition in the buffer. Now, change this line to get the conversion of the current time "now()":
try{ listener.lookupTransform("/turtle2", "/turtle1", ros::Time::now(), transform);
First, make sure you have stopped the startup file from the previous tutorial (using ctrl-c). Compile the code and run it again:
$ catkin_make
$ roslaunch learning_tf start_demo.launch
All of the sudden lookupTransform() failed, telling you repeatedly:
[ERROR] [1593244295.486927555]: "turtle2" passed to lookupTransform argument target_frame does not exist. [ERROR] [1593244296.487242916]: Lookup would require extrapolation into the future. Requested time 1593244296.487054225 but the latest data is at time 1593244296.485805631, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244297.487563375]: Lookup would require extrapolation into the future. Requested time 1593244297.487426933 but the latest data is at time 1593244297.478300250, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244298.487860078]: Lookup would require extrapolation into the future. Requested time 1593244298.487720426 but the latest data is at time 1593244298.485835624, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244299.488147246]: Lookup would require extrapolation into the future. Requested time 1593244299.488011257 but the latest data is at time 1593244299.477407355, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244300.488496293]: Lookup would require extrapolation into the future. Requested time 1593244300.488357332 but the latest data is at time 1593244300.485526880, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244301.488840603]: Lookup would require extrapolation into the future. Requested time 1593244301.488703340 but the latest data is at time 1593244301.477613318, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244302.489149021]: Lookup would require extrapolation into the future. Requested time 1593244302.489012258 but the latest data is at time 1593244302.485458383, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244303.489441347]: Lookup would require extrapolation into the future. Requested time 1593244303.489303431 but the latest data is at time 1593244303.477806639, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244304.489796913]: Lookup would require extrapolation into the future. Requested time 1593244304.489661855 but the latest data is at time 1593244304.486178673, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244305.490139069]: Lookup would require extrapolation into the future. Requested time 1593244305.490001497 but the latest data is at time 1593244305.477462519, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244306.490485060]: Lookup would require extrapolation into the future. Requested time 1593244306.490348978 but the latest data is at time 1593244306.485902964, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244307.490828487]: Lookup would require extrapolation into the future. Requested time 1593244307.490691740 but the latest data is at time 1593244307.478282159, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244308.491162073]: Lookup would require extrapolation into the future. Requested time 1593244308.491025430 but the latest data is at time 1593244308.485529031, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244309.491495458]: Lookup would require extrapolation into the future. Requested time 1593244309.491358303 but the latest data is at time 1593244309.477552194, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244310.491841852]: Lookup would require extrapolation into the future. Requested time 1593244310.491705238 but the latest data is at time 1593244310.485717984, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244311.492188325]: Lookup would require extrapolation into the future. Requested time 1593244311.492052189 but the latest data is at time 1593244311.477919606, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244312.492513610]: Lookup would require extrapolation into the future. Requested time 1593244312.492388180 but the latest data is at time 1593244312.486240605, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244313.492850102]: Lookup would require extrapolation into the future. Requested time 1593244313.492712866 but the latest data is at time 1593244313.477790258, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244314.493177217]: Lookup would require extrapolation into the future. Requested time 1593244314.493035425 but the latest data is at time 1593244314.486245855, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244315.493459044]: Lookup would require extrapolation into the future. Requested time 1593244315.493327800 but the latest data is at time 1593244315.478292697, when looking up transform from frame [turtle1] to frame [turtle2] [ERROR] [1593244316.493780943]: Lookup would require extrapolation into the future. Requested time 1593244316.493655831 but the latest data is at time 1593244316.485756488, when looking up transform from frame [turtle1] to frame [turtle2] [broadcaster_frame-7] killing on exit
Why is that? Each listener has a buffer in which all coordinate transformations from different tf broadcasters are stored. When the broadcaster issues a transition, it takes some time (usually a few milliseconds) for the transition to enter the buffer. Therefore, when you request frame conversion at "now" time, you should wait for a few milliseconds for that information to arrive.
5.2Wait for transforms
tf provides a nice tool that will wait until the transformation is available. Let's see what the code looks like:
try{ ros::Time now = ros::Time::now(); listener.waitForTransform("/turtle2", "/turtle1", now, ros::Duration(3.0)); listener.lookupTransform("/turtle2", "/turtle1", now, transform);
waitForTransform() takes four parameters:
1. Wait for conversion of this frame
2. … To this framework,
3. At this time
4. Timeout: do not wait longer than this maximum duration
Note: this example uses ROS:: time:: now(). Typically, this will be the timestamp of the data you want to transform.
As a result, waitForTransform() actually blocks until the transition between the two tortoises is available (which usually takes a few milliseconds), or - if the transition is not available - until the timeout is reached. First, make sure you have stopped the startup file from the previous tutorial (using ctrl-c). Now compile the network code and run it again:
$ catkin_make
$ roslaunch learning_tf start_demo.launch
You may still see an error (error messages may vary):
[ERROR] [1593245004.532610719]: Lookup would require extrapolation into the past. Requested time 1593245001.519151647 but the earliest data is at time 1593245002.121131312, when looking up transform from frame [turtle1] to frame [turtle2]
This happens because it takes a non-zero time for tutle2 to generate and start releasing tf frames. Therefore, the first time you request / turnle2 frame may not exist. When you request conversion, the conversion may not exist and fails for the first time. After the first transformation, all transformations exist and the tortoise behaves as expected.
5.3Checking the results
Now, you should be able to use the arrow keys again to bypass the first turtle (make sure your terminal window is active, not the simulator window), and then you will see the second turtle after the second one!
As a result, you notice that there is no significant difference in turtle behavior. That's because the actual Time difference is only a few milliseconds. But why do we change from Time (0) to now()? Just to teach you about tf buffers and their associated Time delays. For the actual tf use case, it is usually best to use Time (0).
6.Time travel with tf (C++)
6.1Time travel
So let's go back to the end of the previous tutorial. Go to your package for this tutorial:
$ roscd learning_tf
And open the file src / turtle_tf_listener.cpp . Take a look at lines 25-30:
try{ ros::Time now = ros::Time::now(); listener.waitForTransform("/turtle2", "/turtle1", now, ros::Duration(1.0)); listener.lookupTransform("/turtle2", "/turtle1", now, transform);
Now, instead of moving the second tortoise to where the first tortoise was, let the second tortoise move to where the first tortoise was five seconds ago:
try{ ros::Time past = ros::Time::now() - ros::Duration(5.0); listener.waitForTransform("/turtle2", "/turtle1", past, ros::Duration(1.0)); listener.lookupTransform("/turtle2", "/turtle1", past, transform);
So now, if you run this command, what do you want to see? Surely in the first five seconds, the second tortoise didn't know where to go, because we didn't have the history of the first tortoise for five seconds. But what happens in five seconds? Let's try:
$ make or catkin_make $ roslaunch learning_tf start_demo.launch
Is your tortoise driving uncontrollably like this screenshot? So what happened?
6.2 Advanced API for lookupTransform
So how can we ask such questions? The API enables us to say exactly when each frame is converted. The code is as follows:
try{ ros::Time now = ros::Time::now(); ros::Time past = now - ros::Duration(5.0); listener.waitForTransform("/turtle2", now, "/turtle1", past, "/world", ros::Duration(1.0)); listener.lookupTransform("/turtle2", now, "/turtle1", past, "/world", transform);
The advanced API for lookupTransform() has six parameters:
1. Convert from this frame,
2. At this time
3… To this frame,
4. At this time.
5. Specify a framework that does not change over time, in this case the "/ world" framework, and
6. Variables used to store results.
Note that like lookupTransform(), waitForTransform() has basic and advanced API s.
This figure shows the tf's operations in the background. In the past, it calculated the transition from the first tortoise to the world. Around the world, time goes from the past to the present. Now, tf accounting changes from the world to the second tortoise.
6.3Checking the results
$ make or catkin_make $ roslaunch learning_tf start_demo.launch