#!/usr/bin/env python

import roslib
import rospy 
import tf
from nav_msgs.msg import Odometry
from std_srvs.srv import Empty
import sys


rospy.init_node('odom2tf')

br = tf.TransformBroadcaster()

def odomCB(msg):
    pose = msg.pose.pose
    br.sendTransform((pose.position.x, pose.position.y, pose.position.z),
                     (pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w),
                     rospy.Time.now(),
                     'base_link',
                     'world')
    
rospy.Subscriber('state', Odometry, odomCB)


# unpause physics after a while
unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
#unpause.wait_for_service()
unpause()
    
rospy.spin()
