ps3joy.py

Go to the documentation of this file.

#!/usr/bin/python
#***********************************************************
#* Software License Agreement (BSD License)
#*
#*  Copyright (c) 2009, Willow Garage, Inc.
#*  All rights reserved.
#*
#*  Redistribution and use in source and binary forms, with or without
#*  modification, are permitted provided that the following conditions
#*  are met:
#*
#*   * Redistributions of source code must retain the above copyright
#*     notice, this list of conditions and the following disclaimer.
#*   * Redistributions in binary form must reproduce the above
#*     copyright notice, this list of conditions and the following
#*     disclaimer in the documentation and/or other materials provided
#*     with the distribution.
#*   * Neither the name of the Willow Garage nor the names of its
#*     contributors may be used to endorse or promote products derived
#*     from this software without specific prior written permission.
#*
#*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
#*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
#*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
#*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
#*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
#*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
#*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
#*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
#*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
#*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
#*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
#*  POSSIBILITY OF SUCH DAMAGE.
#***********************************************************

from bluetooth import *
import select
import fcntl
import os
import time
import sys                    
import traceback
import subprocess

L2CAP_PSM_HIDP_CTRL = 17
L2CAP_PSM_HIDP_INTR = 19

class uinput:
    EV_KEY = 1
    EV_REL = 2
    EV_ABS = 3
    BUS_USB = 3
    ABS_MAX = 0x3f

class uinputjoy:
    def open_uinput(self):
        for name in ["/dev/input/uinput", "/dev/misc/uinput", "/dev/uinput"]:
            try:
                return os.open(name, os.O_WRONLY)
                break
            except Exception, e:
                #print >> sys.stderr, "Error opening uinput: %s"%str(e)
                pass
        return None

    def __init__(self, buttons, axes, axmin, axmax, axfuzz, axflat):
        self.file = self.open_uinput()
        if self.file == None:
            print >> sys.stderr, "Trying to modprobe uinput."
            os.system("modprobe uinput > /dev/null 2>&1")
            time.sleep(1) # uinput isn't ready to go right away.
            self.file = self.open_uinput()
            if self.file == None:
                print >> sys.stderr, "Can't open uinput device. Is it accessible by this user? Did you mean to run as root?"
                raise IOError
        #id = uinput.input_id()
        #id.bustype = uinput.BUS_USB
        #id.vendor = 0x054C
        #id.product = 0x0268
        #id.version = 0
        #info = uinput.uinput_user_dev()
        #info.name = "Sony Playstation SixAxis/DS3"
        #info.id = id
        
        UI_SET_EVBIT   = 0x40045564
        UI_SET_KEYBIT  = 0x40045565
        UI_SET_RELBIT  = 0x40045566
        UI_DEV_CREATE  = 0x5501
        UI_SET_RELBIT  = 0x40045566
        UI_SET_ABSBIT  = 0x40045567
        uinput_user_dev = "80sHHHHi" + (uinput.ABS_MAX+1)*4*'i'

        if len(axes) != len(axmin) or len(axes) != len(axmax):
            raise Exception("uinputjoy.__init__: axes, axmin and axmax should have same length")
        absmin = [0] * (uinput.ABS_MAX+1)
        absmax = [0] * (uinput.ABS_MAX+1)
        absfuzz = [2] * (uinput.ABS_MAX+1)
        absflat = [4] * (uinput.ABS_MAX+1)
        for i in range(0, len(axes)):
            absmin[axes[i]] = axmin[i]
            absmax[axes[i]] = axmax[i]
            absfuzz[axes[i]] = axfuzz[i]
            absflat[axes[i]] = axflat[i]

        os.write(self.file, struct.pack(uinput_user_dev, "Sony Playstation SixAxis/DS3",
            uinput.BUS_USB, 0x054C, 0x0268, 0, 0, *(absmax + absmin + absfuzz + absflat)))

        fcntl.ioctl(self.file, UI_SET_EVBIT, uinput.EV_KEY)
        
        for b in buttons:
            fcntl.ioctl(self.file, UI_SET_KEYBIT, b)
        
        for a in axes:
            fcntl.ioctl(self.file, UI_SET_EVBIT, uinput.EV_ABS)
            fcntl.ioctl(self.file, UI_SET_ABSBIT, a)
        
        fcntl.ioctl(self.file, UI_DEV_CREATE)

        self.value = [None] * (len(buttons) + len(axes))
        self.type = [uinput.EV_KEY] * len(buttons) + [uinput.EV_ABS] * len(axes)
        self.code = buttons + axes
    
    def update(self, value):
        input_event = "LLHHi"
        t = time.time()
        th = int(t)
        tl = int((t - th) * 1000000)
        if len(value) != len(self.value):
            print >> sys.stderr, "Unexpected length for value in update (%i instead of %i). This is a bug."%(len(value), len(self.value))
        for i in range(0, len(value)):
            if value[i] != self.value[i]:
                os.write(self.file, struct.pack(input_event, th, tl, self.type[i], self.code[i], value[i]))
        self.value = list(value)

