Transformation

Choosing the right reference point is crucial for many algorithms in the world of robotics. Knowledge of the relative location of objects allows the implementation of tasks such as sensor fusion or simultaneous localization and mapping (SLAM). In ROS you don't have to worry about math because all transformations are determined using the tf2 package.

Prerequisites

This tutorial is dedicated to the ROSbot XL. However, most of the information and commands are also compatible with ROSbot 2R/2 PRO, and most changes are simple and come down to launching the appropriate container, depending on your hardware configuration. It is also possible to solve the tutorial using the Gazebo simulation.

info

If you don't already own a ROSbot, you can get one from the Husarion store.

• ROSbot XL
• ROSbot 2R
• ROSbot 2 PRO

The tutorials are designed to flow sequentially and are best followed in order. If you're looking to quickly assess the content of a specific tutorial, refer to the ROS 2 Tutorials repository, which contains the completed outcomes.

Introduction

Transformations are functions that map n-D vectors to other n-D vectors: T:Rn→Rn. They can represent geometric operations, which are caused by movement or action, as well as changes of coordinates, which are caused by changes of interpretation. Many common spatial transformations, including translations, rotations, and scaling are represented by matrix / vector operations. Changes of coordinate frames are also matrix / vector operations. As a result, transformation matrices are stored and operated on ubiquitously in robotics.

Coordinate transformation is used in many areas of mathematics, science, and engineering, and it has various applications. In this tutorial, transformations will be used to:

1. Changing perspective: Coordinate transformation can help us view the same object or phenomenon from different perspectives, which can be useful in many applications. For example, in physics, we might want to change the coordinate system to make the equations of motion simpler or to study a system in a different reference frame.
2. Mapping: Coordinate transformation can help map one coordinate system to another, which can be useful in navigation, cartography, and other fields.

A transformation is a simple operation consisting of a translation and a rotation. The picture below shows two frames of reference rotated relative to each other. To find the position of point P in the second frame of reference, multiply the point by the rotation matrix.

The translation boils down to a simple addition of the coordinates of the second reference point.

In general, transformations involve simple matrix operations, and coordinate transformation itself is a powerful tool that can help us understand and work with complex systems and phenomena. Fortunately, the tf package has a lot of useful functions that do all these calculations for us.

Basic Transformation

ROS can help track coordinate systems over time. The tf2 package is used for this - a library that use special geometry_msgs/TransformStamped message type. This message type consists:

• Header header - header message with timestamp for current transformation and information about parent frame id,
• string child_frame_id - the frame id of the child frame,
• Transform transform - transformation from coordinate frame header.frame_id to the coordinate frame child_frame_id.

Static Transformation

This tutorial will show you how to add a static transformation between two frames. Suppose we have a robot that is fixed at (1,-1,0) relative to the map and rotated to the left. For this purpose, we can use the static_transform_publisher node from the tf2 package. Running the function is simple and just execute the command below:

ros2 run tf2_ros static_transform_publisher --frame-id map --child-frame-id robot <optional arguments>

Optional argument:

• --x - x component of translation
• --y - y component of translation
• --z - z component of translation
• --qx - x component of quaternion rotation
• --qy - y component of quaternion rotation
• --qz - z component of quaternion rotation
• --qw - w component of quaternion rotation
• --roll - roll component Euler rotation
• --pitch - pitch component Euler rotation
• --yaw - yaw component Euler rotation

Now all you have to do is complete the above fields in the appropriate way.

husarion@husarion:~$

ros2 run tf2_ros static_transform_publisher --frame-id map --child-frame-id robot --x 1 --y -1 --yaw 1.6

Visualization

TF_Echo

A simple tool that allows you to quickly check the transformation between any two frames.

husarion@husarion:~$

ros2 run tf2_ros tf2_echo map robot

TF_Tree

The tool shows all the relationships between the individual forms in a diagram. This tool allows you to check if all frames are properly connected to each other.

husarion@husarion:~$

ros2 run rqt_tf_tree rqt_tf_tree

RViz

The RVIZ program known from Tutorial 5 will be used for visualization. To run RViz just type in new terminal.

rviz2

Now click Add. In the new window, select TF from the list.

Task 1

Try adding another reverse_camera frame which is 20 cm above the robot and facing the back of the robot. Use static_transform_publisher and check the result with all the tools presented above.

Solution

In new terminal type:

ros2 run tf2_ros static_transform_publisher --frame-id robot --child-frame-id reverse_camera --z 0.2 --yaw 3.14

