Revision history [back]

tf2 has a built-in function for this called quatRotate(). It is defined here on line 436: Quaternion.h.

Usage:

tf2::Vector3 v_orig = ... ;
tf2::Quaternion q = ... ;
tf2::Vector3 v_new = quatRotate(q, v_orig);

To convert your quaternion/vector3 from the geometry_msgs format to tf2::Quaternion or tf2::Vector3, do

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

tf2::Quaternion q;
tf2::Vector3 v_orig;
tf2::fromMsg(my_msg.my_orientation, q); // q is now loaded with data
tf2::fromMsg(my_msg.my_vector, v_orig); // v_orig is now loaded with data