How to Subscribe to Imu data and broadcast transform

edit

I have a UAV application and I am using Hector-SLAM for the navigation. I have got Hector-SLAM working with just the map, base link and laser frames but I would like to add a base_stabilised frame to read data from the IMU and correctly reference the base link.

I have set up a static transform between the base_stabilised and base_link frames (as below) but I have not managed to vary this transform based on data from the IMU.

The topic with the IMU data is: "mavros/imu/data" and the message type is "sensor_msgs/Imu".

I have got the stabilized_to_link.py file to this stage but I don't know how to actually subscribe to the Imu data and create a transform from it. Please can someone help? Many Thanks!

import roslib
import rospy
import time
import tf
from sensor_msgs.msg import Imu

def publisher_of_tf():
rospy.init_node('imu_data_broadcaster', anonymous=True)
rate = rospy.Rate(10.0) #10Hz

imu_data = rospy.Subscriber("/mavros/imu/data", Imu) #Subscribes to /mavros/imu/data topic

br = tf.TransformBroadcaster()

while not rospy.is_shutdown():
    br.sendTransform((0.0, 0.0, 0.0),
                     (0.0, 0.0, 0.0, 1.0),
                     rospy.Time.now(),
         #child
                     "base_link",
         #parent
                     "base_stabilized")
         rate.sleep()   

if __name__ == '__main__':
try:
    publisher_of_tf()
except rospy.ROSInterruptException:
    pass