[Melodic][TF Tutorial] 6. Time travel with tf (Python)

1. Time travel

$ 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", "/turtle1", rospy.Time(), rospy.Duration(4.0))
    while not rospy.is_shutdown():
        try:
            now = rospy.Time.now() - rospy.Duration(5.0)
            listener.waitForTransform("/turtle2", "/turtle1", now, rospy.Duration(1.0))
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', now)
        except (tf.Exception, 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

Now, instead of making the second turtle go to where the first turtle is now, make the second turtle go to where the first turtle was 5 seconds ago.

 

So now, if you would run this, what would you expect to see?

Definitely during the first 5 seconds the second turtle would not know where to go, because we do not yet have a 5 second history of the first turtle. But what after these 5 seconds?

Let's just give it a try:

 

Is your turtle driving around uncontrollably like in this screenshot?

So what is happening?

• We asked tf, "What was the pose of /turtle1 5 seconds ago, relative to /turtle2 5 seconds ago?". This means we are controlling the second turtle based on where it was 5 seconds ago as well as where the first turtle was 5 seconds ago.
• What we really want to ask is, "What was the pose of /turtle1 5 seconds ago, relative to the current position of the /turtle2?".

 

2. Advanced API for lookupTransform

So how can we ask tf a question like that?

This API gives us the power to say explicitly when each frame is transformed.

This is what the code would look like:

$ 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", "/turtle1", rospy.Time(), rospy.Duration(4.0))
    while not rospy.is_shutdown():
        try:
            now = rospy.Time.now() 
            past = now - rospy.Duration(5.0)
            listener.waitForTransformFull("/turtle2", now,
                                          "/turtle1", past, 
                                          "/world", rospy.Duration(1.0))
            (trans,rot) = listener.lookupTransformFull('/turtle2', now,
                                                       '/turtle1', past,
                                                       "/world")
        except (tf.Exception, 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()

 

2.1 waitForTransformFull() and lookupTransformFull()

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))

    def waitForTransformFull(self, target_frame, target_time, source_frame, source_time, fixed_frame, 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_full(strip_leading_slash(target_frame), target_time, strip_leading_slash(source_frame), source_time, strip_leading_slash(fixed_frame), 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 advanced API for lookupTransform() takes six arguments:

1. Give the transform from this frame,
2. at this time ...
3. ... to this frame,
4. at this time.
5. Specify the frame that does not change over time, in this case the "/world" frame, and
6. the variable to store the result in.

This figure shows what tf is doing in the background.

In the past it computes the transform from the first turtle to the world.

In the world frame tf time travels from the past to now.

And at time now tf computes the transform from the world to the second turtle.

 

3. Checking the results

Let's run the simulator again, this time with the advanced time-travel API:

# 1st terminal
$ roslaunch learning_tf start_demo.launch

And yes, the second turtle is directed to where the first turtle was 5 seconds ago!

 

ref.

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

tf/Tutorials/Time travel with tf (Python) - ROS Wiki