#!/usr/bin/env python

import argparse
import rospy
from geometry_msgs.msg import TwistStamped
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
import tf
global x,y,th
global last_time
rospy.init_node("odom_relay")
last_time = rospy.Time.now()
x=0;y=0;th=0;

def callback(msg):
    global x,y,th,last_time
    current_time = rospy.Time.now()
    new_msg = Odometry()
    new_msg.header.stamp = msg.header.stamp
    new_msg.header.frame_id = "odom"
    new_msg.child_frame_id = "base_footprint"
    dt = (current_time - last_time).to_sec()
    delta_x=msg.twist.twist.linear.x*dt
    delta_y=msg.twist.twist.linear.y*dt
    delta_th=msg.twist.twist.angular.z*dt
    x += delta_x
    y += delta_y
    th += delta_th
    pi=3.14
    degree=th*pi/180
    if degree>=360:
        th=180*(degree-360)/pi
    odom_broadcaster = tf.TransformBroadcaster()
    odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
    
    odom_broadcaster.sendTransform(
        (x, y, 0.),
        odom_quat,
        current_time,
        "base_footprint",
        "odom"
    )
    new_msg.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))
    new_msg.twist=msg.twist
    
    pub.publish(new_msg)
    last_time = current_time

sub = rospy.Subscriber("controllers/diff_drive/odom", Odometry, callback, tcp_nodelay=True)
pub = rospy.Publisher("odom", Odometry, queue_size=1)

rospy.spin()