ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

plot/print rpy from quaternion

asked 2011-10-13 15:12:36 -0600

brice rebsamen gravatar image

updated 2011-10-13 15:35:26 -0600


I have topics that publish quaternions (e.g. Pose, Odometry, etc.). This is not very human-friendly, e.g. when doing a rostopic echo, or a rxplot.

I am dealing with a car, and I can assume a 2D model most of the time, so yaw=2*acos(w). I know this can be done from the code, so what I'm doing for now is to publish in parallel a message that gives rpy, that I can use for debugging. But it would be nice if there was some kind of filter that could be applied on live or recorded data to do the transformation.

edit retag flag offensive close merge delete

3 Answers

Sort by ยป oldest newest most voted

answered 2011-10-13 17:29:00 -0600

Thomas D gravatar image

updated 2012-09-05 08:11:18 -0600

Here is a Python node I wrote that subscribes to quaternions, converts quaternions to Euler angles, and then publish the Euler angles. The Euler angles are in a custom message, my_pkg/msg/Eulers.msg:

Header header
float64 roll
float64 pitch
float64 yaw

Then create the file my_pkg/

# -*- coding: utf-8 -*-

# Start up ROS pieces.
PKG = 'my_pkg'
import roslib; roslib.load_manifest(PKG)
import rospy
import tf

# ROS messages.
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu
from my_pkg.msg import Eulers

class QuatToEuler():
    def __init__(self):
        self.got_new_msg = False
        self.euler_msg = Eulers()

        # Create subscribers and publishers.
        sub_imu   = rospy.Subscriber("imu", Imu, self.imu_callback)
        sub_odom  = rospy.Subscriber("odom", Odometry, self.odom_callback)
        pub_euler = rospy.Publisher("euler", Eulers)

        # Main while loop.
        while not rospy.is_shutdown():
            # Publish new data if we got a new message.
            if self.got_new_msg:
                self.got_new_msg = False

    # Odometry callback function.
    def odom_callback(self, msg):
        # Convert quaternions to Euler angles.
        (r, p, y) = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
        self.fill_euler_msg(msg, r, p, y)

    # IMU callback function.
    def imu_callback(self, msg):
        # Convert quaternions to Euler angles.
        (r, p, y) = tf.transformations.euler_from_quaternion([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w])
        self.fill_euler_msg(msg, r, p, y)

    # Fill in Euler angle message.
    def fill_euler_msg(self, msg, r, p, y):
        self.got_new_msg = True
        self.euler_msg.header.stamp = msg.header.stamp
        self.euler_msg.roll  = r
        self.euler_msg.pitch = p
        self.euler_msg.yaw   = y

# Main function.    
if __name__ == '__main__':
    # Initialize the node and name it.
    # Go to class functions that do all the heavy lifting. Do error checking.
        quat_to_euler = QuatToEuler()
    except rospy.ROSInterruptException: pass

In my launch file I start it with:

<node pkg="my_pkg" type="" name="imu_euler_angles">
    <remap from="imu"   to="microstrain/data"/>
    <remap from="euler" to="microstrain/euler"/>

<node pkg="my_pkg" type="" name="kf_euler_angles">
    <remap from="odom"  to="kf/odom"/>
    <remap from="euler" to="kf/euler"/>

Then you can plot your IMU roll vs. your KF roll (for example):

rxplot /microstrain/euler/roll:/kf/euler/roll
edit flag offensive delete link more


@Thomas D - could you guide me for the same w.r.t C++?? I am new to ROS and my programming isn't that good to understand the source code of the package that I am working on -> ros_vrpn_client. Will it be possible for you to check the source code and guide me a bit?

nemesis gravatar image nemesis  ( 2013-03-06 11:34:33 -0600 )edit

Why are the euler_from_quaternion arguments in the order x-y-z-w? The axes argument defaults to 'sxyz'instead!

jondo gravatar image jondo  ( 2015-06-22 09:19:52 -0600 )edit

Since this post is somewhat dated, can I ask if it applies REP-103 using ENU? If so what needs to be changed to convert rpy back to NED coordinate frame convention?

b2256 gravatar image b2256  ( 2016-07-15 08:02:43 -0600 )edit

answered 2013-03-18 12:59:59 -0600

georgwi gravatar image

Hey, here is a c++ solution to your problem :) just in case you still have use for it! Needed the transformation from quaternion to rpy-angles too so here is what I put together. Used the code fragment from (here) for the actual transformation.


Conversion from a quaternion to roll, pitch and yaw.

subscribed /rotation_quaternion (message of type geometry_msgs::Quaternion)
published /rpy_angles (message oftype geometry_msgs::Vector3.h)


#include "ros/ros.h"
#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/Quaternion.h"
#include "tf/transform_datatypes.h"
#include "LinearMath/btMatrix3x3.h"

// Here I use global publisher and subscriber, since I want to access the
// publisher in the function MsgCallback:
ros::Publisher rpy_publisher;
ros::Subscriber quat_subscriber;

// Function for conversion of quaternion to roll pitch and yaw. The angles
// are published here too.
void MsgCallback(const geometry_msgs::Quaternion msg)
    // the incoming geometry_msgs::Quaternion is transformed to a tf::Quaterion
    tf::Quaternion quat;
    tf::quaternionMsgToTF(msg, quat);

    // the tf::Quaternion has a method to acess roll pitch and yaw
    double roll, pitch, yaw;
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

    // the found angles are written in a geometry_msgs::Vector3
    geometry_msgs::Vector3 rpy;
    rpy.x = roll;
    rpy.y = pitch;
    rpy.z = yaw;

    // this Vector is then published:
    ROS_INFO("published rpy angles: roll=%f pitch=%f yaw=%f", rpy.x, rpy.y, rpy.z);

int main(int argc, char **argv)
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    rpy_publisher = n.advertise<geometry_msgs::Vector3>("rpy_angles", 1000);
    quat_subscriber = n.subscribe("rotation_quaternion", 1000, MsgCallback);

    // check for incoming quaternions untill ctrl+c is pressed
    ROS_INFO("waiting for quaternion");
    return 0;
edit flag offensive delete link more


@georgwi - Could you please have a look at this as well -

nemesis gravatar image nemesis  ( 2013-03-20 11:22:39 -0600 )edit

Thanks a lot. this was very helpful.

Rika gravatar image Rika  ( 2021-06-27 01:47:17 -0600 )edit

answered 2013-03-21 05:54:39 -0600

devesh gravatar image

updated 2016-02-24 13:16:23 -0600

130s gravatar image

There is a well documented page which gives information about all the apis, etc.

Hope this helps others who come across this question later on.

edit flag offensive delete link more

Question Tools



Asked: 2011-10-13 15:12:36 -0600

Seen: 27,440 times

Last updated: Feb 24 '16