{{{#!wiki tip Using ROS 2? Check out the [[https://docs.ros.org/en/rolling/Tutorials/Intermediate/Tf2/Tf2-Main.html|ROS 2 tf2 tutorials]]. }}} ## page was renamed from tf22/Tutorials/Learning about tf22 and time (C++) #################################### ## for a custom note with links: ## note = This tutorial assumes you have completed the adding a frame tutorial [[tf2/Tutorials/Adding a frame (Python)|(Python)]] [[tf2/Tutorials/Adding a frame (C++)|(C++)]] ## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links ## note.0= ## descriptive title for the tutorial ## title = Learning about tf2 and time (C++) ## multi-line description to be displayed in search ## description = This tutorial teaches you to wait for a transform to be available on the tf2 tree when using the lookupTransform() function. ## the next tutorial description (optional) ## next = Time travel with tf2 ## links to next tutorial (optional) ## next.0.link=[[tf2/Tutorials/Time travel with tf2 (C++)|(C++)]] ## what level user is this tutorial for ## level= IntermediateCategory ## keywords = #################################### <> <> ##introstart == tf2 and Time == In the previous tutorials we learned about how tf2 keeps track of a tree of coordinate frames. This tree changes over time, and tf2 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 tf2 tree, without knowing at what time that transform was recorded. This tutorial will teach you how to get a transform '''at a specific time'''. ##introend So let's go back to where we ended in the [[tf2/Tutorials/Adding a frame (C++)|adding a frame tutorial]]. Go to your package for the tutorial: {{{ $ roscd learning_tf2 }}} and open the file '''`src/turtle_tf2_listener.cpp`'''. Take a look at lines 32-33: {{{ try{ transformStamped = listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0)); } catch (tf2::TransformException &ex) { ROS_WARN("Could NOT transform turtle2 to turtle1: %s", ex.what()); } }}} Let's make the second turtle follow the first turtle, and not the carrot. Change your code to the following: {{{ try{ transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0)); } catch (tf2::TransformException &ex) { ROS_WARN("Could NOT transform turtle2 to turtle1: %s", ex.what()); } }}} You can also see we specified a time equal to 0. For tf2, time 0 means "the latest available" transform in the buffer. Now, change this line to get the transform at the current time, "`now()`": {{{ try{ transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time::now()); } catch (tf2::TransformException &ex) { ROS_WARN("Could NOT transform turtle2 to turtle1: %s", ex.what()); } }}} First, make sure you stopped the launch file from the previous tutorial (use ctrl-c). Compile the code, and run it again: {{{ $ roslaunch learning_tf2 start_demo.launch }}} So, all of the sudden `lookupTransform()` is failing, telling you: . {{{ [ERROR] 1253918454.307455000: Extrapolation Too Far in the future: target_time is 1253918454.307, but the closest tf2 data is at 1253918454.300 which is 0.007 seconds away.Extrapolation Too Far in the future: target_time is 1253918454.307, but the closest tf2 data is at 1253918454.301 which is 0.006 seconds away. See http://pr.willowgarage.com/pr-docs/ros-packages/tf2/html/faq.html for more info. When trying to transform between /turtle1 and /turtle2. See http://www.ros.org/wiki/tf2#Frequently_Asked_Questions .... }}} ##lookupstart Why is that? Well, each listener has a buffer where it stores all the coordinate transforms coming from the different tf2 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 == tf2 provides a nice tool that will wait until a transform becomes available. You use this by adding a `Duration` parameter to `lookupTransform()`. Let's look at what the code would look like: ##lookupend {{{ try{ transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time::now(), ros::Duration(3.0)); } catch (tf2::TransformException &ex) { ROS_WARN("Could NOT transform turtle2 to turtle1: %s", ex.what()); } }}} The `lookupTransform()` can take four arguments. The 4th is an optional timeout. It will block for up to that duration waiting for it to timeout. (`ros::Duration` time values are given in seconds or seconds and nanoseconds. See [[roscpp/Overview/Time|the ROS Time and Duration overview]] for more information.) So `lookupTransform``()` 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: {{{ $ roslaunch learning_tf2 start_demo.launch }}} But wait you still see the error once: . {{{ [ERROR] 1253918454.307455000: Extrapolation Too Far in the future: target_time is 1253918454.307, but the closest tf2 data is at 1253918454.300 which is 0.007 seconds away.Extrapolation Too Far in the future: target_time is 1253918454.307, but the closest tf2 data is at 1253918454.301 which is 0.006 seconds away. See http://pr.willowgarage.com/pr-docs/ros-packages/tf2/html/faq.html for more info. When trying to transform between /turtle1 and /turtle2. See http://www.ros.org/wiki/tf2#Frequently_Asked_Questions }}} This happens because `turtle2` takes a non-zero time to spawn and start publishing tf2 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. ##checkingstart == 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 tf2 buffer and the time delays that are associated with it. For real tf2 use cases, it is often perfectly fine to use `Time(0)`. For high performance applications you will want to [[tf2/Tutorials/Using stamped datatypes with tf2::MessageFilter|use a tf2::MessageFilter]]. Now you're ready to move on to the next tutorial, where you'll learn about time travel in tf2 [[tf2/Tutorials/Time travel with tf2 (Python)|(Python)]] [[tf2/Tutorials/Time travel with tf2 (C++)|(C++)]] ##checkingend ## AUTOGENERATED DO NOT DELETE ## TutorialCategory ## FILL IN THE STACK TUTORIAL CATEGORY HERE ## C++Category ## LearningCategory