 # He, Peng

A Roboticist.

Robotic is the integration of all human knowledge, so far.

Abstract: TF, is very useful when dealing with transformations in robot navigation. Unfortunately, the ROS wiki did a very poor job to make all the concepts in the same manner and there is merely any well done tutorials online about this. In this article, I will help you have a general idea on how to use TF, including how to inverse a transform matrix, and clear up several important iterms.

## First thing first: Confusions

There are two major confusions in my experience:

1. The TF's function signature param names are used inconsistently.
2. Different "Transform" data structures with same name and are used inconsistently.

### Issue 1: source vs target

In my experience, srouce means where you are from and target mean where you are going to. In other words, if I want to look up the tf "from source A" to "target B", then your offsets are calculated wrt to the source coordinate by using:

offset = target(desired) - source(current)
The ROS TF is always translate first then followed by the rotaion For exmaple, in this case, `rosrun tf tf_echo /world /child` will print out:

``````At time 1549399583.364
- Translation: [1.000, 2.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
in RPY (radian) [0.000, -0.000, 0.000]
in RPY (degree) [0.000, -0.000, 0.000]
``````

You see that make sense that the child frame is translated by (1, 2, 0)
And I snap over the CLI tool for TF below:

``````   std::string source_frameid = std::string(argv); //in our case, this is /world
std::string target_frameid = std::string(argv); //in our case, this is /child

// Wait for up to one second for the first transforms to become avaiable.
echoListener.tf.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(1.0));

tf::StampedTransform echo_transform;
echoListener.tf.lookupTransform(source_frameid, target_frameid, ros::Time(), echo_transform);
``````

Here is what not making sense
I snap over the function signature of `lookupTransform` in both roscpp and rospy API:  You can clearly see the "target" goes first, but if we really put the target frame (/child) first:

``````(trans, rot) = listener.lookupTransform('child', 'world',  rospy.Time(0))
transform = ts.concatenate_matrices(ts.translation_matrix(trans), ts.quaternion_matrix(rot))
print (transform)
``````

This will print out:

``````[[ 1.  0.  0. -1.]
[ 0.  1.  0. -2.]
[ 0.  0.  1.  0.]
[ 0.  0.  0.  1.]]
``````

Which is different from our expection. And the same thing applies on roscpp API. However, if we put thing in the CLI command order:

``````transformStamped = tfBuffer.lookupTransform("world", "child_cpp", ros::Time(0));
``````

This will now start working!

Conclusion: When you in C++, go with the order of loopUp(from_parent, to_child) no matter what the API says in this case. You need to becarefull when in python. I'm going to write a custom wrapper for this if needed in future.

## Issue 2: Confusing "Transform" types

For those who are beginners in ROS, this will be very hard to figure out, this is because almost all the examples online that I've seen are implicit defiend message type and they just send it away.

Let me give an example explicitly in roscpp:
When you are dealing with the TF, you probably want to have a data structure that holds the T matrix. Here we have a `tf::Transform` for the data type and the broadcaster is using the same type to send out our TF.

``````tf::Transform transform;
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "child_cpp"));
``````

However, when you receive a TF lookup return, the data is in another type.

``````geometry_msgs::TransformStamped transformStamped;

