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.

Create Data Conversion Package (Python)

Background

To use the tf2_ros Python conversion methods you must define functions which will convert to and from a message datatype. Note that all the message datatypes must be Stamped which includes the timestamp and frame_id fields. And because we're not leveraging the C++ compiler template logic we must register the conversion function.

Required Methods for Automatic Conversion

You must define a function which will take a message as an argument and it will return the equivalent datatype wrapped in the tf2_ros.Stamped datatype.

For example:

   1 def from_msg_vector(msg):
   2     vector = PyKDL.Vector(msg.point.x, msg.point.y, msg.point.z)
   3     return tf2_ros.Stamped(vector, msg.header.stamp, msg.header.frame_id)

Then you must register this function as a from_msg converter.

   1 tf2_ros.ConvertRegistration().add_from_msg(PyKDL.Vector, from_msg_vector)

For the other side you must implement a method that coverts from the custom datatype into a message.

   1 def to_msg_vector(vector):
   2     msg = PointStamped()
   3     msg.header = vector.header
   4     msg.point.x = vector[0]
   5     msg.point.y = vector[1]
   6     msg.point.z = vector[2]
   7     return msg

And register the conversion in this direction too.

   1 tf2_ros.ConvertRegistration().add_to_msg(PyKDL.Vector, to_msg_vector)

Required Methods for Transforming Custom Datatypes

To support transforming custom datatypes you must provide a template specialization of the method doTransform defined in tf2/convert.h

template <class T>
  void doTransform(const T& data_in, T& data_out, const geometry_msgs::TransformStamped& transform);

For example in tf2_kdl it looks like this:

template <>
inline
  void doTransform(const tf2::Stamped<KDL::Vector>& t_in, tf2::Stamped<KDL::Vector>& t_out, const geometry_msgs::TransformStamped& transform)
  {
    t_out = tf2::Stamped<KDL::Vector>(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id);
  }

Use in code

With these methods defined you can use tf2::convert to go between a message and any datatype with these methods defined. And using slightly more logic it can actually convert between any two datatypes with a mutual message datatype intermediary.

The other place that this can be leveraged is that the tf2_ros API is entirely templated such that you can pass any supported datatype as an input and ask for any datatype with the above conditions as an output and it will work.

Recommendations for Implementers

This approach has been chosen to allow the core package to remain with as few dependencies as necessary. It is recommended to create standalone packages to support these conversions which have the minimum dependencies of the message package, the package with the definition of the datatype, and if necessary the minimum libraries necessary to implement the transform.

The convention for these packages is tf2_MESSAGE_DATATYPE_PACKAGE_NAME such as tf2_kdl, tf2_eigen, and tf2_bullet. These are also good example implementation packages for reference.