[Melodic][TF Tutorial] 5. Learning about tf and time (Python)

1. 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.

$ roscd learning_tf/
$ nano ./nodes/turtle_tf_listener.py

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')

    listener = tf.TransformListener()

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(4, 2, 0, 'turtle2')

    turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            now = rospy.Time.now()
            (trans,rot) = listener.lookupTransform('/turtle2', '/carrot1', now)
        except (tf.LookupException, tf.ConnectivityException):
            continue

        angular = 4 * math.atan2(trans[1], trans[0])
        linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        cmd = geometry_msgs.msg.Twist()
        cmd.linear.x = linear
        cmd.angular.z = angular
        turtle_vel.publish(cmd)

        rate.sleep()

# 1st terminal
$ roslaunch learning_tf start_demo.launch
Traceback (most recent call last):
  File "/home/jstar/catkin_ws/src/tf_tutorials/learning_tf/nodes/turtle_tf_listener.py", line 25, in <module>
    (trans,rot) = listener.lookupTransform('/turtle2', '/carrot1', now)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/tf/listener.py", line 104, in lookupTransform
    msg = self._buffer.lookup_transform(strip_leading_slash(target_frame), strip_leading_slash(source_frame), time)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/tf2_ros/buffer.py", line 87, in lookup_transform
    return self.lookup_transform_core(target_frame, source_frame, time)
tf2.ExtrapolationException: Lookup would require extrapolation at time 1661542129.853631020, but only time 1661542129.853168964 is in the buffer, when looking up transform from frame [carrot1] to frame [turtle2]
[listener-6] process has died [pid 8073, exit code 1, cmd /home/jstar/catkin_ws/src/tf_tutorials/learning_tf/nodes/turtle_tf_listener.py __name:=listener __log:=/home/jstar/.ros/log/4db3e990-2575-11ed-81ad-000c2949cb2d/listener-6.log].

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.

 

2. 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:

$ roscd learning_tf/
$ nano ./nodes/turtle_tf_listener.py

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')

    listener = tf.TransformListener()

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(4, 2, 0, 'turtle2')

    turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)

    rate = rospy.Rate(10.0)
    listener.waitForTransform("/turtle2", "/carrot1", rospy.Time(), rospy.Duration(4.0))
    while not rospy.is_shutdown():
        try:
            now = rospy.Time.now()
            listener.waitForTransform("/turtle2", "/carrot1", now, rospy.Duration(4.0))
            (trans,rot) = listener.lookupTransform('/turtle2', '/carrot1', now)
        except (tf.LookupException, tf.ConnectivityException):
            continue

        angular = 4 * math.atan2(trans[1], trans[0])
        linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        cmd = geometry_msgs.msg.Twist()
        cmd.linear.x = linear
        cmd.angular.z = angular
        turtle_vel.publish(cmd)

        rate.sleep()

# 1st terminal
$ roslaunch learning_tf start_demo.launch

 

2.1 waitForTransform()

is as follows.

https://github.com/ros/geometry/blob/melodic-devel/tf/src/tf/listener.py

GitHub - ros/geometry: Packages for common geometric calculations including the ROS transform library, "tf". Also includes ROS b

class TransformListener(TransformerROS):

    """
    TransformListener is a subclass of :class:`tf.TransformerROS` that
    subscribes to the ``"/tf"`` message topic, and calls :meth:`tf.Transformer.setTransform`
    with each incoming transformation message.
    In this way a TransformListener object automatically
    stays up to to date with all current transforms.  Typical usage might be::
        import tf
        from geometry_msgs.msg import PointStamped
        class MyNode:
            def __init__(self):
                self.tl = tf.TransformListener()
                rospy.Subscriber("/sometopic", PointStamped, self.some_message_handler)
                ...
            
            def some_message_handler(self, point_stamped):
                # want to work on the point in the "world" frame
                point_in_world = self.tl.transformPoint("world", point_stamped)
                ...
        
    """

class TransformerROS(Transformer):
    """
    TransformerROS extends the base class :class:`tf.Transformer`,
    adding methods for handling ROS messages. 
    """

class Transformer(object):

    def __init__(self, interpolate=True, cache_time=None):
        self._buffer = tf2_ros.Buffer(cache_time, debug=False)
        self._using_dedicated_thread = False

    def waitForTransform(self, target_frame, source_frame, time, timeout, polling_sleep_duration=None):
        if not self._using_dedicated_thread:
            raise tf2_ros.TransformException("cannot wait for transform without a dedicated thread that listens to incoming TF messages")
        can_transform, error_msg = self._buffer.can_transform(strip_leading_slash(target_frame), strip_leading_slash(source_frame), time, timeout, return_debug_tuple=True)
        if not can_transform:
            raise tf2_ros.TransformException(error_msg or "no such transformation: \"{}\" -> \"{}\"".format(source_frame, target_frame))

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

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

 

So why the two waitForTransform() calls? At the beginning of code we spawned turtle2 but tf may have not ever seen the /turtle2 frame before waiting for the transform. The first waitForTransform() will wait until the /turtle2 frame is broadcast on tf before trying to waitForTransform() at time now.

 

3.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

 

 

ref.

http://wiki.ros.org/tf/Tutorials/tf%20and%20Time%20%28Python%29

tf/Tutorials/tf and Time (Python) - ROS Wiki

https://github.com/jstar0525/tf_tutorials

GitHub - jstar0525/tf_tutorials: tf_tutorial using ROS Melodic

$ git clone https://github.com/jstar0525/tf_tutorials.git
$ cd tf_tutorials/
$ git checkout 06bc6b0cbe2e0b3b05da5df82cf81d3d9b838838