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 True:
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.charging_state = {0:"Charging",
00357 2:"Charging",
00358 3:"Not Charging"}
00359 self.connection = {18:"USB Connection",
00360 20:"Rumbling",
00361 22:"Bluetooth Connection"}
00362 self.battery_state = {0:"No Charge",
00363 1:"20% Charge",
00364 2:"40% Charge",
00365 3:"60% Charge",
00366 4:"80% Charge",
00367 5:"100% Charge",
00368 238:"Charging"}
00369 self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
00370 self.last_diagnostics_time = rospy.get_rostime()
00371
00372 def publish(self, state):
00373 curr_time = rospy.get_rostime()
00374
00375 if (curr_time - self.last_diagnostics_time).to_sec() < 1.0:
00376 return
00377 self.last_diagnostics_time = curr_time
00378 diag = DiagnosticArray()
00379 diag.header.stamp = curr_time
00380
00381 stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK")
00382 try:
00383 stat.message = self.battery_state[state[1]]
00384 if state[1]<3:
00385 stat.level = DiagnosticStatus.WARN
00386 stat.message = "Please Recharge Battery (%s)."%self.battery_state[state[1]]
00387 except KeyError as ex:
00388 stat.message = "Invalid Battery State %s"%ex
00389 rospy.logwarn("Invalid Battery State %s"%ex)
00390 diag.status.append(stat)
00391
00392 stat = DiagnosticStatus(name="Connection", level=DiagnosticStatus.OK, message="OK")
00393 try:
00394 stat.message = self.connection[state[2]]
00395 except KeyError as ex:
00396 stat.message = "Invalid Connection State %s"%ex
00397 rospy.logwarn("Invalid Connection State %s"%ex)
00398 diag.status.append(stat)
00399
00400 stat = DiagnosticStatus(name="Charging State", level=DiagnosticStatus.OK, message="OK")
00401 try:
00402 stat.message = self.charging_state[state[0]]
00403 except KeyError as ex:
00404 stat.message = "Invalid Charging State %s"%ex
00405 rospy.logwarn("Invalid Charging State %s"%ex)
00406 diag.status.append(stat)
00407
00408 self.diag_pub.publish(diag)
00409
00410 class Quit(Exception):
00411 def __init__(self, errorcode):
00412 Exception.__init__(self)
00413 self.errorcode = errorcode
00414
00415 def check_hci_status():
00416
00417 proc = subprocess.Popen(['hciconfig'], stdout=subprocess.PIPE, stderr=subprocess.PIPE)
00418 (out, err) = proc.communicate()
00419 if out.find('UP') == -1:
00420 os.system("hciconfig hci0 up > /dev/null 2>&1")
00421 if out.find('PSCAN') == -1:
00422 os.system("hciconfig hci0 pscan > /dev/null 2>&1")
00423
00424 class connection_manager:
00425 def __init__(self, decoder):
00426 self.decoder = decoder
00427 self.shutdown = False
00428
00429 def prepare_bluetooth_socket(self, port):
00430 sock = BluetoothSocket(L2CAP)
00431 return self.prepare_socket(sock, port)
00432
00433 def prepare_net_socket(self, port):
00434 sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
00435 return self.prepare_socket(sock, port)
00436
00437 def prepare_socket(self, sock, port):
00438 first_loop = True
00439 while True:
00440 try:
00441 sock.bind(("", port))
00442 except Exception, e:
00443 print repr(e)
00444 if first_loop:
00445 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."
00446 first_loop = False
00447 time.sleep(0.5)
00448 continue
00449 sock.listen(1)
00450 return sock
00451
00452 def listen_net(self,intr_port, ctrl_port):
00453 intr_sock = self.prepare_net_socket(intr_port)
00454 ctrl_sock = self.prepare_net_socket(ctrl_port)
00455 self.listen(intr_sock, ctrl_sock)
00456
00457 def listen_bluetooth(self):
00458 intr_sock = self.prepare_bluetooth_socket(L2CAP_PSM_HIDP_INTR)
00459 ctrl_sock = self.prepare_bluetooth_socket(L2CAP_PSM_HIDP_CTRL)
00460 self.listen(intr_sock, ctrl_sock)
00461
00462 def listen(self, intr_sock, ctrl_sock):
00463 self.n = 0
00464 while not self.shutdown:
00465 print "Waiting for connection. Disconnect your PS3 joystick from USB and press the pairing button."
00466 try:
00467 intr_sock.settimeout(5)
00468 ctrl_sock.settimeout(1)
00469 while True:
00470 try:
00471 (intr, (idev, iport)) = intr_sock.accept();
00472 break
00473 except Exception, e:
00474 if str(e) == 'timed out':
00475 check_hci_status()
00476 else:
00477 raise
00478
00479 try:
00480 try:
00481 (ctrl, (cdev, cport)) = ctrl_sock.accept();
00482 except Exception, e:
00483 print >> sys.stderr, "Got interrupt connection without control connection. Giving up on it."
00484 continue
00485 try:
00486 if idev == cdev:
00487 self.decoder.run(intr, ctrl)
00488 print "Connection terminated."
00489 quit(0)
00490 else:
00491 print >> sys.stderr, "Simultaneous connection from two different devices. Ignoring both."
00492 finally:
00493 ctrl.close()
00494 finally:
00495 intr.close()
00496 except BadJoystickException:
00497 pass
00498 except KeyboardInterrupt:
00499 print "\nCTRL+C detected. Exiting."
00500 rospy.signal_shutdown("\nCTRL+C detected. Exiting.")
00501 quit(0)
00502 except Exception, e:
00503 traceback.print_exc()
00504 print >> sys.stderr, "Caught exception: %s"%str(e)
00505 time.sleep(1)
00506
00507 inactivity_timout_string = "--inactivity-timeout"
00508 no_disable_bluetoothd_string = "--no-disable-bluetoothd"
00509 redirect_output_string = "--redirect-output"
00510
00511
00512 def usage(errcode):
00513
00514 print "usage: ps3joy.py ["+inactivity_timout_string+"=<n>] ["+no_disable_bluetoothd_string+"] ["+redirect_output_string+"]=<f>"
00515 print "<n>: inactivity timeout in seconds (saves battery life)."
00516 print "<f>: file name to redirect output to."
00517
00518 print "Unless "+no_disable_bluetoothd_string+" is specified, bluetoothd will be stopped."
00519 raise Quit(errcode)
00520
00521 def is_arg_with_param(arg, prefix):
00522 if not arg.startswith(prefix):
00523 return False
00524 if not arg.startswith(prefix+"="):
00525 print "Expected '=' after "+prefix
00526 print
00527 usage(1)
00528 return True
00529
00530 if __name__ == "__main__":
00531 errorcode = 0
00532 try:
00533 inactivity_timeout = float(1e3000)
00534 disable_bluetoothd = True
00535 deamon = False
00536 for arg in sys.argv[1:]:
00537 if arg == "--help":
00538 usage(0)
00539 elif is_arg_with_param(arg, inactivity_timout_string):
00540 str_value = arg[len(inactivity_timout_string)+1:]
00541 try:
00542 inactivity_timeout = float(str_value)
00543 if inactivity_timeout < 0:
00544 print "Inactivity timeout must be positive."
00545 print
00546 usage(1)
00547 except ValueError:
00548 print "Error parsing inactivity timeout: "+str_value
00549 print
00550 usage(1)
00551 elif arg == no_disable_bluetoothd_string:
00552 disable_bluetoothd = False
00553 elif is_arg_with_param(arg, redirect_output_string):
00554 str_value = arg[len(redirect_output_string)+1:]
00555 try:
00556 print "Redirecting output to:", str_value
00557 sys.stdout = open(str_value, "a", 1)
00558 except IOError, e:
00559 print "Error opening file to redirect output:", str_value
00560 raise Quit(1)
00561 sys.stderr = sys.stdout
00562
00563
00564 else:
00565 print "Ignoring parameter: '%s'"%arg
00566 if os.getuid() != 0:
00567 print >> sys.stderr, "ps3joy.py must be run as root."
00568 quit(1)
00569 if disable_bluetoothd:
00570 os.system("/etc/init.d/bluetooth stop > /dev/null 2>&1")
00571 time.sleep(1)
00572 try:
00573 while os.system("hciconfig hci0 > /dev/null 2>&1") != 0:
00574 print >> sys.stderr, "No bluetooth dongle found or bluez rosdep not installed. Will retry in 5 seconds."
00575 time.sleep(5)
00576 if inactivity_timeout == float(1e3000):
00577 print "No inactivity timeout was set. (Run with --help for details.)"
00578 else:
00579 print "Inactivity timeout set to %.0f seconds."%inactivity_timeout
00580 cm = connection_manager(decoder(deamon, inactivity_timeout = inactivity_timeout))
00581 cm.listen_bluetooth()
00582 finally:
00583 if disable_bluetoothd:
00584 os.system("/etc/init.d/bluetooth start > /dev/null 2>&1")
00585 except Quit, e:
00586 errorcode = e.errorcode
00587 except KeyboardInterrupt:
00588 print "\nCTRL+C detected. Exiting."
00589 rospy.signal_shutdown("\nCTRL+C detected. Exiting.")
00590 exit(errorcode)
00591