Revision history [back]

I'm not sure how to have quaternions filtered for rxplot. However, to do the conversion (without assuming a 2D model) I do this:

void Nav::compassCallback(const sensor_msgs::Imu::ConstPtr& msg)
{
    // Convert quaternion to RPY.
    btScalar tmp_roll = 0.0, tmp_pitch = 0.0, tmp_yaw = 0.0;
    btQuaternion q;
    tf::quaternionMsgToTF(msg->orientation, q);
    btMatrix3x3(q).getRPY(tmp_roll, tmp_pitch, tmp_yaw);
    roll  = tmp_roll;
    pitch = tmp_roll;
    yaw   = tmp_yaw;
    ROS_DEBUG("RPY = (%lf, %lf, %lf)\n", roll, pitch, yaw);
} // end compassCallback()

Here, I am subscribing to a compass topic publishing a sensor_msgs::Imu message that uses a quaternion for the orientation. The variables roll, pitch and yaw are members of the Nav class and they are type double. It's necessary to pass btScalar types to the getRPY function, but then you can move the btScalar values to doubles.

I'm not sure how to have quaternions filtered for rxplot. However, to do the conversion (without assuming a 2D model) I do this:

void Nav::compassCallback(const sensor_msgs::Imu::ConstPtr& msg)
{
    // Convert quaternion to RPY.
    btScalar tmp_roll = 0.0, tmp_pitch = 0.0, tmp_yaw = 0.0;
btQuaternion q;
 btQuaternion q;
double roll, pitch, yaw;
    tf::quaternionMsgToTF(msg->orientation, q);
    btMatrix3x3(q).getRPY(tmp_roll, tmp_pitch, tmp_yaw);
    roll  = tmp_roll;
    pitch = tmp_roll;
    yaw   = tmp_yaw;
btMatrix3x3(q).getRPY(roll, pitch, yaw);
    ROS_DEBUG("RPY = (%lf, %lf, %lf)\n", %lf)", roll, pitch, yaw);
} // end compassCallback()

Here, I am subscribing to a compass topic publishing a sensor_msgs::Imu message that uses a quaternion for the orientation. The variables roll, pitch and yaw are normally members of the Nav class and they are type double. It's necessary to pass class, but I am showing them declared inside this callback for clarity. The getRPY function accepts either double or btScalar types to the getRPY function, but then you can move the btScalar values to doubles.types.

I'm not sure how Here is a Python node I wrote that subscribes to have quaternions, converts quaternions filtered for rxplot. However, to do Euler angles, and then publish the conversion (without assuming Euler angles. The Euler angles are in a 2D model) I do thiscustom message, my_pkg/msg/Eulers.msg:

void Nav::compassCallback(const sensor_msgs::Imu::ConstPtr& msg)
{
    // Convert quaternion to RPY.
    btQuaternion q;
    double roll, pitch, yaw;
    tf::quaternionMsgToTF(msg->orientation, q);
    btMatrix3x3(q).getRPY(roll, pitch, yaw);
    ROS_DEBUG("RPY = (%lf, %lf, %lf)", roll, pitch, yaw);
} // end compassCallback()
Header header
float64 roll
float64 pitch
float64 yaw

Here, 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 acs_utils.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 am subscribing to a compass topic publishing a sensor_msgs::Imu message that uses a quaternion for the orientation. The variables roll, pitch and yaw are normally members of the Nav class, but I am showing them declared inside this callback for clarity. The getRPY function accepts either double or btScalar types.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>

rxplot /microstrain/euler/roll:/kf/euler/roll

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 acs_utils.msg 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