**********
ROS2 TF2
**********

Introduction
================

This tutorial explains static and dynamic transforms. ROS is
using the package tf2_ros as an easy accessible and intuitive
API to describe the relationship between coordinate frames.
The online documentation can be found at
`http://wiki.ros.org/tf2 <http://wiki.ros.org/tf2>`__. 

-  Lines beginning with $ are terminal commands.

   -  To open a new terminal → use the shortcut ``Ctrl + Alt + T``.

   -  To open a new tab inside an existing terminal → use the shortcut ``Ctrl + Shift +T``.

   -  To kill a process in a terminal → use the shortcut ``Ctrl + C``.

-  Lines beginning with # indicate the syntax of the commands.

-  Code is separated in boxes.

-  Code is case sensitive.

Creating a Static Transform
=============================

A static transform is an invariant coordinate transformation and describes the
relationship between two frames. A definition can be done through launch files
by using the *static_transform_publisher* node, which contains a publisher to
the */tf_static* topic. Provide the following arguments to describe the
relationship between a *parent* and a *child* frame:

.. code-block:: bash

    Arguments: x y z yaw pitch roll parent_frame child_frame


The translation is in meters. The rotation is denoted as Euler angles in radians
or in quaternion values as shown in the alternative below:

.. code-block:: bash

    Arguments: x y z qx qy qz qw parent_frame child_frame

Create a new package **myrobottf** in your *robot_ws* workspace.
Now create a launch file *turtlebot3_tf.launch.py* in this package.
Use the given example as a skeleton (Euler angle syntax):

.. literalinclude:: /_resources/code/tutorials/launch/simple_static_tf.launch.py
   :language: python
   :lines: 1-
   :caption: simple static tf broadcaster launch file

Create the transformation tree of the turtlebot3 by starting multiple running
processes of the node *static_transform_publisher*. Take care to name them
differently:

→ Relationship between the frames *base_footprint* and *my_base_link*:

  * Translation, t = (x, y, z)´ = (-1.0, 0, 0.01)´

  * Rotation, r = (roll, pitch, yaw)´ = (0, 0, 0)´

→ Relationship between the frames *my_base_link* and *my_camera_rgb_optical_frame*:

  * Translation, t = (x, y, z)´ = (0.015, 0.0913, 0.0945)´

  * Rotation, r = (roll, pitch, yaw)´ = (0, 0, 0)´

→ Relationship between the frames *my_base_link* and *my_imu_link*:

  * Translation, t = (x, y, z)´ = (0, 0, 0.068)´

  * Rotation, r = (roll, pitch, yaw)´ = (0, 0, 0)´

→ Relationship between the frames *my_base_link* and *my_base_scan*:

  * Translation, t = (x, y, z)´ = (-0.09, 0, 0.114)´

  * Rotation, r = (roll, pitch, yaw)´ = (0, 0, 0)´

Visualize the tf tree in Rviz:

.. code-block:: bash

    $ ros2 launch myrobottf turtlebot3_tf.launch.py

    $ ros2 run rviz2 rviz2

* Click the **Add** button and choose **TF**, to visualize the tf information.

.. hint::
   Switch your fixed_frame in RVIZ to the start frame of your tf tree,
   that is right now, base_footprint.

→ You should see the different coordinate frames of your Turtlebot3,
based on your static transforms.

Creating a Dynamic Transform
===============================

Dynamic transforms are variant transformations, which change over time.
The convenient way is to use the Python API to define frame relationships.

Dynamic TF Exercise
--------------------

Write a transform Broadcaster and define a dynamic relationship between the frames *odom*
and *base_footprint*. Therefore, create a node named *simulated_odom.py* in the package
**myrobottf**. By providing changes in the translation values *x* and *y*, the turtlebot
takes a simulated path. Use the following mathematical definition and the Python code example.

.. figure:: /_resources/image/simulated_trajectory_path.png
   :align: center

.. figure:: /_resources/image/virtual_turtlebot_path.png

   Image 1. Virtual Turtlebot Path

.. literalinclude:: /_resources/code/tutorials/scripts/simulated_odom2.py
   :language: python
   :lines: 1-
   :caption: simulated_odom.py

To simplify the transformation from euler to quaternion we are using a scipy
function. For that purpose we need to install the scipy library:

.. code-block:: bash

   $ pip3 install scipy

Explanation
-------------

The basic structure of a simple node is already well known.
So, only the tf specific lines will be explained.

.. literalinclude:: /_resources/code/tutorials/scripts/simulated_odom2.py
   :language: python
   :lines: 4

These lines import the essential TF modules. The **tf2_ros** package provides
ROS2 bindings to tf2.

Create an object *br* of the TransformBroadcaster class:

.. literalinclude:: /_resources/code/tutorials/scripts/simulated_odom2.py
   :language: python
   :lines: 49

This will initialize a ROS2 TF2 Broadcaster, which allows to send transforms
from one frame to another one.

.. literalinclude:: /_resources/code/tutorials/scripts/simulated_odom2.py
   :language: python
   :lines: 48

Calculates the transform from base_footprint to odom

.. literalinclude:: /_resources/code/tutorials/scripts/simulated_odom2.py
   :language: python
   :lines: 22

Returns the virtual x and y position of the turtlebot.

.. literalinclude:: /_resources/code/tutorials/scripts/simulated_odom2.py
   :language: python
   :lines: 50

The handler function *sendTransform()* for the turtlebot pose broadcasts the turtlebot's
translation and rotation, and publishes it as a transform from frame *odom* to frame *base_footprint*.

The *sendTransform* function is from the message type *TransformStamped*, which was set up in
the **calculate_tf** definition:

  * Header

  * Child Frame ID (Frame ID to which the transform is happening)

  * Transform

Exercise
----------

Run your node and visualize the dynamic tf tree in Rviz.