Remember that if you point to a non-existing frame, a new relation will be created that is not related in any way to the currently existing transformations.

Broadcasting Transformation

As you can see, the static transformation is very simple, but what to do when we want to simulate the mutual position of moving objects. ROS also allows this, but this time we need to create a node in which we will describe the movement of one frame relative to the other. Suppose we want to create an object that moves around the map. In this case, it is necessary to create a so-called TransformBroadcaster, which is a special sender type for sending TransformStamped messages. Despite the name difference, it works very similar to the classic Publisher except that it can only send TransformStamped messages.

Let's start with creating a new file and name it tf_broadcaster.cpp.

~/ros2_ws/src/tutorial_pkg/src/tf_broadcaster.cpp

#include <chrono>
#include <functional>
#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"

using namespace std::chrono_literals;

class TF_Brodcaster : public rclcpp::Node
{
public:
  TF_Brodcaster() : Node("tf_broadcaster")
  {
    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
    timer_ = create_wall_timer(10ms, std::bind(&TF_Brodcaster::timer_callback, this));

    RCLCPP_INFO(get_logger(), "Node started!");
  }

private:
  void timer_callback()
  {
    geometry_msgs::msg::TransformStamped transformStamped;

    transformStamped.header.stamp = this->get_clock()->now();
    transformStamped.header.frame_id = "map";
    transformStamped.child_frame_id = "robot";

    double t = this->get_clock()->now().seconds();

    // Set translation
    transformStamped.transform.translation.x = std::cos(t);
    transformStamped.transform.translation.y = std::sin(t);
    transformStamped.transform.translation.z = 0.0;

    // Set rotation using roll, pitch, and yaw
    tf2::Quaternion q;
    q.setRPY(0, 0, std::fmod(t, 2 * M_PI) + M_PI_2);
    transformStamped.transform.rotation.x = q.x();
    transformStamped.transform.rotation.y = q.y();
    transformStamped.transform.rotation.z = q.z();
    transformStamped.transform.rotation.w = q.w();

    tf_broadcaster_->sendTransform(transformStamped);
  }

  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<TF_Brodcaster>());
  rclcpp::shutdown();
  return 0;
}

The code explained

1. Declare specific publisher type and timer.

   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
   rclcpp::TimerBase::SharedPtr timer_;

2. Declare transformation message:

   geometry_msgs::msg::TransformStamped transformStamped;

3. Receiving a time in seconds:

   double t = this->get_clock()->now().seconds();

4. Setting translation of frame. Since we want the object to rotate around the center of the map, the variables at the center must be time dependent.

   transformStamped.transform.translation.x = std::cos(t);
   transformStamped.transform.translation.y = std::sin(t);
   transformStamped.transform.translation.z = 0.0;

5. Setting rotation of frame.

    tf2::Quaternion q;
    q.setRPY(0, 0, std::fmod(t, 2 * M_PI) + M_PI_2);
    transformStamped.transform.rotation.x = q.x();
    transformStamped.transform.rotation.y = q.y();
    transformStamped.transform.rotation.z = q.z();
    transformStamped.transform.rotation.w = q.w();

Build code

Now in CMakeLists.txt file find line

~/ros2_ws/src/tutorial_pkg/CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(tutorial_pkg)

# Set C++ standard to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(OpenCV REQUIRED)

find_package(ament_cmake REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)

add_executable(my_first_node src/my_first_node.cpp)
ament_target_dependencies(my_first_node rclcpp sensor_msgs std_msgs std_srvs)

add_executable(img_control src/img_control.cpp)
ament_target_dependencies(img_control rclcpp geometry_msgs std_msgs)

add_executable(img_follow src/img_follow.cpp)
ament_target_dependencies(img_follow rclcpp OpenCV geometry_msgs std_msgs)

add_executable(tf_broadcaster src/tf_broadcaster.cpp)
ament_target_dependencies(tf_broadcaster rclcpp geometry_msgs tf2 tf2_ros)


install(TARGETS
  my_first_node
  img_control
  img_follow
  tf_broadcaster
  DESTINATION lib/${PROJECT_NAME}
)

install(DIRECTORY
  launch
  img_data
  DESTINATION share/${PROJECT_NAME})

ament_package()

Task 2

Run code and try adding another frame using static_transform_publisher and see the result in RViz. Result:

Solution

You can use the same command as in the Task 1.

