quaternion_rpy.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest("cart_sim");
00003 import rospy
00004 import tf
00005 
00006 # ROS messages.
00007 from nav_msgs.msg import Odometry
00008 from sensor_msgs.msg import Imu
00009 from auv_msgs.msg import RPY
00010 
00011 class QuatToEuler():
00012     def __init__(self):
00013         self.got_new_msg = False
00014         self.euler_msg = RPY()
00015 
00016         # Create subscribers and publishers.
00017         sub_imu   = rospy.Subscriber("imu", Imu, self.imu_callback)
00018         sub_odom  = rospy.Subscriber("odom", Odometry, self.odom_callback)
00019         pub_euler = rospy.Publisher("euler", RPY)
00020 
00021         # Main while loop.
00022         while not rospy.is_shutdown():
00023             # Publish new data if we got a new message.
00024             if self.got_new_msg:
00025                 pub_euler.publish(self.euler_msg)
00026                 self.got_new_msg = False
00027 
00028     # Odometry callback function.
00029     def odom_callback(self, msg):
00030         # Convert quaternions to Euler angles.
00031         (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])
00032         self.fill_euler_msg(msg, r, p, y)
00033 
00034     # IMU callback function.
00035     def imu_callback(self, msg):
00036         # Convert quaternions to Euler angles.
00037         (r, p, y) = tf.transformations.euler_from_quaternion([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w])
00038         self.fill_euler_msg(msg, r, p, y)
00039 
00040     # Fill in Euler angle message.
00041     def fill_euler_msg(self, msg, r, p, y):
00042         self.got_new_msg = True
00043         self.euler_msg.roll  = r
00044         self.euler_msg.pitch = p
00045         self.euler_msg.yaw   = y
00046 
00047 # Main function.    
00048 if __name__ == '__main__':
00049     # Initialize the node and name it.
00050     rospy.init_node('quat_to_euler')
00051     # Go to class functions that do all the heavy lifting. Do error checking.
00052     try:
00053         quat_to_euler = QuatToEuler()
00054     except rospy.ROSInterruptException: pass


labust_uvapp
Author(s): Dula Nad
autogenerated on Fri Feb 7 2014 11:36:37