python版本
#my_pkg/msg/Eulers.msg类型
Header header
float64 roll
float64 pitch
float64 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 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
#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;
}
参考文档: