tf2 Tutorial: Timestamp and time travel tf2 tutorials (Python) (ros2#1770)

* add timestamp and time travel tf2 tutorials

* links to previous tutorials

* minor fixes in Learning about tf2 and time (Python) tutorial

* add more explanations to the time travel tutorial

* add explanation about timeout

* use case for timeout

* add more context and remove questions

* update summary

* remove trailing whitespaces

Author: kurshakuz

source/Tutorials/Tf2/Learning-About-Tf2-And-Time-Py.rst

@@ -0,0 +1,96 @@
+.. _LearningAboutTf2AndTimePy:
+
+Learning about tf2 and time (Python)
+====================================
+
+**Goal:** Learn to use the ``timeout`` in ``lookup_transform`` function to wait for a transform to be available on the tf2 tree.
+
+**Tutorial level:** Intermediate
+
+**Time:** 10 minutes
+
+.. contents:: Contents
+   :depth: 2
+   :local:
+
+Background
+----------
+
+In previous tutorials, we recreated the turtle demo by writing a :ref:`tf2 broadcaster <WritingATf2BroadcasterPy>` and a :ref:`tf2 listener <WritingATf2ListenerPy>`.
+We also learned how to :ref:`add a new frame to the transformation tree <AddingAFramePy>`.
+Now we will learn more about the ``timeout`` argument which makes the ``lookup_transform`` wait for the specified transform for up to the specified duration before throwing an exception.
+This tool can be useful to listen for transforms that are published at varying rates or those incoming source with unreliable networking and non negligible latency.
+This tutorial will teach you how use the timeout in ``lookup_transform`` function to wait for a transform to be available on the tf2 tree.
+
+Tasks
+-----
+
+1 Update the listener node
+^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Edit ``turtle_tf2_listener.py`` and remove the ``timeout=Duration(seconds=1.0)`` parameter that is passed to the ``lookup_transform()`` call on line 76.
+It should look like shown below:
+
+.. code-block:: python
+
+   trans = self._tf_buffer.lookup_transform(
+      to_frame_rel,
+      from_frame_rel,
+      now)
+
+Moreover, import additional exceptions that we will handle in the beggining of the file:
+
+.. code-block:: python
+
+   from tf2_ros import LookupException, ConnectivityException, ExtrapolationException
+
+Edit the exception handling on line 81 by adding newly imported exceptions and ``raise`` statement to see the exception:
+
+.. code-block:: python
+
+   except (LookupException, ConnectivityException, ExtrapolationException):
+      self.get_logger().info('transform not ready')
+      raise
+      return
+
+If you now try to run the launch file, you will notice that it is failing:
+
+.. code-block:: console
+
+   ros2 launch learning_tf2_py turtle_tf2_demo.launch.py
+
+2 Fix the listener node
+^^^^^^^^^^^^^^^^^^^^^^^
+
+You now should notice that ``lookup_transform()`` is failing. It tells you that the frame does not exist or that the data is in the future.
+To fix this, edit your code on line 76 as shown below (return the ``timeout`` parameter):
+
+.. code-block:: python
+
+   trans = self._tf_buffer.lookup_transform(
+      to_frame_rel,
+      from_frame_rel,
+      now,
+      timeout=Duration(seconds=1.0))
+
+The ``lookup_transform`` can take four arguments, where the last one is an optional timeout.
+It will block for up to that duration waiting for it to timeout.
+
+.. note::
+
+   Once this change is made, remove the ``raise`` line from the ``except()`` block that we added above or the code will continue to fail.
+
+You can now run the launch file.
+
+.. code-block:: console
+
+   ros2 launch learning_tf2_py turtle_tf2_demo.launch.py
+
+You should notice that ``lookup_transform()`` will actually block until the transform between the two turtles becomes available (this will usually take a few milli-seconds).
+Once the timeout has been reached (one second in this case), an exception will be raised only if the transform is still not available.
+
+Summary
+-------
+
+In this tutorial you learned more about the ``lookup_transform`` function and its timeout features.
+You also learned how to catch and handle additional exceptions that can be thrown by tf2.

source/Tutorials/Tf2/Tf2-Main.rst

@@ -20,6 +20,8 @@ once for C++ and once for Python.
    Writing-A-Tf2-Broadcaster-Py
    Writing-A-Tf2-Listener-Py
    Adding-A-Frame-Py
+   Learning-About-Tf2-And-Time-Py
+   Time-Travel-With-Tf2-Py
 
 Workspace Setup
 ---------------
@@ -51,3 +53,13 @@ Learning tf2
 #. :ref:`Adding a frame (Python) <AddingAFramePy>`.
 
    This tutorial teaches you how to add an extra fixed frame to tf2.
+
+#. :ref:`Learning about tf2 and time (Python) <LearningAboutTf2AndTimePy>`.
+
+   This tutorial teaches you to use the timeout in ``lookup_transform`` function to
+   wait for a transform to be available on the tf2 tree.
+
+#. :ref:`Time travel with tf2 (Python) <TimeTravelWithTf2Py>`.
+
+   This tutorial teaches you about advanced time travel features of tf2.
+

source/Tutorials/Tf2/Time-Travel-With-Tf2-Py.rst

@@ -0,0 +1,116 @@
+.. _TimeTravelWithTf2Py:
+
+Time travel with tf2 (Python)
+=============================
+
+**Goal:** Learn about advanced time travel features of tf2.
+
+**Tutorial level:** Advanced
+
+**Time:** 10 minutes
+
+.. contents:: Contents
+   :depth: 2
+   :local:
+
+Background
+----------
+
+In the previous tutorial, we discussed the :ref:`basics of tf2 and time <LearningAboutTf2AndTimePy>`.
+This tutorial will take us one step further and expose a powerful tf2 trick: the time travel.
+In short, one of the key features of tf2 library is that it is able to transform data in time as well as in space.
+
+This tf2 time travel feature can be useful for various tasks, like monitoring the state of the robot for long period of time or building a follower robot that will follow the "steps" of the leader.
+We will use that time travel feature to look up for the transforms back in time and program the ``turtle1`` follow 5 seconds behind the ``turtle2``.
+
+Time travel
+-----------
+
+First, let's go back to where we ended in the previous tutorial `Learning about tf2 and time <LearningAboutTf2AndTimePy>`.
+Go to your ``learning_tf2_py`` package.
+
+Now, instead of making the second turtle go to where the carrot is now, we will make the second turtle go to where the first carrot was 5 seconds ago.
+Edit the code on lines 75-80 in the ``turtle_tf2_listener.py`` file to:
+
+.. code-block:: python
+
+    when = self.get_clock().now() - rclpy.time.Duration(seconds=5.0)
+    trans = self._tf_buffer.lookup_transform(
+        to_frame_rel,
+        from_frame_rel,
+        when,
+        timeout=Duration(seconds=1.0))
+
+Now if you run this, 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:
+
+.. code-block:: console
+
+    ros2 launch learning_tf2_py turtle_tf2_demo.launch.py
+
+.. image:: turtlesim_delay1.png
+
+You should now notice that your turtle is driving around uncontrollably like in this screenshot. Let's try to understand reason behind that behavior.
+
+#. In our code we asked tf2 the following question: "What was the pose of ``carrot1`` 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.
+
+#. However, what we really want to ask is: "What was the pose of ``carrot1`` 5 seconds ago, relative to the current position of the ``turtle2``?".
+
+Advanced API for lookup_transform
+---------------------------------
+
+To ask the tf2 that particular question, we will use an advanced API that gives us the power to say explicitly when to acquire the specified transformations.
+This is done by calling the ``lookup_transform_full`` method with additional parameters.
+Your code now would look like this:
+
+.. code-block:: python
+
+    when = self.get_clock().now() - rclpy.time.Duration(seconds=5.0)
+    trans = self._tf_buffer.lookup_transform_full(
+            target_frame=to_frame_rel,turtle1
+            target_time=rclpy.time.Time(),
+            source_frame=from_frame_rel,
+            source_time=when,
+            fixed_frame='world',
+            timeout=Duration(seconds=1.0))
+
+The advanced API for ``lookup_transform_full()`` takes six arguments:
+
+#. Target frame
+
+#. The time to transform to
+
+#. Source frame
+
+#. The time at which source frame will be evaluated
+
+#. The time at which we want to transform
+
+#. Frame that does not change over time, in this case the ``world`` frame
+
+#. Time to wait for the target frame to become available
+
+To sum up, tf2 does the following in the background.
+In the past, it computes the transform from the ``carrot1`` to the ``world``.
+In the ``world`` frame, tf2 time travels from the past to now.
+And at the current time, tf2 computes the transform from the ``world`` to the ``turtle2``.
+
+Checking the results
+--------------------
+
+Let's run the simulation again, this time with the advanced time-travel API:
+
+.. code-block:: console
+
+    ros2 launch learning_tf2_py turtle_tf2_demo.launch.py
+
+.. image:: turtlesim_delay2.png
+
+And yes, the second turtle is directed to where the first carrot was 5 seconds ago!
+
+Summary
+-------
+
+In this tutorial, you have seen one of the advanced features of tf2.
+You learned that tf2 can transform data in time and learned how to do that with turtlesim example.
+tf2 allowed you to go back in time and make frame transformations between old and current states of turtles by using the advanced ``lookup_transform_full`` API.