Ask Your Question

Revision history [back]

Assuming that your Pose and Twist are already in the same reference frame, simply add the linear portion of the Twist to your pose's position.

Then, convert your Pose's orientation to a tf::Quaternion. Store the angular portion of the Twist as a second quaternion using setRPY() and multiply the two Quaternions together.