getYaw.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #AUTHOR: Younghun Ju <yhju@yujinrobot.comm>, <yhju83@gmail.com>
00003 
00004 import roslib; roslib.load_manifest('kobuki_node')
00005 import rospy
00006 
00007 from tf.transformations import euler_from_quaternion
00008 from math import degrees
00009 
00010 from sensor_msgs.msg import Imu
00011 from geometry_msgs.msg import Quaternion
00012 from std_msgs.msg import Float64
00013 
00014 class Converter(object):
00015 
00016         def __init__(self):
00017                 rospy.init_node("getYaw", anonymous=True)
00018                 self.sub_imu  = rospy.Subscriber("imu", Imu, self.ImuCallback)
00019                 self.pub_angle = rospy.Publisher("angle", Float64)
00020                 self.pub_angle_rate = rospy.Publisher("angle_rate", Float64)
00021 
00022         def ImuCallback(self,data):
00023                 quat = data.orientation
00024                 q = [quat.x, quat.y, quat.z, quat.w]
00025                 roll, pitch, yaw = euler_from_quaternion(q)
00026                 print "angle: " + "{0:+.4f}".format(yaw) + " rad; "\
00027                 + "{0:+.2f}".format(degrees(yaw)) + " deg"
00028                 print "rate:  " + "{0:+.2f}".format(data.angular_velocity.z) + " rad/s; "\
00029                 + "{0:+.2f}".format(degrees(data.angular_velocity.z)) + " deg/s"
00030                 print '---'
00031                 angle = Float64()
00032                 angle_rate = Float64()
00033                 angle.data = yaw
00034                 angle_rate.data = data.angular_velocity.z
00035                 self.pub_angle.publish(angle)
00036                 self.pub_angle_rate.publish(angle_rate)
00037 
00038 if __name__ == '__main__':
00039         try:
00040                 instance = Converter()
00041                 print 
00042                 print "It prints angle and angular_velocity from Imu message of single yaw gyro."
00043                 print
00044                 rospy.spin()
00045         except rospy.ROSInterruptException: pass


kobuki_node
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Wed Sep 16 2015 04:35:37