Revision history [back]

I'm not aware of any existing functions, but I'm sure you could google around some for the proper code snippet.

But, I would just construct atf2::Matrix3x3 from the rotational components, then create a tf2::Vector3 from the rest.

Then, you can just convert to the message.

tf2::Vector3 position;  /* obtained from 4th column */
tf2::Matrix3x3 rot_robot;  /* obtained from first three columns */
tf2::Quaternion orientation;
rot_robot.getRotation(orientation);

geometry_msgs::Pose out;
tf2::toMsg(position, out.position);
tf2::toMsg(orientation, out.orientation);