tf教程（七）：Debugging tf problems

Debugging tf problems

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

Tutorial Level:  ADVANCED 

目录

1. Starting the example
2. Finding the tf request
3. How to create a tf listener
4. Checking the Frames
5. Checking the Timestamp

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.h>
   2 #include /transform_listener.h>
   3 #include /Twist.h>
   4 #include /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 /turtle3 to frame /turtle1 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 turtle2in 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 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:

切换行号显示

   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:

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:

切换行号显示

   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   }