try{
transformStamped = tfBuffer.lookupTransform("world", "child_cpp",
ros::Time(0));
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
``````

You can ignore the "Stamped", but you see the returned data is the type of "geometry_msgs::Transform" that is also called "Transform" but it is not the `tf::Transform`!

Which means it does not have all the `tf::Transform` class member functions. for example, the `inverse()`. Where you can just call this `transform = transform.inverse();` to get your transform inversed if it is `tf::Transform`. And by the way, the rospy does not have `tf::Transform` type at all (to my reaserch so far). So if you want to inverse a transform in rospy, then you need to do it manually like this:

``````(trans, rot) = listener.lookupTransform('world', 'child', rospy.Time(0))
transform = ts.concatenate_matrices(ts.translation_matrix(trans), ts.quaternion_matrix(rot))
inversed_transform = ts.inverse_matrix(transform)
``````

## Speaking of sending out a TF

### In ROS C++

The story is, `sendTransform(some transform)` can takes two type of transform (we talked about them eailer). The first signature is for tf::Tranform, the other three are geometry_msgs type. And the `tf::Transform` has a constructor helps you assign the values, so far everything looks good, the parent link goes first then the child.

``````StampedTransform (const tf::Transform &input, const ros::Time &timestamp, const std::string &frame_id, const std::string &child_frame_id)
``````

### In ROS Python

However, I hate to say this, but the guy who wrote the python API really messed up. He flips the signature order: This is a very evil movement in your whole TF program because the order will make you confused all the time.

## A small test after all

By this point, I hope my article can help you eliminate the confusions.
Now let's do a little test in both roscpp and rospy.

Our goal is to publish a fix tf from `/world` to `/child` and lookup this tf still from `/world` to `/child` and then we inverse this tf, and finally, we broadcast this inversed tf from `/child` to `/world_inv`.

Our expected results will be the `/world_inv` goes back to the same place where `/world` is.

And you can see now, the final result is correct. ## Gists / Source Code

You can find the files I'm using at here.

I also paste them here for quicker reference.

``````#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/TransformStamped.h>

int main(int argc, char** argv){
ros::init(argc, argv, "fix_tf_cpp");
ros::NodeHandle node;

tf::Transform transform;

tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);

ros::Rate rate(10.0);
while (node.ok()){
transform.setOrigin( tf::Vector3(5.0, 2.0, 0.0) );
//    transform.setRotation( tf::Quaternion(0, 0, 0, 1) ); // another way to do the angles
transform.setRotation(tf::createQuaternionFromRPY(0., 0., 0.));

// the signature order in C++ is always being parent then child
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "child_cpp"));

geometry_msgs::TransformStamped transformStamped;

try{
transformStamped = tfBuffer.lookupTransform("world", "child_cpp",
ros::Time(0));
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
// the received data is a geometry_msgs/Transform, we need to convert it to tf::Transform.
tf::transformMsgToTF(transformStamped.transform, transform);
// inverse the tf and create a frame
transform = transform.inverse();

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "child_cpp", "world_inv_cpp"));

rate.sleep();
}
return 0;
};
``````
``````#!/usr/bin/env python

import rospy
import tf
from tf import transformations as ts

from geometry_msgs.msg import Transform, Vector3, Quaternion
from geometry_msgs.msg import TransformStamped

if __name__ == '__main__':
listener = tf.TransformListener()
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
# this is here to show your that this function signature is not consistent with others
br.sendTransform((3.0, 2.0, 0.0),
tf.transformations.quaternion_from_euler(0, 0, 45.),
rospy.Time.now(),
"world")

# the Transform in python is not from tf::Transform, you need to feed in the exactly msg type.
trans = Transform(translation=Vector3(1.0, 2.0, 0),
rotation=Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0))
)

# I use the stamped msg signature call to prevent the order confusion
br.sendTransformMessage(trans_stamp)

try:
(trans, rot) = listener.lookupTransform('world', 'child_py', rospy.Time(0))
# because it is not the tf::Transform, there is no reverse member function call, do it manually
transform = ts.concatenate_matrices(ts.translation_matrix(trans), ts.quaternion_matrix(rot))
inversed_trans = ts.inverse_matrix(transform)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue

# now publish the inverse frame back, it should stay at the same location of '/world'
trans = Transform(translation=Vector3(*ts.translation_from_matrix(inversed_trans)),
rotation=Quaternion(*ts.quaternion_from_matrix(inversed_trans))
)

br.sendTransformMessage(trans_stamp_inv)

rate.sleep()
``````
Recent Posts

## ROS Project Folder Structure Quick Setup Script

Project Folder Structure Logic Preserver the file name and path to not break the download link. Thi…