#!/usr/bin/env python

# ME133A - Lab 2      Move the turtlebot along some basic closed shape
#                     (so that it theoretically will end where it started)

# A basic script to make the turtlebot move forwards for 2 seconds. Press CTRL + C to stop.  To run:
# On TurtleBot:
# roslaunch turtlebot_bringup minimal.launch
# On work station:
# python goforward.py

import rospy
from geometry_msgs.msg import Twist
from kobuki_msgs.msg import SensorState
import time
import math
import csv

class DrawShape():
    def __init__(self):
        # initiliaze
        rospy.init_node('ME133a_Lab_1', anonymous=False)

        # tell user how to stop TurtleBot
        rospy.loginfo("To stop TurtleBot CTRL + C")

        # What function to call when you ctrl + c    
        rospy.on_shutdown(self.shutdown)
            
        # Create a publisher which can "talk" to TurtleBot and tell it to move
        # Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if you're not using TurtleBot2
        self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
        
        # Open a file to write all the sensor values to
        self.file = open('encoderValues.csv','w')
        self.file.write('Left Encoder,Right Encoder\n')
        
        # Subscribe to the sensor core topic to read the encoder values
        self.subscriber = rospy.Subscriber("/mobile_base/sensors/core",SensorState,self.writeData)
         
        #TurtleBot will stop if we don't keep telling it to move.  How often should we tell it to move? 10 HZ
        r = rospy.Rate(10)
        t0 = time.time()
        
        ######################################################
        #----------------Modify this code---------------------
        # Draw out some basic shape
        # NOTE: linear is in m/s and angular is meant to be in
        # rad/s but moves at only around 75% intended speed,
        # which is compensated for by the factor of 1.33 applied
        ######################################################

        # Here we'll loop through a series of commands
        while (time.time() - t0 < 5.0):
            move_cmd = Twist()
            if (time.time() - t0 < 2.0):
                move_cmd.linear.x = 0.2
                move_cmd.angular.z = 0
            # Publish the commands to the turtlebot node then wait for the next cycle
            # move_cmd.angular.z = move_cmd.angular.z * 1.33; # Offset to correct the rotational speed (uncomment if using the real turtlebot)
            self.cmd_vel.publish(move_cmd)
            r.sleep()
        
        ######################################################
    
    # Write data from the encoders to the csv we opened
    def writeData(self,data):
        self.file.write('%(left)d,%(right)d\n' % {'left': data.left_encoder,'right': data.right_encoder})

    # Unsubscribe from the sensor topic and tell the turtlebot to stop moving
    def shutdown(self):
        self.subscriber.unregister()
        self.file.close()
        rospy.loginfo("Stop TurtleBot")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)
 
if __name__ == '__main__':
    try:
        DrawShape()
    except:
        rospy.loginfo("DrawShape node terminated.")