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
00030
00031
00032
00033
00034
00035 import roslib; roslib.load_manifest('ps3joy')
00036 import rospy
00037 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
00038
00039 from bluetooth import *
00040 import select
00041 import fcntl
00042 import os
00043 import time
00044 import sys
00045 import traceback
00046 import subprocess
00047 from array import array
00048 import sensor_msgs.msg
00049 import rosgraph.masterapi
00050
00051 L2CAP_PSM_HIDP_CTRL = 17
00052 L2CAP_PSM_HIDP_INTR = 19
00053
00054 class uinput:
00055 EV_KEY = 1
00056 EV_REL = 2
00057 EV_ABS = 3
00058 BUS_USB = 3
00059 ABS_MAX = 0x3f
00060
00061
00062 class uinputjoy:
00063 def open_uinput(self):
00064 for name in ["/dev/input/uinput", "/dev/misc/uinput", "/dev/uinput"]:
00065 try:
00066 return os.open(name, os.O_WRONLY)
00067 break
00068 except Exception, e:
00069
00070 pass
00071 return None
00072
00073 def __init__(self, buttons, axes, axmin, axmax, axfuzz, axflat):
00074 self.file = self.open_uinput()
00075 if self.file == None:
00076 print >> sys.stderr, "Trying to modprobe uinput."
00077 os.system("modprobe uinput > /dev/null 2>&1")
00078 time.sleep(1)
00079 self.file = self.open_uinput()
00080 if self.file == None:
00081 print >> sys.stderr, "Can't open uinput device. Is it accessible by this user? Did you mean to run as root?"
00082 raise IOError
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092 UI_SET_EVBIT = 0x40045564
00093 UI_SET_KEYBIT = 0x40045565
00094 UI_SET_RELBIT = 0x40045566
00095 UI_DEV_CREATE = 0x5501
00096 UI_SET_RELBIT = 0x40045566
00097 UI_SET_ABSBIT = 0x40045567
00098 uinput_user_dev = "80sHHHHi" + (uinput.ABS_MAX+1)*4*'i'
00099
00100 if len(axes) != len(axmin) or len(axes) != len(axmax):
00101 raise Exception("uinputjoy.__init__: axes, axmin and axmax should have same length")
00102 absmin = [0] * (uinput.ABS_MAX+1)
00103 absmax = [0] * (uinput.ABS_MAX+1)
00104 absfuzz = [2] * (uinput.ABS_MAX+1)
00105 absflat = [4] * (uinput.ABS_MAX+1)
00106 for i in range(0, len(axes)):
00107 absmin[axes[i]] = axmin[i]
00108 absmax[axes[i]] = axmax[i]
00109 absfuzz[axes[i]] = axfuzz[i]
00110 absflat[axes[i]] = axflat[i]
00111
00112 os.write(self.file, struct.pack(uinput_user_dev, "Sony Playstation SixAxis/DS3",
00113 uinput.BUS_USB, 0x054C, 0x0268, 0, 0, *(absmax + absmin + absfuzz + absflat)))
00114
00115 fcntl.ioctl(self.file, UI_SET_EVBIT, uinput.EV_KEY)
00116
00117 for b in buttons:
00118 fcntl.ioctl(self.file, UI_SET_KEYBIT, b)
00119
00120 for a in axes:
00121 fcntl.ioctl(self.file, UI_SET_EVBIT, uinput.EV_ABS)
00122 fcntl.ioctl(self.file, UI_SET_ABSBIT, a)
00123
00124 fcntl.ioctl(self.file, UI_DEV_CREATE)
00125
00126 self.value = [None] * (len(buttons) + len(axes))
00127 self.type = [uinput.EV_KEY] * len(buttons) + [uinput.EV_ABS] * len(axes)
00128 self.code = buttons + axes
00129
00130 def update(self, value):
00131 input_event = "LLHHi"
00132 t = time.time()
00133 th = int(t)
00134 tl = int((t - th) * 1000000)
00135 if len(value) != len(self.value):
00136 print >> sys.stderr, "Unexpected length for value in update (%i instead of %i). This is a bug."%(len(value), len(self.value))
00137 for i in range(0, len(value)):
00138 if value[i] != self.value[i]:
00139 os.write(self.file, struct.pack(input_event, th, tl, self.type[i], self.code[i], value[i]))
00140 self.value = list(value)
00141
00142 class BadJoystickException(Exception):
00143 def __init__(self):
00144 Exception.__init__(self, "Unsupported joystick.")
00145
00146 class decoder:
00147 def __init__(self, deamon, inactivity_timeout = float(1e3000)):
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159 buttons = range(0x100,0x111)
00160 axes = range(0, 20)
00161 axmin = [0] * 20
00162 axmax = [255] * 20
00163 axfuzz = [2] * 20
00164 axflat = [4] * 20
00165 for i in range(-4,0):
00166 axmax[i] = 1023
00167 axfuzz[i] = 4
00168 axflat[i] = 4
00169 for i in range(4,len(axmin)-4):
00170 axmin[i] = -axmax[i]
00171 self.joy = uinputjoy(buttons, axes, axmin, axmax, axfuzz, axflat)
00172 self.axmid = [sum(pair)/2 for pair in zip(axmin, axmax)]
00173 self.fullstop()
00174 self.outlen = len(buttons) + len(axes)
00175 self.inactivity_timeout = inactivity_timeout
00176 self.deamon = deamon
00177 self.init_ros()
00178 step_active = 1
00179 step_idle = 2
00180 step_error = 3
00181
00182 def init_ros(self):
00183 try:
00184 rospy.init_node('ps3joy', anonymous=True, disable_signals=True)
00185 except:
00186 print "rosnode init failed"
00187 rospy.Subscriber("joy/set_feedback",sensor_msgs.msg.JoyFeedbackArray,self.set_feedback)
00188 self.diagnostics = Diagnostics()
00189 self.led_values = [1,0,0,0]
00190 self.rumble_cmd = [0, 255]
00191 self.led_cmd = 2
00192 self.core_down = False
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228 def step(self, rawdata):
00229 if len(rawdata) == 50:
00230 joy_coding = "!1B2x3B1x4B4x12B3x1B1B1B9x4H"
00231 all_data = list(struct.unpack(joy_coding, rawdata))
00232 state_data = all_data[20:23]
00233 data = all_data[0:20]+all_data[23:]
00234 prefix = data.pop(0)
00235 self.diagnostics.publish(state_data)
00236 if prefix != 161:
00237 print >> sys.stderr, "Unexpected prefix (%i). Is this a PS3 Dual Shock or Six Axis?"%prefix
00238 return self.step_error
00239 out = []
00240 for j in range(0,2):
00241 curbyte = data.pop(0)
00242 for k in range(0,8):
00243 out.append(int((curbyte & (1 << k)) != 0))
00244 out = out + data
00245 self.joy.update(out)
00246 axis_motion = [abs(out[17:][i] - self.axmid[i]) > 20 for i in range(0,len(out)-17-4)]
00247
00248 if any(out[0:17]) or any(axis_motion):
00249 return self.step_active
00250 return self.step_idle
00251 elif len(rawdata) == 13:
00252
00253 print >> sys.stderr, "Your bluetooth adapter is not supported. Does it support Bluetooth 2.0? Please report its model to blaise@willowgarage.com"
00254 raise BadJoystickException()
00255 else:
00256 print >> sys.stderr, "Unexpected packet length (%i). Is this a PS3 Dual Shock or Six Axis?"%len(rawdata)
00257 return self.step_error
00258
00259 def fullstop(self):
00260 self.joy.update([0] * 17 + self.axmid)
00261
00262
00263 def set_feedback(self,msg):
00264 for feedback in msg.array:
00265 if feedback.type == sensor_msgs.msg.JoyFeedback.TYPE_LED and feedback.id < 4:
00266 self.led_values[feedback.id] = int(round(feedback.intensity))
00267 elif feedback.type == sensor_msgs.msg.JoyFeedback.TYPE_RUMBLE and feedback.id < 2:
00268 self.rumble_cmd[feedback.id] = int(feedback.intensity*255)
00269 else:
00270 rospy.logwarn("Feedback %s of type %s does not exist for this joystick.",feedback.id, feedback.type)
00271 self.led_cmd = self.led_values[0]*pow(2,1) + self.led_values[1]*pow(2,2) + self.led_values[2]*pow(2,3) + self.led_values[3]*pow(2,4)
00272 self.new_msg = True
00273
00274 def send_cmd(self, ctrl):
00275 command = [0x52,
00276 0x01,
00277 0x00, 0xfe, self.rumble_cmd[1], 0xfe, self.rumble_cmd[0],
00278 0x00, 0x00, 0x00, 0x00, self.led_cmd,
00279 0xff, 0x27, 0x10, 0x00, 0x32,
00280 0xff, 0x27, 0x10, 0x00, 0x32,
00281 0xff, 0x27, 0x10, 0x00, 0x32,
00282 0xff, 0x27, 0x10, 0x00, 0x32,
00283 0x00, 0x00, 0x00, 0x00, 0x00
00284 ]
00285 ctrl.send(array('B', command).tostring())
00286 self.new_msg = False
00287
00288 def run(self, intr, ctrl):
00289 activated = False
00290 try:
00291 self.fullstop()
00292 lastactivitytime = lastvalidtime = time.time()
00293 while not rospy.is_shutdown():
00294 (rd, wr, err) = select.select([intr], [], [], 0.1)
00295 curtime = time.time()
00296 if len(rd) + len(wr) + len(err) == 0:
00297
00298 ctrl.send("\x53\xf4\x42\x03\x00\x00")
00299 else:
00300
00301 if not activated:
00302 self.send_cmd(ctrl)
00303 time.sleep(0.5)
00304 self.rumble_cmd[1] = 0
00305 self.send_cmd(ctrl)
00306 print "Connection activated"
00307 activated = True
00308 try:
00309 if(self.new_msg):
00310 self.send_cmd(ctrl)
00311 rawdata = intr.recv(128)
00312 except BluetoothError, s:
00313 print "Got Bluetooth error %s. Disconnecting."%s
00314 return
00315 if len(rawdata) == 0:
00316 print "Joystick shut down the connection, battery may be discharged."
00317 return
00318 if not rosgraph.masterapi.is_online():
00319 print "The roscore or node shutdown, ps3joy shutting down."
00320 return
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337 stepout = self.step(rawdata)
00338 if stepout != self.step_error:
00339 lastvalidtime = curtime
00340 if stepout == self.step_active:
00341 lastactivitytime = curtime
00342 if curtime - lastactivitytime > self.inactivity_timeout:
00343 print "Joystick inactive for %.0f seconds. Disconnecting to save battery."%self.inactivity_timeout
00344 return
00345 if curtime - lastvalidtime >= 0.1:
00346 self.fullstop()
00347 if curtime - lastvalidtime >= 5:
00348 print "No valid data for 5 seconds. Disconnecting. This should not happen, please report it."
00349 return
00350 time.sleep(0.005)
00351 finally:
00352 self.fullstop()
00353
00354 class Diagnostics():
00355 def __init__(self):
00356 self.STATE_TEXTS_CHARGING = {
00357 0:"Charging",
00358 1:"Not Charging"}
00359 self.STATE_TEXTS_CONNECTION = {
00360 18:"USB Connection",
00361 20:"Rumbling",
00362 22:"Bluetooth Connection"}
00363 self.STATE_TEXTS_BATTERY = {
00364 0:"No Charge",
00365 1:"20% Charge",
00366 2:"40% Charge",
00367 3:"60% Charge",
00368 4:"80% Charge",
00369 5:"100% Charge",
00370 238:"Charging"}
00371 self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
00372 self.last_diagnostics_time = rospy.get_rostime()
00373
00374 def publish(self, state):
00375 STATE_INDEX_CHARGING = 0
00376 STATE_INDEX_BATTERY = 1
00377 STATE_INDEX_CONNECTION = 2
00378
00379
00380 curr_time = rospy.get_rostime()
00381 if (curr_time - self.last_diagnostics_time).to_sec() < 1.0:
00382 return
00383 self.last_diagnostics_time = curr_time
00384
00385
00386 diag = DiagnosticArray()
00387 diag.header.stamp = curr_time
00388
00389 stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK")
00390 try:
00391 battery_state_code = state[STATE_INDEX_BATTERY]
00392 stat.message = self.STATE_TEXTS_BATTERY[battery_state_code]
00393 if battery_state_code < 3:
00394 stat.level = DiagnosticStatus.WARN
00395 if battery_state_code < 1:
00396 stat.level = DiagnosticStatus.ERROR
00397 stat.message = "Please Recharge Battery (%s)." % self.STATE_TEXTS_BATTERY[battery_state_code]
00398 except KeyError as ex:
00399 stat.message = "Invalid Battery State %s"%ex
00400 rospy.logwarn("Invalid Battery State %s"%ex)
00401 stat.level = DiagnosticStatus.ERROR
00402 diag.status.append(stat)
00403
00404 stat = DiagnosticStatus(name='ps3joy'": Connection Type", level=DiagnosticStatus.OK, message="OK")
00405 try:
00406 stat.message = self.STATE_TEXTS_CONNECTION[state[STATE_INDEX_CONNECTION]]
00407 except KeyError as ex:
00408 stat.message = "Invalid Connection State %s"%ex
00409 rospy.logwarn("Invalid Connection State %s"%ex)
00410 stat.level = DiagnosticStatus.ERROR
00411 diag.status.append(stat)
00412
00413 stat = DiagnosticStatus(name='ps3joy'": Charging State", level=DiagnosticStatus.OK, message="OK")
00414 try:
00415 stat.message = self.STATE_TEXTS_CHARGING[state[STATE_INDEX_CHARGING]]
00416 except KeyError as ex:
00417 stat.message = "Invalid Charging State %s"%ex
00418 rospy.logwarn("Invalid Charging State %s"%ex)
00419 stat.level = DiagnosticStatus.ERROR
00420 diag.status.append(stat)
00421
00422 self.diag_pub.publish(diag)
00423
00424 class Quit(Exception):
00425 def __init__(self, errorcode):
00426 Exception.__init__(self)
00427 self.errorcode = errorcode
00428
00429 def check_hci_status():
00430
00431 proc = subprocess.Popen(['hciconfig'], stdout=subprocess.PIPE, stderr=subprocess.PIPE)
00432 (out, err) = proc.communicate()
00433 if out.find('UP') == -1:
00434 os.system("hciconfig hci0 up > /dev/null 2>&1")
00435 if out.find('PSCAN') == -1:
00436 os.system("hciconfig hci0 pscan > /dev/null 2>&1")
00437
00438 class connection_manager:
00439 def __init__(self, decoder):
00440 self.decoder = decoder
00441
00442 def prepare_bluetooth_socket(self, port):
00443 sock = BluetoothSocket(L2CAP)
00444 return self.prepare_socket(sock, port)
00445
00446 def prepare_net_socket(self, port):
00447 sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
00448 return self.prepare_socket(sock, port)
00449
00450 def prepare_socket(self, sock, port):
00451 first_loop = True
00452 while True:
00453 try:
00454 sock.bind(("", port))
00455 except Exception, e:
00456 print repr(e)
00457 if first_loop:
00458 print >> sys.stderr, "Error binding to socket, will retry every 5 seconds. Do you have another ps3joy.py running? This error occurs on some distributions (such as Ubuntu Karmic). Please read http://www.ros.org/wiki/ps3joy/Troubleshooting for solutions."
00459 first_loop = False
00460 time.sleep(0.5)
00461 continue
00462 sock.listen(1)
00463 return sock
00464
00465 def listen_net(self,intr_port, ctrl_port):
00466 intr_sock = self.prepare_net_socket(intr_port)
00467 ctrl_sock = self.prepare_net_socket(ctrl_port)
00468 self.listen(intr_sock, ctrl_sock)
00469
00470 def listen_bluetooth(self):
00471 intr_sock = self.prepare_bluetooth_socket(L2CAP_PSM_HIDP_INTR)
00472 ctrl_sock = self.prepare_bluetooth_socket(L2CAP_PSM_HIDP_CTRL)
00473 self.listen(intr_sock, ctrl_sock)
00474
00475 def listen(self, intr_sock, ctrl_sock):
00476 self.n = 0
00477 while not rospy.is_shutdown():
00478 print "Waiting for connection. Disconnect your PS3 joystick from USB and press the pairing button."
00479 try:
00480 intr_sock.settimeout(5)
00481 ctrl_sock.settimeout(1)
00482 while True:
00483 try:
00484 (intr, (idev, iport)) = intr_sock.accept();
00485 break
00486 except Exception, e:
00487 if str(e) == 'timed out':
00488 check_hci_status()
00489 else:
00490 raise
00491
00492 try:
00493 try:
00494 (ctrl, (cdev, cport)) = ctrl_sock.accept();
00495 except Exception, e:
00496 print >> sys.stderr, "Got interrupt connection without control connection. Giving up on it."
00497 continue
00498 try:
00499 if idev == cdev:
00500 self.decoder.run(intr, ctrl)
00501 print "Connection terminated."
00502 quit(0)
00503 else:
00504 print >> sys.stderr, "Simultaneous connection from two different devices. Ignoring both."
00505 finally:
00506 ctrl.close()
00507 finally:
00508 intr.close()
00509 except BadJoystickException:
00510 pass
00511 except KeyboardInterrupt:
00512 print "\nCTRL+C detected. Exiting."
00513 rospy.signal_shutdown("\nCTRL+C detected. Exiting.")
00514 quit(0)
00515 except Exception, e:
00516 traceback.print_exc()
00517 print >> sys.stderr, "Caught exception: %s"%str(e)
00518 time.sleep(1)
00519
00520 inactivity_timout_string = "--inactivity-timeout"
00521 no_disable_bluetoothd_string = "--no-disable-bluetoothd"
00522 redirect_output_string = "--redirect-output"
00523
00524
00525 def usage(errcode):
00526
00527 print "usage: ps3joy.py ["+inactivity_timout_string+"=<n>] ["+no_disable_bluetoothd_string+"] ["+redirect_output_string+"]=<f>"
00528 print "<n>: inactivity timeout in seconds (saves battery life)."
00529 print "<f>: file name to redirect output to."
00530
00531 print "Unless "+no_disable_bluetoothd_string+" is specified, bluetoothd will be stopped."
00532 raise Quit(errcode)
00533
00534 def is_arg_with_param(arg, prefix):
00535 if not arg.startswith(prefix):
00536 return False
00537 if not arg.startswith(prefix+"="):
00538 print "Expected '=' after "+prefix
00539 print
00540 usage(1)
00541 return True
00542
00543 if __name__ == "__main__":
00544 errorcode = 0
00545 try:
00546 inactivity_timeout = float(1e3000)
00547 disable_bluetoothd = True
00548 deamon = False
00549 for arg in sys.argv[1:]:
00550 if arg == "--help":
00551 usage(0)
00552 elif is_arg_with_param(arg, inactivity_timout_string):
00553 str_value = arg[len(inactivity_timout_string)+1:]
00554 try:
00555 inactivity_timeout = float(str_value)
00556 if inactivity_timeout < 0:
00557 print "Inactivity timeout must be positive."
00558 print
00559 usage(1)
00560 except ValueError:
00561 print "Error parsing inactivity timeout: "+str_value
00562 print
00563 usage(1)
00564 elif arg == no_disable_bluetoothd_string:
00565 disable_bluetoothd = False
00566 elif is_arg_with_param(arg, redirect_output_string):
00567 str_value = arg[len(redirect_output_string)+1:]
00568 try:
00569 print "Redirecting output to:", str_value
00570 sys.stdout = open(str_value, "a", 1)
00571 except IOError, e:
00572 print "Error opening file to redirect output:", str_value
00573 raise Quit(1)
00574 sys.stderr = sys.stdout
00575
00576
00577 else:
00578 print "Ignoring parameter: '%s'"%arg
00579
00580
00581 if os.getuid() != 0:
00582 print >> sys.stderr, "ps3joy.py must be run as root."
00583 quit(1)
00584 if disable_bluetoothd:
00585 os.system("/etc/init.d/bluetooth stop > /dev/null 2>&1")
00586 time.sleep(1)
00587 try:
00588 while os.system("hciconfig hci0 > /dev/null 2>&1") != 0:
00589 print >> sys.stderr, "No bluetooth dongle found or bluez rosdep not installed. Will retry in 5 seconds."
00590 time.sleep(5)
00591 if inactivity_timeout == float(1e3000):
00592 print "No inactivity timeout was set. (Run with --help for details.)"
00593 else:
00594 print "Inactivity timeout set to %.0f seconds."%inactivity_timeout
00595 cm = connection_manager(decoder(deamon, inactivity_timeout = inactivity_timeout))
00596 cm.listen_bluetooth()
00597 finally:
00598 if disable_bluetoothd:
00599 os.system("/etc/init.d/bluetooth start > /dev/null 2>&1")
00600 except Quit, e:
00601 errorcode = e.errorcode
00602 except KeyboardInterrupt:
00603 print "\nCTRL+C detected. Exiting."
00604 rospy.signal_shutdown("\nCTRL+C detected. Exiting.")
00605 exit(errorcode)
00606