Note: This tutorial assumes you have completed the adding a frame tutorial (Python) (C++).
(!) 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.

Learning about tf and time (C++)

Description: This tutorial teaches you to use the waitForTransform function to wait for a transform to be available on the tf tree.

Tutorial Level: INTERMEDIATE

Next Tutorial: Time travel with tf (C++)

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.

tf and Time

In the previous tutorials we learned about how tf keeps track of a tree of coordinate frames. This tree changes over time, and tf stores a time snapshot for every transform (for up to 10 seconds by default). Until now we used the lookupTransform() function to get access to the latest available transforms in that tf tree, without knowing at what time that transform was recorded. This tutorial will teach you how to get a transform at a specific time.

So let's go back to where we ended in the adding a frame tutorial. Go to your package for the tutorial:

  $ 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 make the second turtle follow the first turtle, and not the carrot. Change your code to the following:

   try{
      listener.lookupTransform("/turtle2", "/turtle1",  
                               ros::Time(0), transform);

You can also see we specified a time equal to 0. For tf, time 0 means "the latest available" transform in the buffer. Now, change this line to get the transform at the current time, "now()":

  try{
    listener.lookupTransform("/turtle2", "/turtle1",  
                             ros::Time::now(), transform);

First, make sure you stopped the launch file from the previous tutorial (use ctrl-c). Compile the code, and run it again:

  $ catkin_make

  $ make

  $ roslaunch learning_tf start_demo.launch

So, all of the sudden lookupTransform() is failing, telling you repeatedly:

[ERROR] [1287871653.885277559]: You requested a transform that is 0.018 miliseconds in the past, but the most recent transform in the tf buffer is 7.681 miliseconds old.                                                 
 When trying to transform between /turtle1 and /turtle2.  
....

Why is that? Well, each listener has a buffer where it stores all the coordinate transforms coming from the different tf broadcasters. When a broadcaster sends out a transform, it takes some time before that transform gets into the buffer (usually a couple of milliseconds). So, when you request a frame transform at time "now", you should wait a few milliseconds for that information to arrive.

Wait for transforms

tf provides a nice tool that will wait until a transform becomes available. Let's look at what the code would look like:

  try{
    ros::Time now = ros::Time::now();
    listener.waitForTransform("/turtle2", "/turtle1",
                              now, ros::Duration(3.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             now, transform);

The waitForTransform() takes four arguments:

  1. Wait for the transform from this frame...
  2. ... to this frame,
  3. at this time, and
  4. timeout: don't wait for longer than this maximum duration

Note: The use of ros::Time::now() is for this example. Usually this would be the timestamp of the data wishing to be transformed.

So waitForTransform() will actually block until the transform between the two turtles becomes available (this will usually take a few milliseconds), OR --if the transform does not become available-- until the timeout has been reached.

First, make sure you stopped the launch file from the previous tutorial (use ctrl-c). Now compile the net code, and run it again:

  $ catkin_make

  $ make

  $ roslaunch learning_tf start_demo.launch

But wait, you might still see the error once (error msg might vary):

[ERROR] [1287872014.408401177]: You requested a transform that is 3.009 seconds in the past, but the tf buffer only has a history of 2.688 seconds.                                                                   
 When trying to transform between /turtle1 and /turtle2.

This happens because turtle2 takes a non-zero time to spawn and start publishing tf frames. So the first time that you ask for now the /turtle2 frame may not have existed, when the transform is requested the transform may not exist yet and fails the first time. After the first transform all the transforms exist and the turtle behaves as expected.

Checking the results

Now once again you should be able to simply drive around the first turtle using the arrow keys (make sure your terminal window is active, not your simulator window), and you'll see the second turtle following the first turtle!

So, you noticed there is no noticeable difference in the turtle behavior. That is because the actual timing difference is only a few milliseconds. But then why did we make this change from Time(0) to now()? Just to teach you about the tf buffer and the time delays that are associated with it. For real tf use cases, it is often perfectly fine to use Time(0).

Now you're ready to move on to the next tutorial, where you'll learn about time travel in tf (Python) (C++)

Wiki: tf/Tutorials/tf and Time (C++) (last edited 2021-04-01 07:41:46 by FelixvonDrigalski)