Quaternion transformations in Python

asked 2013-07-29 21:21:59 -0600

nevik
241 ●4 ●4 ●6

updated 2014-01-28 17:17:28 -0600

ngrennan
1 ●1 ●1

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 flag offensive close merge delete

add a comment

47

answered 2013-07-30 00:15:34 -0600

Miguel S.
1114 ●12 ●16 ●24

updated 2013-07-30 00:19:17 -0600

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[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]

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[0]
pitch = euler[1]
yaw = euler[2]

edit flag offensive delete link

Comments

11

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))

vtalpaert ( 2016-08-16 04:28:19 -0600 )edit

2

Also remember to import

import tf
import geometry_msgs

And for someone who doesn't know star "*" here *tf_conversions in python is an unpacking operator which as name suggests unpacks list to separate argument for the function

Combinacijus ( 2021-03-22 17:32:32 -0600 )edit

add a comment

6

answered 2013-07-30 12:00:26 -0600

forrestv
154 ●1 ●1 ●5

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)

edit flag offensive delete link

add a comment

3

answered 2017-10-18 04:45:38 -0600

R. Tellez
874 ●4 ●18 ●14

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.