ps3joy.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #***********************************************************
00003 #* Software License Agreement (BSD License)
00004 #*
00005 #*  Copyright (c) 2009, Willow Garage, Inc.
00006 #*  All rights reserved.
00007 #*
00008 #*  Redistribution and use in source and binary forms, with or without
00009 #*  modification, are permitted provided that the following conditions
00010 #*  are met:
00011 #*
00012 #*   * Redistributions of source code must retain the above copyright
00013 #*     notice, this list of conditions and the following disclaimer.
00014 #*   * Redistributions in binary form must reproduce the above
00015 #*     copyright notice, this list of conditions and the following
00016 #*     disclaimer in the documentation and/or other materials provided
00017 #*     with the distribution.
00018 #*   * Neither the name of the Willow Garage nor the names of its
00019 #*     contributors may be used to endorse or promote products derived
00020 #*     from this software without specific prior written permission.
00021 #*
00022 #*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 #*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 #*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 #*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 #*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 #*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 #*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 #*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 #*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 #*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 #*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 #*  POSSIBILITY OF SUCH DAMAGE.
00034 #***********************************************************
00035 
00036 from bluetooth import *
00037 import select
00038 import fcntl
00039 import os
00040 import time
00041 import sys                    
00042 import traceback
00043 import subprocess
00044 
00045 L2CAP_PSM_HIDP_CTRL = 17
00046 L2CAP_PSM_HIDP_INTR = 19
00047 
00048 class uinput:
00049     EV_KEY = 1
00050     EV_REL = 2
00051     EV_ABS = 3
00052     BUS_USB = 3
00053     ABS_MAX = 0x3f
00054 
00055 class uinputjoy:
00056     def open_uinput(self):
00057         for name in ["/dev/input/uinput", "/dev/misc/uinput", "/dev/uinput"]:
00058             try:
00059                 return os.open(name, os.O_WRONLY)
00060                 break
00061             except Exception, e:
00062                 #print >> sys.stderr, "Error opening uinput: %s"%str(e)
00063                 pass
00064         return None
00065 
00066     def __init__(self, buttons, axes, axmin, axmax, axfuzz, axflat):
00067         self.file = self.open_uinput()
00068         if self.file == None:
00069             print >> sys.stderr, "Trying to modprobe uinput."
00070             os.system("modprobe uinput > /dev/null 2>&1")
00071             time.sleep(1) # uinput isn't ready to go right away.
00072             self.file = self.open_uinput()
00073             if self.file == None:
00074                 print >> sys.stderr, "Can't open uinput device. Is it accessible by this user? Did you mean to run as root?"
00075                 raise IOError
00076         #id = uinput.input_id()
00077         #id.bustype = uinput.BUS_USB
00078         #id.vendor = 0x054C
00079         #id.product = 0x0268
00080         #id.version = 0
00081         #info = uinput.uinput_user_dev()
00082         #info.name = "Sony Playstation SixAxis/DS3"
00083         #info.id = id
00084         
00085         UI_SET_EVBIT   = 0x40045564
00086         UI_SET_KEYBIT  = 0x40045565
00087         UI_SET_RELBIT  = 0x40045566
00088         UI_DEV_CREATE  = 0x5501
00089         UI_SET_RELBIT  = 0x40045566
00090         UI_SET_ABSBIT  = 0x40045567
00091         uinput_user_dev = "80sHHHHi" + (uinput.ABS_MAX+1)*4*'i'
00092 
00093         if len(axes) != len(axmin) or len(axes) != len(axmax):
00094             raise Exception("uinputjoy.__init__: axes, axmin and axmax should have same length")
00095         absmin = [0] * (uinput.ABS_MAX+1)
00096         absmax = [0] * (uinput.ABS_MAX+1)
00097         absfuzz = [2] * (uinput.ABS_MAX+1)
00098         absflat = [4] * (uinput.ABS_MAX+1)
00099         for i in range(0, len(axes)):
00100             absmin[axes[i]] = axmin[i]
00101             absmax[axes[i]] = axmax[i]
00102             absfuzz[axes[i]] = axfuzz[i]
00103             absflat[axes[i]] = axflat[i]
00104 
00105         os.write(self.file, struct.pack(uinput_user_dev, "Sony Playstation SixAxis/DS3",
00106             uinput.BUS_USB, 0x054C, 0x0268, 0, 0, *(absmax + absmin + absfuzz + absflat)))
00107 
00108         fcntl.ioctl(self.file, UI_SET_EVBIT, uinput.EV_KEY)
00109         
00110         for b in buttons:
00111             fcntl.ioctl(self.file, UI_SET_KEYBIT, b)
00112         
00113         for a in axes:
00114             fcntl.ioctl(self.file, UI_SET_EVBIT, uinput.EV_ABS)
00115             fcntl.ioctl(self.file, UI_SET_ABSBIT, a)
00116         
00117         fcntl.ioctl(self.file, UI_DEV_CREATE)
00118 
00119         self.value = [None] * (len(buttons) + len(axes))
00120         self.type = [uinput.EV_KEY] * len(buttons) + [uinput.EV_ABS] * len(axes)
00121         self.code = buttons + axes
00122     
00123     def update(self, value):
00124         input_event = "LLHHi"
00125         t = time.time()
00126         th = int(t)
00127         tl = int((t - th) * 1000000)
00128         if len(value) != len(self.value):
00129             print >> sys.stderr, "Unexpected length for value in update (%i instead of %i). This is a bug."%(len(value), len(self.value))
00130         for i in range(0, len(value)):
00131             if value[i] != self.value[i]:
00132                 os.write(self.file, struct.pack(input_event, th, tl, self.type[i], self.code[i], value[i]))
00133         self.value = list(value)
00134 
00135 class BadJoystickException(Exception):
00136     def __init__(self):
00137         Exception.__init__(self, "Unsupported joystick.")
00138 
00139 class decoder:
00140     def __init__(self, inactivity_timeout = float(1e3000), continuous_motion_output = False):
00141         #buttons=[uinput.BTN_SELECT, uinput.BTN_THUMBL, uinput.BTN_THUMBR, uinput.BTN_START, 
00142         #         uinput.BTN_FORWARD, uinput.BTN_RIGHT, uinput.BTN_BACK, uinput.BTN_LEFT, 
00143         #         uinput.BTN_TL, uinput.BTN_TR, uinput.BTN_TL2, uinput.BTN_TR2,
00144         #         uinput.BTN_X, uinput.BTN_A, uinput.BTN_B, uinput.BTN_Y,
00145         #         uinput.BTN_MODE]
00146         #axes=[uinput.ABS_X, uinput.ABS_Y, uinput.ABS_Z, uinput.ABS_RX,
00147         #         uinput.ABS_RX, uinput.ABS_RY, uinput.ABS_PRESSURE, uinput.ABS_DISTANCE,
00148         #         uinput.ABS_THROTTLE, uinput.ABS_RUDDER, uinput.ABS_WHEEL, uinput.ABS_GAS,
00149         #         uinput.ABS_HAT0Y, uinput.ABS_HAT1Y, uinput.ABS_HAT2Y, uinput.ABS_HAT3Y,
00150         #         uinput.ABS_TILT_X, uinput.ABS_TILT_Y, uinput.ABS_MISC, uinput.ABS_RZ,
00151         #         ]
00152         buttons = range(0x100,0x111)
00153         axes = range(0, 20)
00154         axmin = [0] * 20
00155         axmax = [255] * 20
00156         axfuzz = [2] * 20
00157         axflat = [4] * 20
00158         for i in range(-4,0): # Gyros have more bits than other axes
00159             axmax[i] = 1023
00160             axfuzz[i] = 4
00161             axflat[i] = 4
00162             if continuous_motion_output:
00163               axfuzz[i] = 0
00164               axflat[i] = 0
00165         for i in range(4,len(axmin)-4): # Buttons should be zero when not pressed
00166             axmin[i] = -axmax[i]
00167         self.joy = uinputjoy(buttons, axes, axmin, axmax, axfuzz, axflat)
00168         self.axmid = [sum(pair)/2 for pair in zip(axmin, axmax)]
00169         self.fullstop() # Probably useless because of uinput startup bug
00170         self.outlen = len(buttons) + len(axes)           
00171         self.inactivity_timeout = inactivity_timeout
00172 
00173     step_active = 1
00174     step_idle = 2
00175     step_error = 3
00176 
00177     def step(self, rawdata): # Returns true if the packet was legal
00178         if len(rawdata) == 50:
00179             joy_coding = "!1B2x3B1x4B4x12B15x4H"
00180             data = list(struct.unpack(joy_coding, rawdata))
00181             prefix = data.pop(0)
00182             if prefix != 161:
00183                 print >> sys.stderr, "Unexpected prefix (%i). Is this a PS3 Dual Shock or Six Axis?"%prefix
00184                 return self.step_error
00185             out = []
00186             for j in range(0,2): # Split out the buttons.
00187                 curbyte = data.pop(0)
00188                 for k in range(0,8):
00189                     out.append(int((curbyte & (1 << k)) != 0))
00190             out = out + data
00191             self.joy.update(out)
00192             axis_motion = [abs(out[17:][i] - self.axmid[i]) > 20 for i in range(0,len(out)-17-4)]  
00193                                                                        # 17 buttons, 4 inertial sensors
00194             if any(out[0:17]) or any(axis_motion):
00195                 return self.step_active
00196             return self.step_idle
00197         elif len(rawdata) == 13:
00198             #print list(rawdata)
00199             print >> sys.stderr, "Your bluetooth adapter is not supported. Does it support Bluetooth 2.0? Please report its model to blaise@willowgarage.com"
00200             raise BadJoystickException()
00201         else:
00202             print >> sys.stderr, "Unexpected packet length (%i). Is this a PS3 Dual Shock or Six Axis?"%len(rawdata)
00203             return self.step_error
00204 
00205     def fullstop(self):
00206         self.joy.update([0] * 17 + self.axmid)
00207 
00208     def run(self, intr, ctrl):
00209         activated = False
00210         try:
00211             self.fullstop()
00212             lastactivitytime = lastvalidtime = time.time()
00213             while True:
00214                 (rd, wr, err) = select.select([intr], [], [], 0.1)
00215                 curtime = time.time()
00216                 if len(rd) + len(wr) + len(err) == 0: # Timeout
00217                     #print "Activating connection."
00218                     ctrl.send("\x53\xf4\x42\x03\x00\x00") # Try activating the stream.
00219                 else: # Got a frame.
00220                     #print "Got a frame at ", curtime, 1 / (curtime - lastvalidtime)
00221                     if not activated:
00222                         print "Connection activated"
00223                         activated = True
00224                     try:
00225                         rawdata = intr.recv(128)
00226                     except BluetoothError, s:
00227                         print "Got Bluetooth error %s. Disconnecting."%s
00228                         return
00229                     if len(rawdata) == 0: # Orderly shutdown of socket
00230                         print "Joystick shut down the connection, battery may be discharged."
00231                         return
00232                     stepout = self.step(rawdata)
00233                     if stepout != self.step_error:
00234                         lastvalidtime = curtime
00235                     if stepout == self.step_active:
00236                         lastactivitytime = curtime
00237                 if curtime - lastactivitytime > self.inactivity_timeout:
00238                     print "Joystick inactive for %.0f seconds. Disconnecting to save battery."%self.inactivity_timeout
00239                     return
00240                 if curtime - lastvalidtime >= 0.1: # Zero all outputs if we don't hear a valid frame for 0.1 to 0.2 seconds
00241                     self.fullstop()
00242                 if curtime - lastvalidtime >= 5: # Disconnect if we don't hear a valid frame for 5 seconds
00243                     print "No valid data for 5 seconds. Disconnecting. This should not happen, please report it."
00244                     return
00245                 time.sleep(0.005) # No need to blaze through the loop when there is an error
00246         finally:
00247             self.fullstop()
00248 
00249 class Quit(Exception):
00250     def __init__(self, errorcode):
00251         Exception.__init__(self)
00252         self.errorcode = errorcode
00253 
00254 def check_hci_status():
00255     # Check if hci0 is up and pscanning, take action as necessary.
00256     proc = subprocess.Popen(['hciconfig'], stdout=subprocess.PIPE, stderr=subprocess.PIPE)
00257     (out, err) = proc.communicate()
00258     if out.find('UP') == -1:
00259         os.system("hciconfig hci0 up > /dev/null 2>&1")
00260     if out.find('PSCAN') == -1:
00261         os.system("hciconfig hci0 pscan > /dev/null 2>&1")   
00262 
00263 class connection_manager:
00264     def __init__(self, decoder):
00265         self.decoder = decoder
00266         self.shutdown = False
00267 
00268     def prepare_bluetooth_socket(self, port):
00269         sock = BluetoothSocket(L2CAP)
00270         return self.prepare_socket(sock, port)
00271 
00272     def prepare_net_socket(self, port):
00273         sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
00274         return self.prepare_socket(sock, port)
00275 
00276     def prepare_socket(self, sock, port):
00277         first_loop = True
00278         while True:
00279             try:
00280                 sock.bind(("", port))
00281             except Exception, e:
00282                 print repr(e)
00283                 if first_loop:
00284                     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."
00285                 first_loop = False
00286                 time.sleep(0.5)
00287                 continue 
00288             sock.listen(1)
00289             return sock
00290 
00291     def listen_net(self,intr_port, ctrl_port):
00292         intr_sock = self.prepare_net_socket(intr_port)
00293         ctrl_sock = self.prepare_net_socket(ctrl_port)
00294         self.listen(intr_sock, ctrl_sock)
00295 
00296     def listen_bluetooth(self):
00297         intr_sock = self.prepare_bluetooth_socket(L2CAP_PSM_HIDP_INTR)
00298         ctrl_sock = self.prepare_bluetooth_socket(L2CAP_PSM_HIDP_CTRL)
00299         self.listen(intr_sock, ctrl_sock)
00300     
00301     def listen(self, intr_sock, ctrl_sock):
00302         self.n = 0
00303         while not self.shutdown:
00304             print "Waiting for connection. Disconnect your PS3 joystick from USB and press the pairing button."
00305             try:
00306                 intr_sock.settimeout(5)
00307                 ctrl_sock.settimeout(1)
00308                 while True:
00309                     try:
00310                         (intr, (idev, iport)) = intr_sock.accept();
00311                         break
00312                     except Exception, e:
00313                         if str(e) == 'timed out':
00314                             check_hci_status()
00315                         else:
00316                             raise
00317                     
00318                 try:
00319                     try:
00320                         (ctrl, (cdev, cport)) = ctrl_sock.accept();
00321                     except Exception, e:
00322                         print >> sys.stderr, "Got interrupt connection without control connection. Giving up on it."
00323                         continue
00324                     try:
00325                         if idev == cdev:
00326                             self.decoder.run(intr, ctrl)
00327                             print "Connection terminated."
00328                         else:
00329                             print >> sys.stderr, "Simultaneous connection from two different devices. Ignoring both."
00330                     finally:
00331                         ctrl.close()
00332                 finally:
00333                     intr.close()        
00334             except BadJoystickException:
00335                 pass
00336             except KeyboardInterrupt:
00337                 print "CTRL+C detected. Exiting."
00338                 quit(0)
00339             except Exception, e:
00340                 traceback.print_exc()
00341                 print >> sys.stderr, "Caught exception: %s"%str(e)
00342                 time.sleep(1)
00343             print
00344                     
00345 inactivity_timout_string = "--inactivity-timeout"
00346 no_disable_bluetoothd_string = "--no-disable-bluetoothd"
00347 redirect_output_string = "--redirect-output"
00348 continuous_motion_output_string = "--continuous-output"
00349                     
00350 def usage(errcode):
00351     print "usage: ps3joy.py ["+inactivity_timout_string+"=<n>] ["+no_disable_bluetoothd_string+"] ["+redirect_output_string+"] ["+continuous_motion_output_string+"]=<f>"
00352     print "<n>: inactivity timeout in seconds (saves battery life)."
00353     print "<f>: file name to redirect output to."
00354     print "Unless "+no_disable_bluetoothd_string+" is specified, bluetoothd will be stopped."
00355     raise Quit(errcode)
00356 
00357 def is_arg_with_param(arg, prefix):
00358     if not arg.startswith(prefix):
00359         return False
00360     if not arg.startswith(prefix+"="):
00361         print "Expected '=' after "+prefix
00362         print
00363         usage(1)
00364     return True
00365 
00366 if __name__ == "__main__":
00367     errorcode = 0
00368     try:
00369         inactivity_timeout = float(1e3000)
00370         disable_bluetoothd = True
00371         continuous_output = False
00372         for arg in sys.argv[1:]: # Be very tolerant in case we are roslaunched.
00373             if arg == "--help":
00374                 usage(0)
00375             elif is_arg_with_param(arg, inactivity_timout_string):
00376                 str_value = arg[len(inactivity_timout_string)+1:]
00377                 try:
00378                     inactivity_timeout = float(str_value)
00379                     if inactivity_timeout < 0:
00380                         print "Inactivity timeout must be positive."
00381                         print
00382                         usage(1)
00383                 except ValueError:
00384                     print "Error parsing inactivity timeout: "+str_value
00385                     print
00386                     usage(1)
00387             elif arg == no_disable_bluetoothd_string:
00388                 disable_bluetoothd = False
00389             elif arg == continuous_motion_output_string:
00390                 continuous_output = True
00391             elif is_arg_with_param(arg, redirect_output_string):
00392                 str_value = arg[len(redirect_output_string)+1:]
00393                 try:
00394                     print "Redirecting output to:", str_value
00395                     sys.stdout = open(str_value, "a", 1)        
00396                 except IOError, e:
00397                     print "Error opening file to redirect output:", str_value
00398                     raise Quit(1)
00399                 sys.stderr = sys.stdout
00400             else:
00401                 print "Ignoring parameter: '%s'"%arg
00402         if os.getuid() != 0:
00403             print >> sys.stderr, "ps3joy.py must be run as root."
00404             quit(1)
00405         if disable_bluetoothd:
00406             os.system("/etc/init.d/bluetooth stop > /dev/null 2>&1")
00407             time.sleep(1) # Give the socket time to be available.
00408         try:
00409             while os.system("hciconfig hci0 > /dev/null 2>&1") != 0:
00410                 print >> sys.stderr,  "No bluetooth dongle found or bluez rosdep not installed. Will retry in 5 seconds."
00411                 time.sleep(5)
00412             if inactivity_timeout == float(1e3000):
00413                 print "No inactivity timeout was set. (Run with --help for details.)"
00414             else:
00415                 print "Inactivity timeout set to %.0f seconds."%inactivity_timeout
00416             cm = connection_manager(decoder(inactivity_timeout = inactivity_timeout, continuous_motion_output = continuous_output))
00417             cm.listen_bluetooth()
00418         finally:
00419             if disable_bluetoothd:
00420                 os.system("/etc/init.d/bluetooth start > /dev/null 2>&1")
00421     except Quit, e:
00422         errorcode = e.errorcode
00423     except KeyboardInterrupt:
00424         print "CTRL+C detected. Exiting."
00425     exit(errorcode)


ps3joy
Author(s): Blaise Gassend, pascal@pabr.org, and Melonee Wise
autogenerated on Sat Dec 28 2013 17:06:56