discrete_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 discrete velocity commands  ##
00008 ##  (by default, the robot moves for 1 second only)  ##
00009 #######################################################
00010 
00011 # configuration :
00012 MAX_SPEED = 0.3
00013 MAX_ROTATIONAL_SPEED = 0.3
00014 MOVE_TIME = 1
00015 
00016 #######################################################
00017 ##      DO NOT CHANGE ANYTHING AFTER THIS LINE!      ##
00018 #######################################################
00019 
00020 import roslib
00021 import rospy
00022 import time
00023 import tty, termios, sys
00024 from geometry_msgs.msg import Twist
00025 rospy.init_node('discrete_keyboard_teleop')
00026 roslib.load_manifest('pioneer_teleop')
00027 
00028 # define constants :
00029 KEY_UP = 65
00030 KEY_DOWN = 66
00031 KEY_RIGHT = 67
00032 KEY_LEFT = 68
00033 KEY_PLUS = 43
00034 KEY_MINUS = 45
00035 KEY_Q = 81
00036 KEY_Q2 = 113
00037 
00038 # define variables :
00039 speed = 0.1
00040 rotationalSpeed = 0.0
00041 keyPress = 0
00042 linearDirection = 0
00043 rotationalDirection = 0
00044 
00045 #init :
00046 pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
00047 twist = Twist()
00048 
00049 while(keyPress != KEY_Q) and (keyPress != KEY_Q2):
00050 
00051         print " - Wainting for key press \n"
00052         print "      > Arrows = move robot \n"
00053         print "      > +/- = change speed \n"
00054         print "      > q = quit \n"
00055 
00056         # get char :
00057         keyPress = " "
00058         fd = sys.stdin.fileno()
00059         old_settings = termios.tcgetattr(fd)
00060         try:
00061                 tty.setraw(sys.stdin.fileno())
00062                 ch = sys.stdin.read(1)
00063         finally:
00064                 termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
00065         keyPress = ord(ch)
00066 
00067         # test command :
00068         drive = 0
00069         if (keyPress == KEY_UP):
00070                 linearDirection = 1
00071                 drive = 1
00072                 print "  >> Move forward at speed " + str(speed) + " for " + str(MOVE_TIME) + " sec\n"
00073         elif (keyPress == KEY_DOWN):
00074                 linearDirection = -1
00075                 drive = 1
00076                 print "  >> Move backward at speed " + str(speed) + " for " + str(MOVE_TIME) + " sec\n"
00077         elif (keyPress == KEY_LEFT):
00078                 rotationalDirection = 1
00079                 drive = 1
00080                 print "  >> Turn left at speed " + str(speed) + " for " + str(MOVE_TIME) + " sec\n"
00081         elif (keyPress == KEY_RIGHT):
00082                 rotationalDirection = -1
00083                 drive = 1
00084                 print "  >> Turn Right at speed " + str(speed) + " for " + str(MOVE_TIME) + " sec\n"
00085         elif (keyPress == KEY_PLUS) and (speed < MAX_SPEED):
00086                 speed += 0.1
00087                 print "  >> Set speed to " + str(speed) + "\n"
00088         elif (keyPress == KEY_PLUS):
00089                 print "  >> Maximum speed value already reached : " + str(MAX_SPEED)    
00090         elif (keyPress == KEY_MINUS) and (speed > 0.1):
00091                 speed -= 0.1
00092                 print "  >> Set speed to " + str(speed) + "\n"
00093         elif (keyPress == KEY_MINUS):
00094                 print "  >> Minimum speed value already reached : 0.1"
00095 
00096         if (drive):
00097                 if (speed > MAX_ROTATIONAL_SPEED):
00098                         rotationalSpeed = MAX_ROTATIONAL_SPEED
00099                 else:
00100                         rotationalSpeed = speed
00101 
00102                 # send command :
00103                 twist.linear.x = linearDirection * speed
00104                 twist.angular.z = rotationalDirection * rotationalSpeed
00105                 pub.publish(twist)
00106                 t1 = time.time()
00107                 t2 = t1
00108                 while(t2-t1 < MOVE_TIME):
00109                         pub.publish(twist)
00110                         t2 = time.time()
00111 
00112                 twist.linear.x = 0
00113                 twist.angular.z = 0
00114                 pub.publish(twist)
00115 
00116                 # reinit :
00117                 rotationalSpeed = 0.0
00118                 linearDirection = 0
00119                 rotationalDirection = 0
00120 
00121 print "  >> Quit"
00122 exit()


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