Go to the documentation of this file.00001
00002
00003 import rospy
00004 import roslib
00005
00006 try:
00007 from sensor_msgs.msg import Joy
00008 except:
00009 import roslib; roslib.load_manifest("jsk_teleop_joy")
00010 from sensor_msgs.msg import Joy
00011
00012 class JoyStatus():
00013 def __init__(self):
00014
00015 self.center = False
00016 self.select = False
00017 self.start = False
00018 self.up = False
00019 self.down = False
00020 self.left = False
00021 self.right = False
00022 self.circle = False
00023 self.cross = False
00024 self.triangle = False
00025 self.square = False
00026 self.L1 = False
00027 self.R1 = False
00028 self.L2 = False
00029 self.R2 = False
00030 self.L3 = False
00031 self.R3 = False
00032
00033 self.left_analog_x = 0.0
00034 self.left_analog_y = 0.0
00035 self.right_analog_x = 0.0
00036 self.right_analog_y = 0.0
00037
00038 self.left_analog_up = False
00039 self.left_analog_down = False
00040 self.left_analog_left = False
00041 self.left_analog_right = False
00042
00043 def checkAnalogStick(self):
00044 self.analog_threshold = 0.8
00045 if self.left_analog_x > self.analog_threshold:
00046 self.left_analog_left = True
00047
00048 if self.left_analog_x < -self.analog_threshold:
00049 self.left_analog_right = True
00050
00051 if self.left_analog_y > self.analog_threshold:
00052 self.left_analog_up = True
00053
00054 if self.left_analog_y < -self.analog_threshold:
00055 self.left_analog_down = True
00056
00057
00058 def toPS3Msg(self):
00059 joy = Joy()
00060 joy.header = self.orig_msg.header
00061 joy.buttons = [0] * 17
00062 joy.axes = [0] * 20
00063 if self.center:
00064 joy.buttons[16] = 1
00065 if self.select:
00066 joy.buttons[0] = 1
00067 if self.start:
00068 joy.buttons[3] = 1
00069 if self.L3:
00070 joy.buttons[1] = 1
00071 if self.R3:
00072 joy.buttons[2] = 1
00073 if self.square:
00074 joy.axes[15] = -1.0
00075 joy.buttons[15] = 1
00076 if self.up:
00077 joy.axes[4] = -1.0
00078 joy.buttons[4] = 1
00079 if self.down:
00080 joy.axes[6] = -1.0
00081 joy.buttons[6] = 1
00082 if self.left:
00083 joy.axes[7] = -1.0
00084 joy.buttons[7] = 1
00085 if self.right:
00086 joy.axes[5] = -1.0
00087 joy.buttons[5] = 1
00088 if self.triangle:
00089 joy.axes[12] = -1.0
00090 joy.buttons[12] = 1
00091 if self.cross:
00092 joy.axes[14] = -1.0
00093 joy.buttons[14] = 1
00094 if self.circle:
00095 joy.axes[13] = -1.0
00096 joy.buttons[13] = 1
00097 if self.L1:
00098 joy.axes[10] = -1.0
00099 joy.buttons[10] = 1
00100 if self.R1:
00101 joy.axes[11] = -1.0
00102 joy.buttons[11] = 1
00103 if self.L2:
00104 joy.axes[8] = -1.0
00105 joy.buttons[8] = 1
00106 if self.R2:
00107 joy.axes[9] = -1.0
00108 joy.buttons[9] = 1
00109 joy.axes[0] = self.left_analog_x
00110 joy.axes[1] = self.left_analog_y
00111 joy.axes[2] = self.right_analog_x
00112 joy.axes[3] = self.right_analog_y
00113 return joy
00114
00115
00116 class XBoxStatus(JoyStatus):
00117 def __init__(self, msg):
00118 '''
00119 Xbox game pad
00120 Y button => triangle
00121 X button => square
00122 B button => circle
00123 A button => cross
00124 RB => R1
00125 LB => L1
00126 RT => R2
00127 LT => L2
00128 '''
00129 JoyStatus.__init__(self)
00130 if msg.buttons[8] == 1:
00131 self.center = True
00132 else:
00133 self.center = False
00134 if msg.buttons[6] == 1:
00135 self.select = True
00136 else:
00137 self.select = False
00138 if msg.buttons[7] == 1:
00139 self.start = True
00140 else:
00141 self.start = False
00142 if msg.buttons[9] == 1:
00143 self.L3 = True
00144 else:
00145 self.L3 = False
00146 if msg.buttons[10] == 1:
00147 self.R3 = True
00148 else:
00149 self.R3 = False
00150 if msg.buttons[2] == 1:
00151 self.square = True
00152 else:
00153 self.square = False
00154 if msg.buttons[1] == 1:
00155 self.circle = True
00156 else:
00157 self.circle = False
00158 if msg.axes[7] > 0.1:
00159 self.up = True
00160 else:
00161 self.up = False
00162 if msg.axes[7] < -0.1:
00163 self.down = True
00164 else:
00165 self.down = False
00166 if msg.axes[6] > 0.1:
00167 self.left = True
00168 else:
00169 self.left = False
00170 if msg.axes[6] < -0.1:
00171 self.right = True
00172 else:
00173 self.right = False
00174 if msg.buttons[3] == 1:
00175 self.triangle = True
00176 else:
00177 self.triangle = False
00178 if msg.buttons[0] == 1:
00179 self.cross = True
00180 else:
00181 self.cross = False
00182 if msg.buttons[4] == 1:
00183 self.L1 = True
00184 else:
00185 self.L1 = False
00186 if msg.buttons[5] == 1:
00187 self.R1 = True
00188 else:
00189 self.R1 = False
00190 if msg.axes[2] < -0.5:
00191 self.L2 = True
00192 else:
00193 self.L2 = False
00194 if msg.axes[5] < -0.5:
00195 self.R2 = True
00196 else:
00197 self.R2 = False
00198 self.left_analog_x = msg.axes[0]
00199 self.left_analog_y = msg.axes[1]
00200 self.right_analog_x = msg.axes[3]
00201 self.right_analog_y = msg.axes[4]
00202 self.checkAnalogStick()
00203 self.orig_msg = msg
00204
00205 class PS3Status(JoyStatus):
00206 def __init__(self, msg):
00207 JoyStatus.__init__(self)
00208
00209 if msg.buttons[16] == 1:
00210 self.center = True
00211 else:
00212 self.center = False
00213 if msg.buttons[0] == 1:
00214 self.select = True
00215 else:
00216 self.select = False
00217 if msg.buttons[3] == 1:
00218 self.start = True
00219 else:
00220 self.start = False
00221 if msg.buttons[1] == 1:
00222 self.L3 = True
00223 else:
00224 self.L3 = False
00225 if msg.buttons[2] == 1:
00226 self.R3 = True
00227 else:
00228 self.R3 = False
00229 if msg.axes[15] < 0:
00230 self.square = True
00231 else:
00232 self.square = False
00233 if msg.axes[4] < 0:
00234 self.up = True
00235 else:
00236 self.up = False
00237 if msg.axes[6] < 0:
00238 self.down = True
00239 else:
00240 self.down = False
00241 if msg.axes[7] < 0:
00242 self.left = True
00243 else:
00244 self.left = False
00245 if msg.axes[5] < 0:
00246 self.right = True
00247 else:
00248 self.right = False
00249 if msg.axes[12] < 0:
00250 self.triangle = True
00251 else:
00252 self.triangle = False
00253 if msg.axes[14] < 0:
00254 self.cross = True
00255 else:
00256 self.cross = False
00257 if msg.axes[13] < 0:
00258 self.circle = True
00259 else:
00260 self.circle = False
00261 if msg.axes[10] < 0:
00262 self.L1 = True
00263 else:
00264 self.L1 = False
00265 if msg.axes[11] < 0:
00266 self.R1 = True
00267 else:
00268 self.R1 = False
00269 if msg.axes[8] < 0:
00270 self.L2 = True
00271 else:
00272 self.L2 = False
00273 if msg.axes[9] < 0:
00274 self.R2 = True
00275 else:
00276 self.R2 = False
00277 self.left_analog_x = msg.axes[0]
00278 self.left_analog_y = msg.axes[1]
00279 self.right_analog_x = msg.axes[2]
00280 self.right_analog_y = msg.axes[3]
00281 self.checkAnalogStick()
00282 self.orig_msg = msg
00283
00284 class PS3WiredStatus(JoyStatus):
00285 def __init__(self, msg):
00286 JoyStatus.__init__(self)
00287
00288 if msg.buttons[16] == 1:
00289 self.center = True
00290 else:
00291 self.center = False
00292 if msg.buttons[0] == 1:
00293 self.select = True
00294 else:
00295 self.select = False
00296 if msg.buttons[3] == 1:
00297 self.start = True
00298 else:
00299 self.start = False
00300 if msg.buttons[1] == 1:
00301 self.L3 = True
00302 else:
00303 self.L3 = False
00304 if msg.buttons[2] == 1:
00305 self.R3 = True
00306 else:
00307 self.R3 = False
00308 if msg.buttons[15] == 1:
00309 self.square = True
00310 else:
00311 self.square = False
00312 if msg.buttons[4] == 1:
00313 self.up = True
00314 else:
00315 self.up = False
00316 if msg.buttons[6] == 1:
00317 self.down = True
00318 else:
00319 self.down = False
00320 if msg.buttons[7] == 1:
00321 self.left = True
00322 else:
00323 self.left = False
00324 if msg.buttons[5] == 1:
00325 self.right = True
00326 else:
00327 self.right = False
00328 if msg.buttons[12] == 1:
00329 self.triangle = True
00330 else:
00331 self.triangle = False
00332 if msg.buttons[14] == 1:
00333 self.cross = True
00334 else:
00335 self.cross = False
00336 if msg.buttons[13] == 1:
00337 self.circle = True
00338 else:
00339 self.circle = False
00340 if msg.buttons[10] == 1:
00341 self.L1 = True
00342 else:
00343 self.L1 = False
00344 if msg.buttons[11] == 1:
00345 self.R1 = True
00346 else:
00347 self.R1 = False
00348 if msg.buttons[8] == 1:
00349 self.L2 = True
00350 else:
00351 self.L2 = False
00352 if msg.buttons[9] == 1:
00353 self.R2 = True
00354 else:
00355 self.R2 = False
00356 self.left_analog_x = msg.axes[0]
00357 self.left_analog_y = msg.axes[1]
00358 self.right_analog_x = msg.axes[2]
00359 self.right_analog_y = msg.axes[3]
00360 self.checkAnalogStick()
00361 self.orig_msg = msg