#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
import sys, select, tty

def getKey(key_timeout):
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], key_timeout)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''
    return key

pub = rospy.Publisher('/part2_cmr/cmd_vel', Twist, queue_size=1)
rospy.init_node('command_node', anonymous=True)
rate = rospy.Rate(10)  # 10hz
cmd = Twist()
print ('q - left rotation | w - move forward | e - right rotation')
print ('a - move left     | s -move backward | d - move right')
while not rospy.is_shutdown():
    k = getKey(1)
    if k == ('w'):
        cmd.linear.x = 0.5
    elif k == ('s'):
        cmd.linear.x = -0.5
    elif k == ('a'):
        cmd.linear.x = 0.5
        cmd.angular.z = 0.2
    elif k == ('d'):
        cmd.linear.x = 0.5
	cmd.angular.z = -0.2
    elif k == ('q'):
        cmd.angular.z = 0.5
    elif k == ('e'):
        cmd.angular.z = -0.5
    elif k == ('x'):
        rospy.signal_shutdown("shut")
    else:
        cmd.linear.x = 0.0
        cmd.angular.z = 0.0
    pub.publish(cmd)
    rate.sleep()
    cmd = Twist()
    pub.publish(cmd)
