#!/usr/bin/env python

# Translates from NavSatFix to GPSFix and back

import rospy
from sensor_msgs.msg import NavSatFix
from gps_common.msg import GPSFix
import gps_common.gps_message_converter as converter

navsat_pub = rospy.Publisher('navsat_fix_out', NavSatFix, queue_size=10)
gps_pub = rospy.Publisher('gps_fix_out', GPSFix, queue_size=10)


def navsat_callback(navsat_msg):
    gps_msg = converter.navsatfix_to_gpsfix(navsat_msg)
    gps_pub.publish(gps_msg)


# Translates from GPSFix to NavSatFix.
# As GPSFix can store much more information than NavSatFix, 
# a lot of this additional information might get lost.
def gps_callback(gps_msg):
    navsat_msg = converter.gpsfix_to_navsatfix(gps_msg)
    navsat_pub.publish(navsat_msg)


if __name__ == '__main__':
    rospy.init_node('fix_translator', anonymous=True)
    navsat_sub = rospy.Subscriber("navsat_fix_in", NavSatFix, navsat_callback)
    gps_sub = rospy.Subscriber("gps_fix_in", GPSFix, gps_callback)
    rospy.spin()
