Go to the documentation of this file.00001
00002 import rospy
00003 from std_msgs.msg import String
00004 from sensor_msgs.msg import MagneticField
00005 from math import atan2, sin, cos, sqrt
00006
00007 def callback(data):
00008
00009 xmag = data.magnetic_field.x
00010 ymag = data.magnetic_field.y
00011 zmag = data.magnetic_field.z
00012 mag_norm=sqrt((xmag*xmag)+(ymag*ymag)+(zmag*zmag))
00013
00014 magx=xmag/mag_norm
00015 magy=ymag/mag_norm
00016 magz=zmag/mag_norm
00017
00018 Roll = 0;
00019 Pitch = 0;
00020
00021
00022
00023 cos_pitch = cos(Pitch)
00024 sin_pitch = sin(Pitch)
00025 cos_roll = cos(Roll)
00026 sin_roll = sin(Roll)
00027 t_mag_x = magx * cos_pitch + magz * sin_pitch
00028 t_mag_y = magx * sin_roll * sin_pitch + magy * cos_roll - magz * sin_roll * cos_pitch
00029 head_x = t_mag_x
00030 head_y = t_mag_y
00031 yaw = atan2(head_x, head_y)
00032
00033
00034 rospy.loginfo(rospy.get_caller_id() + "yaw: %s", yaw)
00035
00036 def listener():
00037
00038 rospy.init_node('test', anonymous=True)
00039
00040 rospy.Subscriber("/mavros/imu/mag", MagneticField, callback)
00041
00042
00043 rospy.spin()
00044
00045 if __name__ == '__main__':
00046 listener()