#!/usr/bin/env python
from __future__ import division

import rospy 
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from time import sleep
import numpy as np

g_orange_left_frac = 0
g_orange_right_frac = 0

g_centered_difference = 0.01

def frac_callback(orange_frac):
  global g_orange_left_frac
  global g_orange_right_frac

  g_orange_left_frac = float(orange_frac.data.split(' ')[0])
  g_orange_right_frac = float(orange_frac.data.split(' ')[1])
  sleep(0.1)
  
  
def run_control_publisher():
  control_publisher = rospy.Publisher('/ros_project/cmd_vel', Twist, queue_size=1)
  rate = rospy.Rate(1)
  
  vel_msg = Twist()
  vel_msg.linear.x = 0
  vel_msg.linear.y = 0
  vel_msg.linear.z = 0
  vel_msg.angular.x = 0
  vel_msg.angular.y = 0
  vel_msg.angular.z = 0
    
  while not rospy.is_shutdown():
    if g_orange_left_frac > 0.0 or g_orange_right_frac > 0.0:
 	vel_msg.angular.z = g_orange_right_frac - g_orange_left_frac
	vel_msg.linear.x = -0.5
    else:
 	vel_msg.angular.z = .25

    control_publisher.publish(vel_msg)
    rate.sleep()
    
if __name__ == '__main__':
  rospy.init_node('orange_frac_controller', anonymous=True)
  rospy.Subscriber('/ros_project/orange_frac', String, frac_callback)

  try:
    run_control_publisher()
  except rospy.ROSInterruptException:
    pass
    