ros2 run tf2_ros static_transform_publisher --frame-id robot --child-frame-id reverse_camera --z 0.2 --yaw 3.14

Listening Transformation

As was the case with sending, receiving TF data is also a bit different from the Subscriber you already know. A TransformListener is used to receive data, which will automatically determine the transformations between two frames, even if they are not directly connected to each other.

~/ros2_ws/src/tutorial_pkg/src/tf_listener.cpp

#include <chrono>
#include <functional>
#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/exceptions.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"


using namespace std::chrono_literals;

class TF_Listener : public rclcpp::Node
{
public:
  TF_Listener() : Node("tf_listener")
  {
    tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
    timer_ = create_wall_timer(10ms, std::bind(&TF_Listener::timer_callback, this));

    RCLCPP_INFO(get_logger(), "Node started!");
  }

private:
  void timer_callback()
  {
    try
    {
      geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform("map", "robot", tf2::TimePointZero);

      geometry_msgs::msg::Quaternion q_msg = transform.transform.rotation;
      tf2::Quaternion q_tf(q_msg.x, q_msg.y, q_msg.z, q_msg.w);
      tf2::Matrix3x3 m(q_tf);

      double roll, pitch, yaw;
      m.getRPY(roll, pitch, yaw);
      RCLCPP_INFO(this->get_logger(), "Yaw: %f", yaw);
    }
    catch (const tf2::TransformException & ex)
    {
      RCLCPP_INFO(this->get_logger(), "Could not transform: %s", ex.what());
    }
  }

  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<TF_Listener>());
  rclcpp::shutdown();
  return 0;
}

The code explained

1. Declare buffer, specific subscriber type and timer.

   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
   rclcpp::TimerBase::SharedPtr timer_;

2. Find last transformation form map frame to robot frame. The third argument specify time at which we want to transform. Providing tf2::TimePointZero will just get us the latest available transform.

   geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform("map", "robot", tf2::TimePointZero);

3. Converting them to roll, pitch, yaw.

   geometry_msgs::msg::Quaternion q_msg = transform.transform.rotation;
   tf2::Quaternion q_tf(q_msg.x, q_msg.y, q_msg.z, q_msg.w);
   tf2::Matrix3x3 m(q_tf);
   
   double roll, pitch, yaw;
   m.getRPY(roll, pitch, yaw);
   RCLCPP_INFO(this->get_logger(), "Yaw: %f", yaw);

Build code

Declare new executable and specify libraries to link.

~/ros2_ws/src/tutorial_pkg/CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(tutorial_pkg)

# Set C++ standard to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)

add_executable(my_first_node src/my_first_node.cpp)
ament_target_dependencies(my_first_node rclcpp sensor_msgs std_msgs std_srvs)

add_executable(img_control src/img_control.cpp)
ament_target_dependencies(img_control rclcpp geometry_msgs std_msgs)

add_executable(tf_broadcaster src/tf_broadcaster.cpp)
ament_target_dependencies(tf_broadcaster rclcpp geometry_msgs tf2 tf2_ros)

add_executable(tf_listener src/tf_listener.cpp)
ament_target_dependencies(tf_listener rclcpp geometry_msgs tf2 tf2_ros)


install(TARGETS
  my_first_node
  img_control
  tf_broadcaster
  tf_listener
  DESTINATION lib/${PROJECT_NAME}
)

install(DIRECTORY
  launch
  img_data
  DESTINATION share/${PROJECT_NAME})

ament_package()

Task 3

Look at the values ​​of the yaw angle and change the code so that it determines the number of revolutions.

Solution

Replace line RCLCPP_INFO(this->get_logger(), "Yaw: %f", yaw); with below code and build your file.

// Calculate number of rotation
static int num_of_revolutions;
static float prev_yaw;
if(yaw-prev_yaw < 0){
    num_of_revolutions++;
    RCLCPP_INFO(this->get_logger(), "Number of revolutions: %d", num_of_revolutions);
}
prev_yaw = yaw;

Summary

It's all for today. I hope that after this tutorial and the reworked exercises, handling and interpreting the tf2 package will not be a big problem for you. At this stage, you should perfectly understand the concept of transformation, what frames are, and you have the right tools for data visualization.

---

by Rafał Górecki, Husarion

Do you need any support with completing this tutorial or have any difficulties with software or hardware? Feel free to describe your thoughts on our community forum: https://community.husarion.com/ or to contact our support: support@husarion.com