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 tf2 problems

Description: This tutorial gives a systematic approach for debugging tf2 related problems.

Tutorial Level: ADVANCED

This tutorial walks you through the steps to debug a typical tf2 problem. It will apply the steps explailed in the tf2 troubleshooting guide to an example using turtlesim. It will also use many of the tf2 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 build the example:

  $ roscd turtle_tf2
  $ rosmake

And, let's just run it to see what happens:

  $ roslaunch 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 tf2 request

So, if you look at the debugging tf2 problems guide, you'll see we first need to find out what exactly we are asking tf2 to do. So, therefore we go into the part of the code that is using tf2. Open the src/turtle_tf2_listener.cpp file. This is the code you'll see:

   1 #include <ros/ros.h>
   2 #include <tf2/transform_listener.h>
   3 #include <turtlesim/Velocity.h>
   4 #include <turtlesim/Spawn.h>
   5 
   6 int main(int argc, char** argv){
   7   ros::init(argc, argv, "my_tf2_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<turtlesim::Velocity>("turtle2/command_velocity", 10);
  19 
  20   tf2::TransformListener listener;
  21 
  22   ros::Rate rate(10.0);
  23   while (node.ok()){
  24     tf2::Stamped<tf2::Transform> transform;
  25     try{
  26       listener.lookupTransform("/turtle3", "/turtle1",
  27                                ros::Time::now(), transform);
  28     }
  29     catch (tf2::TransformException ex){
  30       ROS_ERROR("%s",ex.what());
  31       ros::Duration(1.0).sleep();
  32     }
  33 
  34     turtlesim::Velocity vel_msg;
  35     vel_msg.angular = 4 * atan2(transform.getOrigin().y(),
  36                                 transform.getOrigin().x());
  37     vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
  38                                 pow(transform.getOrigin().y(), 2));
  39     turtle_vel.publish(vel_msg);
  40 
  41     rate.sleep();
  42   }
  43   return 0;
  44 };

Take a look at lines 25-28: Error: No code_block found Here we do the actual request to tf2. The first three arguments tell us directly what we are asking tf2: transform from frame /turtle3 to frame /turtle1 at time "now".

Now, let's take a look at why this request to tf2 is failing.

Checking the Frames

First we want to find out if tf2 knows about our transform between /turtle3 and /turtle1:

  $ rosrun tf2 tf2_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 tf2_tools view_frames
  $ evince frames.pdf

And you'll get the following output.

frames.png

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:

   1   try{
   2     listener.lookupTransform("/turtle2", "/turtle1",
   3                              ros::Time::now(), transform);
   4   }

And now stop the running demo (Ctrl-c), build it, and run it again:

  $ make
  $ roslaunch 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 tf2 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 tf2 tf2_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 tf2 can only transform between the turtles after 8 milliseconds are passed. So, if we would be asking tf2 for the transformation between the turtles 8 milliseconds ago instead of "now", tf2 would be able to give us an answer sometimes. Let's test this quickly by changing lines 25-28 to:

   1   try{
   2     listener.lookupTransform("/turtle2", "/turtle1",
   3                              ros::Time::now()-ros::Duration(0.1), transform);
   4   }

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:

  $ make
  $ roslaunch start_debug_demo.launch

And you should finally see the turtle move! turtle_tf2_start.png

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:

   1   try{
   2     listener.lookupTransform("/turtle2", "/turtle1",
   3                              ros::Time(0), transform);
   4   }

or like this:

   1   try{
   2     ros::Time now = ros::Time::now();
   3     listener.waitForTransform("/turtle2", "/turtle1", 
   4                               now, ros::Duration(1.0));
   5     listener.lookupTransform("/turtle2", "/turtle1",
   6                              now, transform);
   7   }

Wiki: tf2/Tutorials/Debugging tf2 problems (last edited 2016-10-23 11:05:21 by StefanKohlbrecher)