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 For Holonomic mode (strafing), hold down the shift key:
00018 ---------------------------
00019 U I O
00020 J K L
00021 M < >
00022
00023 t : up (+z)
00024 b : down (-z)
00025
00026 anything else : stop
00027
00028 q/z : increase/decrease max speeds by 10%
00029 w/x : increase/decrease only linear speed by 10%
00030 e/c : increase/decrease only angular speed by 10%
00031
00032 CTRL-C to quit
00033 """
00034
00035 moveBindings = {
00036 'i':(1,0,0,0),
00037 'o':(1,0,0,-1),
00038 'j':(0,0,0,1),
00039 'l':(0,0,0,-1),
00040 'u':(1,0,0,1),
00041 ',':(-1,0,0,0),
00042 '.':(-1,0,0,1),
00043 'm':(-1,0,0,-1),
00044 'O':(1,-1,0,0),
00045 'I':(1,0,0,0),
00046 'J':(0,1,0,0),
00047 'L':(0,-1,0,0),
00048 'U':(1,1,0,0),
00049 '<':(-1,0,0,0),
00050 '>':(-1,-1,0,0),
00051 'M':(-1,1,0,0),
00052 't':(0,0,1,0),
00053 'b':(0,0,-1,0),
00054 }
00055
00056 speedBindings={
00057 'q':(1.1,1.1),
00058 'z':(.9,.9),
00059 'w':(1.1,1),
00060 'x':(.9,1),
00061 'e':(1,1.1),
00062 'c':(1,.9),
00063 }
00064
00065 def getKey():
00066 tty.setraw(sys.stdin.fileno())
00067 select.select([sys.stdin], [], [], 0)
00068 key = sys.stdin.read(1)
00069 termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
00070 return key
00071
00072
00073 def vels(speed,turn):
00074 return "currently:\tspeed %s\tturn %s " % (speed,turn)
00075
00076 if __name__=="__main__":
00077 settings = termios.tcgetattr(sys.stdin)
00078
00079 pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
00080 rospy.init_node('teleop_twist_keyboard')
00081
00082 speed = rospy.get_param("~speed", 0.5)
00083 turn = rospy.get_param("~turn", 1.0)
00084 x = 0
00085 y = 0
00086 z = 0
00087 th = 0
00088 status = 0
00089
00090 try:
00091 print msg
00092 print vels(speed,turn)
00093 while(1):
00094 key = getKey()
00095 if key in moveBindings.keys():
00096 x = moveBindings[key][0]
00097 y = moveBindings[key][1]
00098 z = moveBindings[key][2]
00099 th = moveBindings[key][3]
00100 elif key in speedBindings.keys():
00101 speed = speed * speedBindings[key][0]
00102 turn = turn * speedBindings[key][1]
00103
00104 print vels(speed,turn)
00105 if (status == 14):
00106 print msg
00107 status = (status + 1) % 15
00108 else:
00109 x = 0
00110 y = 0
00111 z = 0
00112 th = 0
00113 if (key == '\x03'):
00114 break
00115
00116 twist = Twist()
00117 twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;
00118 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
00119 pub.publish(twist)
00120
00121 except:
00122 print e
00123
00124 finally:
00125 twist = Twist()
00126 twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
00127 twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
00128 pub.publish(twist)
00129
00130 termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
00131
00132