#!/usr/bin/env python

import rospy

from nav_msgs.msg       import Odometry
from visualization_msgs.msg import Marker

class OdomToMarker:
    def __init__(self):
        self.odom_ekf_sub = rospy.Subscriber('/golfcart_ekf/combined_state', Odometry, self.odom_ekf, queue_size=10)
        self.marker_ekf_pub = rospy.Publisher( '/golfcart_ekf/combined_path',  Marker,       latch=True, queue_size=10)
        self.marker_ekf = Marker()

        self.odom_ins_sub = rospy.Subscriber('/golfcart_ekf/truth_odom', Odometry, self.odom_ins, queue_size=10)
        self.marker_ins_pub = rospy.Publisher( '/golfcart_ekf/ins_covariance_marker',  Marker,       latch=True, queue_size=10)
        self.marker_ins = Marker()


    def odom_ekf(self, msg):
        return
    def odom_ins(self, msg):
        self.marker_ins.header.stamp = rospy.Time.now()
        self.marker_ins.header.frame_id = 'world'
        self.marker_ins.type = self.marker_ins.CYLINDER
        self.marker_ins.action = self.marker_ins.ADD
        self.marker_ins.pose = msg.pose.pose
        self.marker_ins.scale.x = msg.pose.covariance[0]
        self.marker_ins.scale.y = msg.pose.covariance[1]
        #self.marker_ins.color.g = 1.0
        self.marker_ins.color.a = 0.6
        self.marker_ins.id = 1
        self.marker_ins_pub.publish(self.marker_ins)



if __name__ == '__main__':
    rospy.init_node('odom_covariance_marker')
    odom_to_marker = OdomToMarker()
    rospy.spin()