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