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