keyboard_teleop.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 ####################################################### 
00003 ##             bendahmane.amine@gmail.com            ##
00004 ##            last update : Jan 31st, 2016           ##
00005 #######################################################
00006 ##  Description: This program transforms the inputs  ## 
00007 ##      from the keyboard to velocity commands.      ##
00008 #######################################################
00009 
00010 # configuration :
00011 MAX_SPEED = 0.4
00012 MAX_ROTATIONAL_SPEED = 0.3
00013 
00014 #######################################################
00015 ##      DO NOT CHANGE ANYTHING AFTER THIS LINE!      ##
00016 #######################################################
00017 
00018 
00019 import roslib
00020 import rospy
00021 import time
00022 import tty, termios, sys
00023 from geometry_msgs.msg import Twist
00024 rospy.init_node('keyboard_teleop')
00025 roslib.load_manifest('pioneer_teleop')
00026 
00027 # define constants :
00028 KEY_UP = 65
00029 KEY_DOWN = 66
00030 KEY_RIGHT = 67
00031 KEY_LEFT = 68
00032 KEY_Q = 81
00033 KEY_Q2 = 113
00034 KEY_S = 83
00035 KEY_S2 = 115
00036 MOVE_TIME = 0.01
00037 
00038 # define variables :
00039 speed = 0.0
00040 rotationalSpeed = 0.0
00041 keyPress = 0
00042 linearDirection = 0
00043 rotationalDirection = 0
00044 linearSpeed = 0
00045 rotationalSpeed = 0
00046 
00047 # init :
00048 pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
00049 twist = Twist()
00050 
00051 while(keyPress != KEY_Q) and (keyPress != KEY_Q2):
00052 
00053         print " - Wainting for key press \n"
00054         print "      > Arrows = move robot \n"
00055         print "      > s = stop \n"
00056         print "      > q = quit \n"
00057 
00058         # get char :
00059         keyPress = " "
00060         fd = sys.stdin.fileno()
00061         old_settings = termios.tcgetattr(fd)
00062         try:
00063                 tty.setraw(sys.stdin.fileno())
00064                 ch = sys.stdin.read(1)
00065         finally:
00066                 termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
00067         keyPress = ord(ch)
00068 
00069         # test command :
00070         drive = 0
00071         if (keyPress == KEY_UP):
00072                 linearDirection = 1
00073                 print "  >> Inscrement linear speed\n"
00074         elif (keyPress == KEY_DOWN):
00075                 linearDirection = -1
00076                 print "  >> Decrement linear speed\n"
00077         elif (keyPress == KEY_LEFT):
00078                 rotationalDirection = 1
00079                 print "  >> Inscrement rotational speed\n"
00080         elif (keyPress == KEY_RIGHT):
00081                 rotationalDirection = -1
00082                 print "  >> Decrement rotational speed\n"
00083         elif (keyPress == KEY_S) or (keyPress == KEY_S2):
00084                 linearSpeed = 0.0
00085                 rotationalSpeed = 0.0
00086                 linearDirection = 0.0
00087                 rotationalDirection = 0.0
00088                 print "  >> Stop robot\n"
00089 
00090         newLinearSpeed = linearSpeed + linearDirection * 0.1
00091         newRotationalSpeed = rotationalSpeed + rotationalDirection * 0.1
00092 
00093         if (newLinearSpeed > MAX_SPEED) or (newLinearSpeed < MAX_SPEED*(-1)):
00094                 print "     Warning ! Maximum linar speed already reached\n"
00095         else:
00096                 linearSpeed = newLinearSpeed
00097 
00098         if (newRotationalSpeed > MAX_SPEED) or (newRotationalSpeed < MAX_SPEED*(-1)):
00099                 print "     Warning ! Maximum rotational speed already reached\n"
00100         else:
00101                 rotationalSpeed = newRotationalSpeed
00102 
00103         print "     Speed = (linar: "+ str(linearSpeed) +", rotational: "+ str(rotationalSpeed) +")\n"
00104         
00105         # send command :
00106         twist = Twist() 
00107         twist.linear.x = linearSpeed
00108         twist.angular.z = rotationalSpeed
00109         t1 = time.time()
00110         t2 = t1
00111         while(t2-t1 < MOVE_TIME):
00112                 pub.publish(twist)
00113                 t2 = time.time()
00114 
00115         # reinit :
00116         linearDirection = 0
00117         rotationalDirection = 0
00118 
00119 twist.linear.x = 0
00120 twist.angular.z = 0
00121 pub.publish(twist)
00122 print "  >> Quit"
00123 exit()


pioneer_teleop
Author(s): Amine BENDAHMANE
autogenerated on Thu Jun 6 2019 21:32:13