class BadJoystickException(Exception):
    def __init__(self):
        Exception.__init__(self, "Unsupported joystick.")

class decoder:
    def __init__(self, inactivity_timeout = float(1e3000), continuous_motion_output = False):
        #buttons=[uinput.BTN_SELECT, uinput.BTN_THUMBL, uinput.BTN_THUMBR, uinput.BTN_START, 
        #         uinput.BTN_FORWARD, uinput.BTN_RIGHT, uinput.BTN_BACK, uinput.BTN_LEFT, 
        #         uinput.BTN_TL, uinput.BTN_TR, uinput.BTN_TL2, uinput.BTN_TR2,
        #         uinput.BTN_X, uinput.BTN_A, uinput.BTN_B, uinput.BTN_Y,
        #         uinput.BTN_MODE]
        #axes=[uinput.ABS_X, uinput.ABS_Y, uinput.ABS_Z, uinput.ABS_RX,
        #         uinput.ABS_RX, uinput.ABS_RY, uinput.ABS_PRESSURE, uinput.ABS_DISTANCE,
        #         uinput.ABS_THROTTLE, uinput.ABS_RUDDER, uinput.ABS_WHEEL, uinput.ABS_GAS,
        #         uinput.ABS_HAT0Y, uinput.ABS_HAT1Y, uinput.ABS_HAT2Y, uinput.ABS_HAT3Y,
        #         uinput.ABS_TILT_X, uinput.ABS_TILT_Y, uinput.ABS_MISC, uinput.ABS_RZ,
        #         ]
        buttons = range(0x100,0x111)
        axes = range(0, 20)
        axmin = [0] * 20
        axmax = [255] * 20
        axfuzz = [2] * 20
        axflat = [4] * 20
        for i in range(-4,0): # Gyros have more bits than other axes
            axmax[i] = 1023
            axfuzz[i] = 4
            axflat[i] = 4
            if continuous_motion_output:
              axfuzz[i] = 0
              axflat[i] = 0
        for i in range(4,len(axmin)-4): # Buttons should be zero when not pressed
            axmin[i] = -axmax[i]
        self.joy = uinputjoy(buttons, axes, axmin, axmax, axfuzz, axflat)
        self.axmid = [sum(pair)/2 for pair in zip(axmin, axmax)]
        self.fullstop() # Probably useless because of uinput startup bug
        self.outlen = len(buttons) + len(axes)           
        self.inactivity_timeout = inactivity_timeout

    step_active = 1
    step_idle = 2
    step_error = 3

    def step(self, rawdata): # Returns true if the packet was legal
        if len(rawdata) == 50:
            joy_coding = "!1B2x3B1x4B4x12B15x4H"
            data = list(struct.unpack(joy_coding, rawdata))
            prefix = data.pop(0)
            if prefix != 161:
                print >> sys.stderr, "Unexpected prefix (%i). Is this a PS3 Dual Shock or Six Axis?"%prefix
                return self.step_error
            out = []
            for j in range(0,2): # Split out the buttons.
                curbyte = data.pop(0)
                for k in range(0,8):
                    out.append(int((curbyte & (1 << k)) != 0))
            out = out + data
            self.joy.update(out)
            axis_motion = [abs(out[17:][i] - self.axmid[i]) > 20 for i in range(0,len(out)-17-4)]  
                                                                       # 17 buttons, 4 inertial sensors
            if any(out[0:17]) or any(axis_motion):
                return self.step_active
            return self.step_idle
        elif len(rawdata) == 13:
            #print list(rawdata)
            print >> sys.stderr, "Your bluetooth adapter is not supported. Does it support Bluetooth 2.0? Please report its model to blaise@willowgarage.com"
            raise BadJoystickException()
        else:
            print >> sys.stderr, "Unexpected packet length (%i). Is this a PS3 Dual Shock or Six Axis?"%len(rawdata)
            return self.step_error

    def fullstop(self):
        self.joy.update([0] * 17 + self.axmid)

    def run(self, intr, ctrl):
        activated = False
        try:
            self.fullstop()
            lastactivitytime = lastvalidtime = time.time()
            while True:
                (rd, wr, err) = select.select([intr], [], [], 0.1)
                curtime = time.time()
                if len(rd) + len(wr) + len(err) == 0: # Timeout
                    #print "Activating connection."
                    ctrl.send("\x53\xf4\x42\x03\x00\x00") # Try activating the stream.
                else: # Got a frame.
                    #print "Got a frame at ", curtime, 1 / (curtime - lastvalidtime)
                    if not activated:
                        print "Connection activated"
                        activated = True
                    try:
                        rawdata = intr.recv(128)
                    except BluetoothError, s:
                        print "Got Bluetooth error %s. Disconnecting."%s
                        return
                    if len(rawdata) == 0: # Orderly shutdown of socket
                        print "Joystick shut down the connection, battery may be discharged."
                        return
                    stepout = self.step(rawdata)
                    if stepout != self.step_error:
                        lastvalidtime = curtime
                    if stepout == self.step_active:
                        lastactivitytime = curtime
                if curtime - lastactivitytime > self.inactivity_timeout:
                    print "Joystick inactive for %.0f seconds. Disconnecting to save battery."%self.inactivity_timeout
                    return
                if curtime - lastvalidtime >= 0.1: # Zero all outputs if we don't hear a valid frame for 0.1 to 0.2 seconds
                    self.fullstop()
                if curtime - lastvalidtime >= 5: # Disconnect if we don't hear a valid frame for 5 seconds
                    print "No valid data for 5 seconds. Disconnecting. This should not happen, please report it."
                    return
                time.sleep(0.005) # No need to blaze through the loop when there is an error
        finally:
            self.fullstop()

