Ask Your Question

Revision history [back]

Here's a simple Python version:

import numpy as np
import geometry_msgs.msg
import tf
import tf_conversions

p = geometry_msgs.msg.Pose()
# let's define a positive rotation of pi/4 rad about the reference frame's z-axis:
q = tf.transformations.quaternion_from_euler(np.pi/4, 0, 0, 'szyx')
p.orientation = geometry_msgs.msg.Quaternion(*q)

# now let's compute the desired x-axis components in a few ways:
print "x1 =",tf_conversions.fromMsg(p).M.UnitX()
print "x2 =",(tf.transformations.quaternion_matrix(q))[0:3,0]