| Note: This tutorial assumes you have completed the intermediate tutorials.. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Debugging tf problems
Description: This tutorial gives a systematic approach for debugging tf related problems.Tutorial Level: ADVANCED
tf is deprecated in favor of tf2. tf2 provides a superset of the functionality of tf and is actually now the implementation under the hood. If you're just learning now it's strongly recommended to use the tf2/Tutorials instead.
Contents
This tutorial walks you through the steps to debug a typical tf problem. It will apply the steps explained in the tf troubleshooting guide to an example using turtlesim. It will also use many of the tf debugging tools.
Starting the example
For this tutorial we set up a demo application which has a number of problems. The goal of this tutorial is to apply a systematic approach to find these problems.
First, let's just run an example and see what happens:
$ roslaunch turtle_tf start_debug_demo.launch
You'll see the turtlesim come up. If you select the terminal window from where you launched the demo, you can use the arrow keys to drive one of the robot around. In the upper left corner there is a second robot.
If the demo would be working correctly, this second robot should be following the robot you can command with the arrow keys. Obviously, it does not... because we have to solve some problems first. What you do see, is the following error message:
[ERROR] 1254263539.785016000: Frame id /turtle3 does not exist! When trying to transform between /turtle1 and /turtle3.
Finding the tf request
So, if you look at the debugging tf problems guide, you'll see we first need to find out what exactly we are asking tf to do. So, therefore we go into the part of the code that is using tf.
In the previous tutorials we created a tf broadcaster to publish the pose of a turtle to tf. In this tutorial we'll create a tf listener to start using tf.
How to create a tf listener
Let's first create the source files. Go to the package we created in the previous tutorial:
$ roscd learning_tf
Fire up your favorite editor and paste the following code into a new file called src/turtle_tf_listener_debug.cpp.
https://raw.github.com/ros/geometry_tutorials/hydro-devel/turtle_tf/src/turtle_tf_listener_debug.cpp
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("/turtle3", "/turtle1",
27 ros::Time::now(), 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 };
Take a look at lines 25-28: Here we do the actual request to tf. The first three arguments tell us directly what we are asking tf: transform from frame /turtle1 to frame /turtle3 at time "now".
Now, let's take a look at why this request to tf is failing.
Checking the Frames
First we want to find out if tf knows about our transform between /turtle3 and /turtle1:
$ rosrun tf tf_echo turtle3 turtle1
The output tells us that frame turtle3 does not exist:
Exception thrown:Frame id /turtle3 does not exist! When trying to transform between /turtle1 and /turtle3. The current list of frames is: Frame /turtle1 exists with parent /world. Frame /world exists with parent NO_PARENT. Frame /turtle2 exists with parent /world.
The last three lines of the above message tell us what frames do exist. If you like to get a graphical representation of this, type:
$ rosrun tf view_frames $ evince frames.pdf
And you'll get the following output.
So obviously the problem is that we are requesting turtle3, which does not exist. To fix this bug, replace turtle3 with turtle2 in lines 25-28:
And now stop the running demo (Ctrl-c), build it, and run it again:
$ make $ roslaunch learning_tf start_debug_demo.launch
And right away we run into the next problem:
[ERROR] 1254264620.183142000: You requested a transform that is 0.116 miliseconds in the past, but the most recent transform in the tf buffer is 3.565 miliseconds old. When trying to transform between /turtle1 and /turtle2.
Checking the Timestamp
Now that we solved the frame name problem, it is time to look at the timestamps. Remember we are trying to get the transform between turtle2 and turtle1 at time "now". To get statistics on the timing, run:
$ rosrun tf tf_monitor turtle2 turtle1
The result should look something like this:
RESULTS: for /turtle2 to /turtle1 Chain currently is: /turtle1 -> /turtle2 Net delay avg = 0.008562: max = 0.05632 Frames: Broadcasters: Node: /broadcaster1 40.01641 Hz, Average Delay: 0.0001178 Max Delay: 0.000528 Node: /broadcaster2 40.01641 Hz, Average Delay: 0.0001258 Max Delay: 0.000309
The key part here is the delay for the chain from turtle2 to turtle1. The output shows there is an average delay of 8 milliseconds. This means that tf can only transform between the turtles after 8 milliseconds are passed. So, if we would be asking tf for the transformation between the turtles 8 milliseconds ago instead of "now", tf would be able to give us an answer sometimes. Let's test this quickly by changing lines 25-28 to:
So in the new code we are asking for the transform between the turtles 100 milliseconds ago (Why not 8? Just to be safe...). Stop the demo (Ctrl-c), build and run:
Re-build your package at the top folder of your catkin workspace:
$ catkin_make
Then running the example again
$ roslaunch learning_tf start_debug_demo.launch
And you should finally see the turtle move! 
That last fix we made is not really what you want to do, it was just to make sure that was our problem. The real fix would look like this:
or like this:
