00001
00002 import roslib; roslib.load_manifest('teleop_twist_keyboard')
00003 import rospy
00004
00005 from geometry_msgs.msg import Twist
00006
00007 import sys, select, termios, tty
00008
00009 msg = """
00010 Reading from the keyboard and Publishing to Twist!
00011 ---------------------------
00012 Moving around:
00013 u i o
00014 j k l
00015 m , .
00016
00017 q/z : increase/decrease max speeds by 10%
00018 w/x : increase/decrease only linear speed by 10%
00019 e/c : increase/decrease only angular speed by 10%
00020 anything else : stop
00021
00022 CTRL-C to quit
00023 """
00024
00025 moveBindings = {
00026 'i':(1,0),
00027 'o':(1,-1),
00028 'j':(0,1),
00029 'l':(0,-1),
00030 'u':(1,1),
00031 ',':(-1,0),
00032 '.':(-1,1),
00033 'm':(-1,-1),
00034 }
00035
00036 speedBindings={
00037 'q':(1.1,1.1),
00038 'z':(.9,.9),
00039 'w':(1.1,1),
00040 'x':(.9,1),
00041 'e':(1,1.1),
00042 'c':(1,.9),
00043 }
00044
00045 def getKey():
00046 tty.setraw(sys.stdin.fileno())
00047 select.select([sys.stdin], [], [], 0)
00048 key = sys.stdin.read(1)
00049 termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
00050 return key
00051
00052 speed = .5
00053 turn = 1
00054
00055 def vels(speed,turn):
00056 return "currently:\tspeed %s\tturn %s " % (speed,turn)
00057
00058 if __name__=="__main__":
00059 settings = termios.tcgetattr(sys.stdin)
00060
00061 pub = rospy.Publisher('cmd_vel', Twist)
00062 rospy.init_node('teleop_twist_keyboard')
00063
00064 x = 0
00065 th = 0
00066 status = 0
00067
00068 try:
00069 print msg
00070 print vels(speed,turn)
00071 while(1):
00072 key = getKey()
00073 if key in moveBindings.keys():
00074 x = moveBindings[key][0]
00075 th = moveBindings[key][1]
00076 elif key in speedBindings.keys():
00077 speed = speed * speedBindings[key][0]
00078 turn = turn * speedBindings[key][1]
00079
00080 print vels(speed,turn)
00081 if (status == 14):
00082 print msg
00083 status = (status + 1) % 15
00084 else:
00085 x = 0
00086 th = 0
00087 if (key == '\x03'):
00088 break
00089
00090 twist = Twist()
00091 twist.linear.x = x*speed; twist.linear.y = 0; twist.linear.z = 0
00092 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
00093 pub.publish(twist)
00094
00095 except:
00096 print e
00097
00098 finally:
00099 twist = Twist()
00100 twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
00101 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
00102 pub.publish(twist)
00103
00104 termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
00105
00106
