ROS Resources: Documentation | Support | Discussion Forum | Service Status | Q&A answers.ros.org
Ask Your Question
16

Quaternion transformations in Python

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

nevik gravatar image

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

ngrennan gravatar image

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

4 Answers

Sort by » oldest newest most voted
23

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

Miguel S. gravatar image

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 more

Comments

5

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 gravatar imagevtalpaert ( 2016-08-16 04:28:19 -0600 )edit
5

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

forrestv gravatar image

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 more
2

answered 2017-04-28 13:52:26 -0600

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

edit flag offensive delete link more
0

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

R. Tellez gravatar image

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.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

6 followers

Stats

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

Seen: 28,915 times

Last updated: Oct 18