(!) 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 2019-02-12 14:09:59 by Martin Pecka)