ROS中四元素欧拉角变换

python版本

#my_pkg/msg/Eulers.msg类型Header headerfloat64 rollfloat64 pitchfloat64 yaw#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 rospyimport tf# ROS messages.from nav_msgs.msg import Odometryfrom sensor_msgs.msg import Imufrom my_pkg.msg import Eulersclass 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

#launch 文件<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

---

C++版本

/****************************************************************************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;}

---

参考文档：

ROS论坛