Using ROS 2? Check out the ROS 2 tf2 tutorials.

(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Converting Datatypes

Description: This is a quick description of the changes in syntax for converting datatypes.

Tutorial Level: INTERMEDIATE

C++

geometry_msgs

Datatypes from geometry_msgs are used both in tf and tf2:

   1 #include "geometry_msgs/TransformStamped.h"
   2 
   3 geometry_msgs::Vector3 vector;
   4 geometry_msgs::Quaternion quaternion;
   5 geometry_msgs::Transform transform;
   6 
   7 geometry_msgs::Vector3Stamped stampedVector;
   8 geometry_msgs::QuaternionStamped stampedQuaternion;
   9 
  10 geometry_msgs::TransformStamped stampedTransformFull;

In tf you would do the following:

tf

Standard datatypes

   1 #include "tf/transform_datatypes.h"
   2 
   3 tf::Vector3 vector;
   4 tf::Quaternion quaternion;
   5 tf::Transform transform;
   6 
   7 tf::Stamped<tf::Vector3> stampedVector;
   8 tf::Stamped<tf::Quaternion> stampedQuaternion;
   9 tf::Stamped<tf::Transform> stampedTransform;
  10 
  11 tf::StampedTransform stampedTransformFull;

Conversion methods

Hard coded methods for bullet to message only.

   1 #include "tf/transform_datatypes.h"
   2 btVector3 btv = btVector3(xvalues[i], yvalues[i], zvalues[i]);
   3 geometry_msgs::Vector3 msgv;
   4 vector3TFToMsg(btv, msgv);

Helper methods

tf/transform_datatypes.h has static helper methods for creating quaternions

   1 #include "tf/transform_datatypes.h"
   2 tf::Quaternion quat_tf = tf::createQuaternionFromRPY(1.0, 0.0, 0.0);
   3 geometry_msgs::Quaternion quat_msg = tf::createQuaternionMsgFromRollPitchYaw(1.0, 0.0, 0.0);

tf2

Standard datatypes

In tf2, they are the same, just in tf2 namespace:

   1 #include "tf2/transform_datatypes.h"
   2 
   3 tf::Vector3 vector;
   4 tf::Quaternion quaternion;
   5 tf::Transform transform;
   6 
   7 tf2::Stamped<tf2::Vector3> stampedVector;
   8 tf2::Stamped<tf2::Quaternion> stampedQuaternion;
   9 tf2::Stamped<tf2::Transform> stampedTransform;
  10 
  11 // there's no TF2 counterpart of tf::StampedTransform
  12 // tf2::StampedTransform stampedTransformFull;
  13 

If you want a counterpart of tf::StampedTransform in tf2, you need to go with geometry_msgs::TransformStamped. Please note that tf2::Stamped<tf2::Transform> has no child_frame_id field which is required within the ROS tf/tf2 framework.

Conversion methods

With tf2 there is a templated convert method. This allows a modular design which can convert any datatype which implements the overloaded template methods.

Same result using bullet and message bindings:

   1 #include "tf2_bullet/tf2_bullet.h"
   2 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
   3 tf2::Stamped<btVector3> b(btVector3(1,2,3), ros::Time(), "my_frame");
   4 geometry_msgs::Vector3Stamped m;
   5 tf2::convert(b, m);

Much more extensible to convert bullet to kdl:

   1 #include "tf2_kdl/tf2_kdl.h"
   2 #include "tf2_bullet/tf2_bullet.h"
   3 tf2::Stamped<btVector3> b(btVector3(1,2,3), ros::Time(), "my_frame");
   4 tf2::Stamped<KDL::Vector> k;
   5 tf2::convert(b, k);

These headers are packaged in their own packages and allows packages to only depend on the converters they actually need.

Current implementations include tf2_bullet tf2_eigen tf2_kdl tf2_geometry_msgs

You can also create your own datatype conversions. There are tutorials for C++ and Python

Helper methods

tf2/transform_datatypes.h doesn't have the quaternion helper functions (you should use tf2::Quaternion methods in conjunction with tf2::convert()).

   1 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
   2 tf2::Quaternion quat_tf;
   3 quat_tf.setRPY(1.0, 0.0, 0.0);
   4 geometry_msgs::Quaternion quat_msg;
   5 tf2::convert(quat_tf, quat_msg);

Wiki: tf2/Tutorials/Migration/DataConversions (last edited 2022-10-06 16:51:56 by ShaneLoretz)