ROS TF, Whoever wrote the Python API, F**ked up the concepts.
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:
- The TF's function signature param names are used inconsistently.
- 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[1]); //in our case, this is /world
std::string target_frameid = std::string(argv[2]); //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 ×tamp, 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>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv){
ros::init(argc, argv, "fix_tf_cpp");
ros::NodeHandle node;
tf::TransformBroadcaster br;
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
from std_msgs.msg import Header
if __name__ == '__main__':
rospy.init_node('fixed_tf_broadcaster')
br = tf.TransformBroadcaster()
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(),
"bad_python_child",
"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))
)
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'world' # the parent link
# I use the stamped msg signature call to prevent the order confusion
trans_stamp = TransformStamped(header, 'child_py', trans)
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'
header.frame_id = 'child_py'
trans = Transform(translation=Vector3(*ts.translation_from_matrix(inversed_trans)),
rotation=Quaternion(*ts.quaternion_from_matrix(inversed_trans))
)
trans_stamp_inv = TransformStamped(header, 'world_inv_py', trans)
br.sendTransformMessage(trans_stamp_inv)
rate.sleep()