# Quaternion transformations in Python

I'm trying to place some Markers in RViz, using a node written in Python. To this end, I need to create a geometry_msgs.mgs.Pose with an orientation Quaternion.

But I can't for the life of me find the utility and conversion functions that I need for Quaternions. There are some in tf.transformations, but those produce numpy Quaternions, which I would have to manually split up and throw into the geometry_msgs.msg.Quaternion constructor.

Considering that Quaternions are of little use without a set of functions to work with them, my question is: Where are the proper conversion functions for Quaternions? That is, functions to create and modify geometry_msgs.msg.Quaternion or to convert between a numpy Quaternion (4-tuple) and geometry_msgs.msg.Quaternion (object with x,y,z,w attributes).

My environment is Ubuntu 12.04 Precise, with ROS fuerte (using fuerte is currently mandatory for this project; I'm already looking into upgrading, but that's not at all easy).

edit retag close merge delete

Sort by » oldest newest most voted

I also remember having problems with this a few months back, the documentation is not overly clear; but in the end it is very simple.

To convert from euler to quaternion:

quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
#type(pose) = geometry_msgs.msg.Pose
pose.orientation.x = quaternion
pose.orientation.y = quaternion
pose.orientation.z = quaternion
pose.orientation.w = quaternion


And to convert from quaternion to euler:

#type(pose) = geometry_msgs.msg.Pose
quaternion = (
pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
roll = euler
pitch = euler
yaw = euler

more

9

As an update in 2016, it could be written as:

pose.orientation = geometry_msgs.msg.Quaternion(*tf_conversions.transformations.quaternion_from_euler(roll, pitch, yaw))


To convert from a numpy array to a Quaternion message type, it's just:

q = numpy.array([.5, .5, .5, .5])
from geometry_msgs.msg import Quaternion
pub.publish(Quaternion(*q))


Converting from the message type to a numpy array is harder and I could never find a provided function, so I usually do:

xyzw_array = lambda o: numpy.array([o.x, o.y, o.z, o.w])
print xyz_array(msg.pose.orientation)

more

The following example can help you understand how to use the conversion from quaternions provided by an Odometry message to Euler angles Roll, Pitch and Yaw.

In the following example, you can see that the program subscribes to the odometry topic to get the orientation of a Turtlebot3 robot. Then it uses the tf.transformation functions to obtain the yaw value (the actual 'z' value or orientation of the odometry).

Then the program uses the roll, pitch and yaw to convert them back into a quaternion.

#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion, quaternion_from_euler

roll = pitch = yaw = 0.0

def get_rotation (msg):
global roll, pitch, yaw
orientation_q = msg.pose.pose.orientation
orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
(roll, pitch, yaw) = euler_from_quaternion (orientation_list)
print yaw

rospy.init_node('my_quaternion_to_euler')

sub = rospy.Subscriber ('/odom', Odometry, get_rotation)

r = rospy.Rate(1)
while not rospy.is_shutdown():
quat = quaternion_from_euler (roll, pitch,yaw)
print quat
r.sleep()


I have created a video that shows how I applied this code to a Turtlebot 3 robot to get its odometry and then convert it to Euler using that code.

more

The way you make this work is:

explicit_quat = [quaternion.x, quaternion.y, quaternion.z, quaternion.w]

your_euler = tf.transformations.euler_from_quaternion(explicit_quat)

Hope this helps

more