teleop_twist_keyboard.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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 


teleop_twist_keyboard
Author(s): Graylin Trevor Jay
autogenerated on Sun Oct 5 2014 22:40:46