Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 import math
00030 import numpy as np
00031 import pygame
00032 import pygame.joystick
00033 import time
00034
00035 import roslib; roslib.load_manifest('tele_mobile')
00036 import rospy
00037
00038 from pygame.locals import *
00039 from tele_mobile.msg import direction
00040
00041
00042 class JoystickCommand:
00043 def __init__(self, topic='tmobile', name='joystick'):
00044 self.pub = rospy.Publisher('tmobile', direction)
00045 try:
00046 rospy.init_node(name, anonymous=True)
00047 except rospy.ROSException, e:
00048 pass
00049
00050
00051 def set_platform(self, x, y,reset, zen, xvel, yvel, avel,zen_reset, lock):
00052 cmd = direction(x, y, reset, zen, xvel, yvel, avel,zen_reset, lock)
00053 self.pub.publish(cmd)
00054 print "publishing:", cmd
00055 time.sleep(0.05)
00056
00057
00058 if __name__ == '__main__':
00059
00060
00061 pygame.init()
00062
00063
00064 joystick_count = pygame.joystick.get_count()
00065
00066 print "Joysticks found: %d\n" % joystick_count
00067
00068 try:
00069 js = pygame.joystick.Joystick(0)
00070 js.init()
00071 except pygame.error:
00072 print "joystick error"
00073 js = None
00074
00075 js_status = js.get_init()
00076 print js_status
00077
00078 screen = pygame.display.set_mode((320, 80))
00079 pygame.display.set_caption('Snozzjoy')
00080
00081 background = pygame.Surface(screen.get_size())
00082 background = background.convert()
00083 background.fill((250, 250, 250))
00084
00085 s = JoystickCommand()
00086
00087 max_xvel = 0.18
00088 max_yvel = 0.15
00089 max_speed = 0.18
00090 max_avel = 0.18
00091
00092 x = 0.
00093 y = 0.
00094 reset = 0.
00095 lock = 0.
00096 zen = 0.
00097 seg_x = 0.
00098 seg_y = 0.
00099 seg_a = 0.
00100 xvel = 0.
00101 yvel = 0.
00102 avel = 0.
00103 zen_reset = 0.
00104 connected = False
00105
00106 while not rospy.is_shutdown():
00107 for event in pygame.event.get():
00108
00109 if (event.type == JOYHATMOTION):
00110 x = event.value[0]
00111 y = event.value[1]
00112 if (event.type == JOYBUTTONDOWN):
00113 if(event.button == 0):
00114 reset = 1
00115
00116
00117 if(event.button == 9):
00118 zen_reset = 1
00119 if(event.button == 4 or event.button == 5 ):
00120 zen = 1
00121 if(event.button == 2 or event.button == 3 ):
00122 zen = -1
00123 if(event.button == 2 or event.button == 3 ):
00124 zen = -1
00125 if (event.type == JOYBUTTONUP):
00126 if(event.button == 0):
00127 reset = 0
00128
00129
00130 if(event.button == 9):
00131 zen_reset = 0
00132 if(event.button == 2 or event.button == 3 or event.button == 4 or event.button == 5):
00133 zen = 0
00134 if (event.type == JOYAXISMOTION):
00135 if event.axis == 0:
00136 seg_y = -event.value
00137 if event.axis == 1:
00138 seg_x = -event.value
00139 if event.axis == 2:
00140 seg_a = -event.value
00141 if event.axis == 3:
00142 lock = (1 - event.value) / 2
00143 if seg_x == 0 and seg_y == 0 and seg_a ==0:
00144 connected = True
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154 if connected:
00155 xvel = seg_x * max_xvel * lock
00156 yvel = seg_y * max_yvel * lock
00157 avel = seg_a * max_avel * lock
00158 s.set_platform(x, y, reset, zen, xvel, yvel, avel, zen_reset, lock)
00159
00160 s.set_platform(0, 0, 0, 0, 0, 0, 0, 0, 0)