class Quit(Exception):
    def __init__(self, errorcode):
        Exception.__init__(self)
        self.errorcode = errorcode

def check_hci_status():
    # Check if hci0 is up and pscanning, take action as necessary.
    proc = subprocess.Popen(['hciconfig'], stdout=subprocess.PIPE, stderr=subprocess.PIPE)
    (out, err) = proc.communicate()
    if out.find('UP') == -1:
        os.system("hciconfig hci0 up > /dev/null 2>&1")
    if out.find('PSCAN') == -1:
        os.system("hciconfig hci0 pscan > /dev/null 2>&1")   

class connection_manager:
    def __init__(self, decoder):
        self.decoder = decoder
        self.shutdown = False

    def prepare_bluetooth_socket(self, port):
        sock = BluetoothSocket(L2CAP)
        return self.prepare_socket(sock, port)

    def prepare_net_socket(self, port):
        sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        return self.prepare_socket(sock, port)

    def prepare_socket(self, sock, port):
        first_loop = True
        while True:
            try:
                sock.bind(("", port))
            except Exception, e:
                print repr(e)
                if first_loop:
                    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."
                first_loop = False
                time.sleep(0.5)
                continue 
            sock.listen(1)
            return sock

    def listen_net(self,intr_port, ctrl_port):
        intr_sock = self.prepare_net_socket(intr_port)
        ctrl_sock = self.prepare_net_socket(ctrl_port)
        self.listen(intr_sock, ctrl_sock)

    def listen_bluetooth(self):
        intr_sock = self.prepare_bluetooth_socket(L2CAP_PSM_HIDP_INTR)
        ctrl_sock = self.prepare_bluetooth_socket(L2CAP_PSM_HIDP_CTRL)
        self.listen(intr_sock, ctrl_sock)
    
    def listen(self, intr_sock, ctrl_sock):
        self.n = 0
        while not self.shutdown:
            print "Waiting for connection. Disconnect your PS3 joystick from USB and press the pairing button."
            try:
                intr_sock.settimeout(5)
                ctrl_sock.settimeout(1)
                while True:
                    try:
                        (intr, (idev, iport)) = intr_sock.accept();
                        break
                    except Exception, e:
                        if str(e) == 'timed out':
                            check_hci_status()
                        else:
                            raise
                    
                try:
                    try:
                        (ctrl, (cdev, cport)) = ctrl_sock.accept();
                    except Exception, e:
                        print >> sys.stderr, "Got interrupt connection without control connection. Giving up on it."
                        continue
                    try:
                        if idev == cdev:
                            self.decoder.run(intr, ctrl)
                            print "Connection terminated."
                        else:
                            print >> sys.stderr, "Simultaneous connection from two different devices. Ignoring both."
                    finally:
                        ctrl.close()
                finally:
                    intr.close()        
            except BadJoystickException:
                pass
            except KeyboardInterrupt:
                print "CTRL+C detected. Exiting."
                quit(0)
            except Exception, e:
                traceback.print_exc()
                print >> sys.stderr, "Caught exception: %s"%str(e)
                time.sleep(1)
            print
                    
