MP #2 TurtleBot

#! usr#!/usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

import time
import math

def shutdown():
	rospy.loginfo("Stop TurtleBot")
	pub.publish(Twist())
	rospy.sleep(1)

rospy.init_node('move_forward')
rospy.loginfo("To stop TurtleBot CTRL + C")
rospy.on_shutdown(shutdown)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
move = Twist()
rp = rospy.Rate(10)

#
# 1.2s for 90 deg
#
def ako_sa_eee():
    move.linear.x = 0.0
    move.angular.z = 0.0
    print("making forward")
    curr_time = time.time()
    while time.time()-curr_time <= 0.2:
	    pub.publish(move)
	    rp.sleep()

def fwd(t=3, v=0.5):
    move.linear.x = v
    move.angular.z = 0.0
    print("making forward")
    curr_time = time.time()
    while time.time()-curr_time <= t:
	    pub.publish(move)
	    rp.sleep()
    move.linear.x = 0.0
    move.angular.z = 0.0
    pub.publish(move)

def rgt():
    print("biglang liko")
    move.linear.x = 0.0
    move.angular.z = -1*math.pi/2
    curr_time = time.time()
    while time.time()-curr_time <= 1.05:
	    pub.publish(move)
	    rp.sleep()
    move.linear.x = 0.0
    move.angular.z = 0.0
    pub.publish(move)

def lft():
    print("biglang liko")
    move.linear.x = 0.0
    move.angular.z = 1*math.pi/2
    curr_time = time.time()
    while time.time()-curr_time <= 1.05:
	    pub.publish(move)
	    rp.sleep()
    pub.publish(Twist())
    move.linear.x = 0.0
    move.angular.z = 0.0
    pub.publish(move)

def e():
    print("\n--------------------------------------------")
    print("Drawing E")
    print("--------------------------------------------\n")
    fwd(1)
    lft()
    fwd(1)
    lft()
    fwd(1)
    lft()
    fwd(2)
    lft()
    fwd(1)
    ako_sa_eee()

def o():
    print("\n--------------------------------------------")
    print("Drawing O")
    print("--------------------------------------------\n")
    fwd(1)
    rgt()
    fwd(1)
    rgt()
    fwd(1)
    rgt()
    fwd(1)

def i():
    print("\n--------------------------------------------")
    print("Drawing I")
    print("--------------------------------------------\n")
    fwd(1)
    ako_sa_eee()

def l():
    print("\n--------------------------------------------")
    print("Drawing L")
    print("--------------------------------------------\n")
    fwd(1, -0.5)
    ako_sa_eee()

def c():
    print("\n--------------------------------------------")
    print("Drawing C")
    print("--------------------------------------------\n")
    lft()
    fwd(1)
    rgt()
    fwd(1)
    rgt()
    fwd(1)
    ako_sa_eee()

def t():
    print("\n--------------------------------------------")
    print("Drawing T")
    print("--------------------------------------------\n")
    fwd(2)
    lft()
    fwd(1)
    fwd(2, -0.5)
    ako_sa_eee()

def r():
    print("\n--------------------------------------------")
    print("Drawing R")
    print("--------------------------------------------\n")
    fwd(1)
    rgt()
    fwd(1)
    rgt()
    fwd(0.5)
    ako_sa_eee()

def a():
    print("\n--------------------------------------------")
    print("Drawing A")
    print("--------------------------------------------\n")
    fwd(2)
    rgt()
    fwd(1)
    rgt()
    fwd(2)
    fwd(1, -0.5)
    rgt()
    fwd(1)
    ako_sa_eee()

def n():
    print("\n--------------------------------------------")
    print("Drawing N")
    print("--------------------------------------------\n")
    fwd(1)
    rgt()
    fwd(1)
    rgt()
    fwd(1)
    ako_sa_eee()

def d():
    print("\n--------------------------------------------")
    print("Drawing D")
    print("--------------------------------------------\n")
    lft()
    fwd(1)
    lft()
    fwd(1)
    lft()
    fwd(1)
    lft()
    fwd(2)
    ako_sa_eee()

def s():
    print("\n--------------------------------------------")
    print("Drawing S")
    print("--------------------------------------------\n")
    lft()
    fwd(1)
    lft()
    fwd(1)
    lft()
    fwd(1)
    rgt()
    fwd(1)
    rgt()
    fwd(1)

def g():
    print("\n--------------------------------------------")
    print("Drawing G")
    print("--------------------------------------------\n")
    lft()
    fwd(1)
    rgt()
    fwd(1)
    rgt()
    fwd(1)
    rgt()
    fwd(2)
    rgt()
    fwd(1)
    ako_sa_eee()

g()