Ask Your Question
6

plot/print rpy from quaternion

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

brice rebsamen gravatar image

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

Hi

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
12

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

Thomas D gravatar image

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

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/quat_to_euler.py:

#!/usr/bin/python
# -*- 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:
                pub_euler.publish(self.euler_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.
    rospy.init_node('quat_to_euler')
    # Go to class functions that do all the heavy lifting. Do error checking.
    try:
        quat_to_euler = QuatToEuler()
    except rospy.ROSInterruptException: pass

In my launch file I start it with:

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

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

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

Comments

@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 imagenemesis ( 2013-03-06 11:34:33 -0500 )edit

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

jondo gravatar imagejondo ( 2015-06-22 09:19:52 -0500 )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 imageb2256 ( 2016-07-15 08:02:43 -0500 )edit
13

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

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 http://answers.ros.org/question/36977/invalid-arguments-convert-from-quaternion-to-euler/ (here) for the actual transformation.

/****************************************************************************

Conversion from a quaternion to roll, pitch and yaw.

Nodes:
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:
    rpy_publisher.publish(rpy);
    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");
    ros::spin();
    return 0;
}
edit flag offensive delete link more

Comments

@georgwi - Could you please have a look at this as well - http://answers.ros.org/question/58742/quaternion-to-roll-pitch-yaw/

nemesis gravatar imagenemesis ( 2013-03-20 11:22:39 -0500 )edit
1

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

devesh gravatar image

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

130s gravatar image

There is a well documented page which gives information about all the apis, etc. http://www.ros.org/wiki/geometry/Rota...

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

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

2 followers

Stats

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

Seen: 17,291 times

Last updated: Feb 24 '16