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 IpegaStatus(JoyStatus):
00117 def __init__(self, msg):
00118 '''
00119 ipaga game pad
00120 No home button
00121 Y button => triangle
00122 X button => square
00123 B button => circle
00124 A button => cross
00125 '''
00126 JoyStatus.__init__(self)
00127 self.center = False
00128 if msg.buttons[10] == 1:
00129 self.select = True
00130 else:
00131 self.select = False
00132 if msg.buttons[11] == 1:
00133 self.start = True
00134 else:
00135 self.start = False
00136 if msg.buttons[13] == 1:
00137 self.L3 = True
00138 else:
00139 self.L3 = False
00140 if msg.buttons[14] == 1:
00141 self.R3 = True
00142 else:
00143 self.R3 = False
00144 if msg.buttons[3] == 1:
00145 self.square = True
00146 else:
00147 self.square = False
00148 if msg.buttons[1] == 1:
00149 self.circle = True
00150 else:
00151 self.circle = False
00152 if msg.axes[7] > 0.1:
00153 self.up = True
00154 else:
00155 self.up = False
00156 if msg.axes[7] < -0.1:
00157 self.down = True
00158 else:
00159 self.down = False
00160 if msg.axes[6] > 0.1:
00161 self.left = True
00162 else:
00163 self.left = False
00164 if msg.axes[6] < -0.1:
00165 self.right = True
00166 else:
00167 self.right = False
00168 if msg.buttons[4] == 1:
00169 self.triangle = True
00170 else:
00171 self.triangle = False
00172 if msg.buttons[0] == 1:
00173 self.cross = True
00174 else:
00175 self.cross = False
00176 if msg.buttons[6] == 1:
00177 self.L1 = True
00178 else:
00179 self.L1 = False
00180 if msg.buttons[7] == 1:
00181 self.R1 = True
00182 else:
00183 self.R1 = False
00184 if msg.axes[5] < -0.5:
00185 self.L2 = True
00186 else:
00187 self.L2 = False
00188 if msg.axes[4] < -0.5:
00189 self.R2 = True
00190 else:
00191 self.R2 = False
00192 self.left_analog_x = msg.axes[0]
00193 self.left_analog_y = msg.axes[1]
00194 self.right_analog_x = msg.axes[2]
00195 self.right_analog_y = msg.axes[3]
00196 self.checkAnalogStick()
00197 self.orig_msg = msg
00198
00199 class XBoxStatus(JoyStatus):
00200 def __init__(self, msg):
00201 '''
00202 Xbox game pad
00203 Y button => triangle
00204 X button => square
00205 B button => circle
00206 A button => cross
00207 RB => R1
00208 LB => L1
00209 RT => R2
00210 LT => L2
00211 '''
00212 JoyStatus.__init__(self)
00213 if msg.buttons[8] == 1:
00214 self.center = True
00215 else:
00216 self.center = False
00217 if msg.buttons[6] == 1:
00218 self.select = True
00219 else:
00220 self.select = False
00221 if msg.buttons[7] == 1:
00222 self.start = True
00223 else:
00224 self.start = False
00225 if msg.buttons[9] == 1:
00226 self.L3 = True
00227 else:
00228 self.L3 = False
00229 if msg.buttons[10] == 1:
00230 self.R3 = True
00231 else:
00232 self.R3 = False
00233 if msg.buttons[2] == 1:
00234 self.square = True
00235 else:
00236 self.square = False
00237 if msg.buttons[1] == 1:
00238 self.circle = True
00239 else:
00240 self.circle = False
00241 if msg.axes[7] > 0.1:
00242 self.up = True
00243 else:
00244 self.up = False
00245 if msg.axes[7] < -0.1:
00246 self.down = True
00247 else:
00248 self.down = False
00249 if msg.axes[6] > 0.1:
00250 self.left = True
00251 else:
00252 self.left = False
00253 if msg.axes[6] < -0.1:
00254 self.right = True
00255 else:
00256 self.right = False
00257 if msg.buttons[3] == 1:
00258 self.triangle = True
00259 else:
00260 self.triangle = False
00261 if msg.buttons[0] == 1:
00262 self.cross = True
00263 else:
00264 self.cross = False
00265 if msg.buttons[4] == 1:
00266 self.L1 = True
00267 else:
00268 self.L1 = False
00269 if msg.buttons[5] == 1:
00270 self.R1 = True
00271 else:
00272 self.R1 = False
00273 if msg.axes[2] < -0.5:
00274 self.L2 = True
00275 else:
00276 self.L2 = False
00277 if msg.axes[5] < -0.5:
00278 self.R2 = True
00279 else:
00280 self.R2 = False
00281 self.left_analog_x = msg.axes[0]
00282 self.left_analog_y = msg.axes[1]
00283 self.right_analog_x = msg.axes[3]
00284 self.right_analog_y = msg.axes[4]
00285 self.checkAnalogStick()
00286 self.orig_msg = msg
00287
00288 class PS3Status(JoyStatus):
00289 def __init__(self, msg):
00290 JoyStatus.__init__(self)
00291
00292 if msg.buttons[16] == 1:
00293 self.center = True
00294 else:
00295 self.center = False
00296 if msg.buttons[0] == 1:
00297 self.select = True
00298 else:
00299 self.select = False
00300 if msg.buttons[3] == 1:
00301 self.start = True
00302 else:
00303 self.start = False
00304 if msg.buttons[1] == 1:
00305 self.L3 = True
00306 else:
00307 self.L3 = False
00308 if msg.buttons[2] == 1:
00309 self.R3 = True
00310 else:
00311 self.R3 = False
00312 if msg.axes[15] < 0:
00313 self.square = True
00314 else:
00315 self.square = False
00316 if msg.axes[4] < 0:
00317 self.up = True
00318 else:
00319 self.up = False
00320 if msg.axes[6] < 0:
00321 self.down = True
00322 else:
00323 self.down = False
00324 if msg.axes[7] < 0:
00325 self.left = True
00326 else:
00327 self.left = False
00328 if msg.axes[5] < 0:
00329 self.right = True
00330 else:
00331 self.right = False
00332 if msg.axes[12] < 0:
00333 self.triangle = True
00334 else:
00335 self.triangle = False
00336 if msg.axes[14] < 0:
00337 self.cross = True
00338 else:
00339 self.cross = False
00340 if msg.axes[13] < 0:
00341 self.circle = True
00342 else:
00343 self.circle = False
00344 if msg.axes[10] < 0:
00345 self.L1 = True
00346 else:
00347 self.L1 = False
00348 if msg.axes[11] < 0:
00349 self.R1 = True
00350 else:
00351 self.R1 = False
00352 if msg.axes[8] < 0:
00353 self.L2 = True
00354 else:
00355 self.L2 = False
00356 if msg.axes[9] < 0:
00357 self.R2 = True
00358 else:
00359 self.R2 = False
00360 self.left_analog_x = msg.axes[0]
00361 self.left_analog_y = msg.axes[1]
00362 self.right_analog_x = msg.axes[2]
00363 self.right_analog_y = msg.axes[3]
00364 self.checkAnalogStick()
00365 self.orig_msg = msg
00366
00367 class PS3WiredStatus(JoyStatus):
00368 def __init__(self, msg):
00369 JoyStatus.__init__(self)
00370
00371 if msg.buttons[16] == 1:
00372 self.center = True
00373 else:
00374 self.center = False
00375 if msg.buttons[0] == 1:
00376 self.select = True
00377 else:
00378 self.select = False
00379 if msg.buttons[3] == 1:
00380 self.start = True
00381 else:
00382 self.start = False
00383 if msg.buttons[1] == 1:
00384 self.L3 = True
00385 else:
00386 self.L3 = False
00387 if msg.buttons[2] == 1:
00388 self.R3 = True
00389 else:
00390 self.R3 = False
00391 if msg.buttons[15] == 1:
00392 self.square = True
00393 else:
00394 self.square = False
00395 if msg.buttons[4] == 1:
00396 self.up = True
00397 else:
00398 self.up = False
00399 if msg.buttons[6] == 1:
00400 self.down = True
00401 else:
00402 self.down = False
00403 if msg.buttons[7] == 1:
00404 self.left = True
00405 else:
00406 self.left = False
00407 if msg.buttons[5] == 1:
00408 self.right = True
00409 else:
00410 self.right = False
00411 if msg.buttons[12] == 1:
00412 self.triangle = True
00413 else:
00414 self.triangle = False
00415 if msg.buttons[14] == 1:
00416 self.cross = True
00417 else:
00418 self.cross = False
00419 if msg.buttons[13] == 1:
00420 self.circle = True
00421 else:
00422 self.circle = False
00423 if msg.buttons[10] == 1:
00424 self.L1 = True
00425 else:
00426 self.L1 = False
00427 if msg.buttons[11] == 1:
00428 self.R1 = True
00429 else:
00430 self.R1 = False
00431 if msg.buttons[8] == 1:
00432 self.L2 = True
00433 else:
00434 self.L2 = False
00435 if msg.buttons[9] == 1:
00436 self.R2 = True
00437 else:
00438 self.R2 = False
00439 self.left_analog_x = msg.axes[0]
00440 self.left_analog_y = msg.axes[1]
00441 self.right_analog_x = msg.axes[2]
00442 self.right_analog_y = msg.axes[3]
00443 self.checkAnalogStick()
00444 self.orig_msg = msg