sensor_service_imu

#!/usr/bin/env python
#
# Sensor service IMU is a node to
#  * convert quaternion orientation into heading angle with our convention
#  * transform NED (North East Down) reference IMU data into ENU (East North Up)
#
# Subscribe to: imu/data (Imu)
# Published topic: heading (Float32)


from __future__ import division

import rospy
import tf
import math

from std_msgs.msg import Float32
from sensor_msgs.msg import Imu

class heading_processing(object):
    def __init__(self):
        rospy.init_node('Heading_service')
        self.heading = 0
        self.heading_pub = rospy.Publisher('heading', Float32, queue_size=10)
        self.pitch_pub = rospy.Publisher('pitch', Float32, queue_size=10)
        self.roll_pub = rospy.Publisher('roll', Float32, queue_size=10)
        rospy.Subscriber('imu/data', Imu, self.heading_publisher)

    def heading_publisher(self, msg):
        imu = msg.orientation
        self.heading = (math.degrees(
                        tf.transformations.euler_from_quaternion(
                        (imu.x, imu.y,imu.z, imu.w))[2]) - 90) % 360
        self.pitch = math.degrees(
                        tf.transformations.euler_from_quaternion(
                        (imu.x, imu.y,imu.z, imu.w))[1])
        self.roll = math.degrees(
                        tf.transformations.euler_from_quaternion(
                        (imu.x, imu.y,imu.z, imu.w))[0])



    def run(self):
        r = rospy.Rate(20)
        while not rospy.is_shutdown():
            self.heading_pub.publish(self.heading)
            r.sleep()

if __name__ == '__main__':
    process = heading_processing()
    try:
        process.run()
    except rospy.ROSInterruptException:
        pass