inactivity_timout_string = "--inactivity-timeout"
no_disable_bluetoothd_string = "--no-disable-bluetoothd"
redirect_output_string = "--redirect-output"
continuous_motion_output_string = "--continuous-output"
                    
def usage(errcode):
    print "usage: ps3joy.py ["+inactivity_timout_string+"=<n>] ["+no_disable_bluetoothd_string+"] ["+redirect_output_string+"] ["+continuous_motion_output_string+"]=<f>"
    print "<n>: inactivity timeout in seconds (saves battery life)."
    print "<f>: file name to redirect output to."
    print "Unless "+no_disable_bluetoothd_string+" is specified, bluetoothd will be stopped."
    raise Quit(errcode)

def is_arg_with_param(arg, prefix):
    if not arg.startswith(prefix):
        return False
    if not arg.startswith(prefix+"="):
        print "Expected '=' after "+prefix
        print
        usage(1)
    return True

if __name__ == "__main__":
    errorcode = 0
    try:
        inactivity_timeout = float(1e3000)
        disable_bluetoothd = True
        continuous_output = False
        for arg in sys.argv[1:]: # Be very tolerant in case we are roslaunched.
            if arg == "--help":
                usage(0)
            elif is_arg_with_param(arg, inactivity_timout_string):
                str_value = arg[len(inactivity_timout_string)+1:]
                try:
                    inactivity_timeout = float(str_value)
                    if inactivity_timeout < 0:
                        print "Inactivity timeout must be positive."
                        print
                        usage(1)
                except ValueError:
                    print "Error parsing inactivity timeout: "+str_value
                    print
                    usage(1)
            elif arg == no_disable_bluetoothd_string:
                disable_bluetoothd = False
            elif arg == continuous_motion_output_string:
                continuous_output = True
            elif is_arg_with_param(arg, redirect_output_string):
                str_value = arg[len(redirect_output_string)+1:]
                try:
                    print "Redirecting output to:", str_value
                    sys.stdout = open(str_value, "a", 1)        
                except IOError, e:
                    print "Error opening file to redirect output:", str_value
                    raise Quit(1)
                sys.stderr = sys.stdout
            else:
                print "Ignoring parameter: '%s'"%arg
        if os.getuid() != 0:
            print >> sys.stderr, "ps3joy.py must be run as root."
            quit(1)
        if disable_bluetoothd:
            os.system("/etc/init.d/bluetooth stop > /dev/null 2>&1")
            time.sleep(1) # Give the socket time to be available.
        try:
            while os.system("hciconfig hci0 > /dev/null 2>&1") != 0:
                print >> sys.stderr,  "No bluetooth dongle found or bluez rosdep not installed. Will retry in 5 seconds."
                time.sleep(5)
            if inactivity_timeout == float(1e3000):
                print "No inactivity timeout was set. (Run with --help for details.)"
            else:
                print "Inactivity timeout set to %.0f seconds."%inactivity_timeout
            cm = connection_manager(decoder(inactivity_timeout = inactivity_timeout, continuous_motion_output = continuous_output))
            cm.listen_bluetooth()
        finally:
            if disable_bluetoothd:
                os.system("/etc/init.d/bluetooth start > /dev/null 2>&1")
    except Quit, e:
        errorcode = e.errorcode
    except KeyboardInterrupt:
        print "CTRL+C detected. Exiting."
    exit(errorcode)