Revision history [back]

tf2 has a Transform class that overrides the * operator to actually convert points between frames using the geometry_msgs/TransformStamped results from the TF lookup. Below is a snippet:

geometry_msgs::TransformStamped tf_result;
try {
  tf_result = buffer.lookupTransform("parent_frame", "child_frame", ros::Time(0));
} catch (tf2::TransformException& ex) {
  // TODO: handle lookup failure appropriately
}

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
tf2::Quaternion q(
    tf_result.transform.rotation.x,
    tf_result.transform.rotation.y,
    tf_result.transform.rotation.z,
    tf_result.transform.rotation.w
);
tf2::Vector3 p(
    tf_result.transform.translation.x,
    tf_result.transform.translation.y,
    tf_result.transform.translation.z
);
tf2::Transform transform(q, p);
tf2::Vector3 point_in_child_coordinates(1, 2, 3);
tf2::Vector3 point_in_parent_coordinates = transform * point_in_child_coordinates;

If you want to convert the other way, from parent coordinate to child coordinates:

tf2::Vector3 child_coordinates = transform.inverse() * point_in_parent_coordinates;