ps3joy_node.py
Go to the documentation of this file.
00001 #!/usr/bin/env 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 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                 #print >> sys.stderr, "Error opening uinput: %s"%str(e)
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) # uinput isn't ready to go right away.
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         #id = uinput.input_id()
00084         #id.bustype = uinput.BUS_USB
00085         #id.vendor = 0x054C
00086         #id.product = 0x0268
00087         #id.version = 0
00088         #info = uinput.uinput_user_dev()
00089         #info.name = "Sony Playstation SixAxis/DS3"
00090         #info.id = id
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         #buttons=[uinput.BTN_SELECT, uinput.BTN_THUMBL, uinput.BTN_THUMBR, uinput.BTN_START,
00149         #         uinput.BTN_FORWARD, uinput.BTN_RIGHT, uinput.BTN_BACK, uinput.BTN_LEFT,
00150         #         uinput.BTN_TL, uinput.BTN_TR, uinput.BTN_TL2, uinput.BTN_TR2,
00151         #         uinput.BTN_X, uinput.BTN_A, uinput.BTN_B, uinput.BTN_Y,
00152         #         uinput.BTN_MODE]
00153         #axes=[uinput.ABS_X, uinput.ABS_Y, uinput.ABS_Z, uinput.ABS_RX,
00154         #         uinput.ABS_RX, uinput.ABS_RY, uinput.ABS_PRESSURE, uinput.ABS_DISTANCE,
00155         #         uinput.ABS_THROTTLE, uinput.ABS_RUDDER, uinput.ABS_WHEEL, uinput.ABS_GAS,
00156         #         uinput.ABS_HAT0Y, uinput.ABS_HAT1Y, uinput.ABS_HAT2Y, uinput.ABS_HAT3Y,
00157         #         uinput.ABS_TILT_X, uinput.ABS_TILT_Y, uinput.ABS_MISC, uinput.ABS_RZ,
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): # Gyros have more bits than other axes
00166             axmax[i] = 1023
00167             axfuzz[i] = 4
00168             axflat[i] = 4
00169         for i in range(4,len(axmin)-4): # Buttons should be zero when not pressed
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() # Probably useless because of uinput startup bug
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     #Raw Data Format
00196     #unsigned char ReportType;         //Report Type 01
00197     #unsigned char Reserved1;          // Unknown
00198     #unsigned int  ButtonState;        // Main buttons
00199     #unsigned char PSButtonState;      // PS button
00200     #unsigned char Reserved2;          // Unknown
00201     #unsigned char LeftStickX;         // left Joystick X axis 0 - 255, 128 is mid
00202     #unsigned char LeftStickY;         // left Joystick Y axis 0 - 255, 128 is mid
00203     #unsigned char RightStickX;        // right Joystick X axis 0 - 255, 128 is mid
00204     #unsigned char RightStickY;        // right Joystick Y axis 0 - 255, 128 is mid
00205     #unsigned char Reserved3[4];       // Unknown
00206     #unsigned char PressureUp;         // digital Pad Up button Pressure 0 - 255
00207     #unsigned char PressureRight;      // digital Pad Right button Pressure 0 - 255
00208     #unsigned char PressureDown;       // digital Pad Down button Pressure 0 - 255
00209     #unsigned char PressureLeft;       // digital Pad Left button Pressure 0 - 255
00210     #unsigned char PressureL2;         // digital Pad L2 button Pressure 0 - 255
00211     #unsigned char PressureR2;         // digital Pad R2 button Pressure 0 - 255
00212     #unsigned char PressureL1;         // digital Pad L1 button Pressure 0 - 255
00213     #unsigned char PressureR1;         // digital Pad R1 button Pressure 0 - 255
00214     #unsigned char PressureTriangle;   // digital Pad Triangle button Pressure 0 - 255
00215     #unsigned char PressureCircle;     // digital Pad Circle button Pressure 0 - 255
00216     #unsigned char PressureCross;      // digital Pad Cross button Pressure 0 - 255
00217     #unsigned char PressureSquare;     // digital Pad Square button Pressure 0 - 255
00218     #unsigned char Reserved4[3];       // Unknown
00219     #unsigned char Charge;             // charging status ? 02 = charge, 03 = normal
00220     #unsigned char Power;              // Battery status
00221     #unsigned char Connection;         // Connection Type
00222     #unsigned char Reserved5[9];       // Unknown
00223     #unsigned int AccelerometerX;      // X axis accelerometer Big Endian 0 - 1023
00224     #unsigned int Accelero             // Y axis accelerometer Big Endian 0 - 1023
00225     #unsigned int AccelerometerZ;      // Z axis accelerometer Big Endian 0 - 1023
00226     #unsigned int GyrometerX;          // Z axis Gyro Big Endian 0 - 1023
00227     #*********************************************************************************
00228     def step(self, rawdata): # Returns true if the packet was legal
00229         if len(rawdata) == 50:
00230             joy_coding = "!1B2x3B1x4B4x12B3x1B1B1B9x4H"
00231             all_data = list(struct.unpack(joy_coding, rawdata)) #removing power data
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): # Split out the buttons.
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                                                                        # 17 buttons, 4 inertial sensors
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             #print list(rawdata)
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],        # rumble values
00278                    0x00, 0x00, 0x00, 0x00, self.led_cmd,
00279                    0xff, 0x27, 0x10, 0x00, 0x32,        # LED 4
00280                    0xff, 0x27, 0x10, 0x00, 0x32,        # LED 3
00281                    0xff, 0x27, 0x10, 0x00, 0x32,        # LED 2
00282                    0xff, 0x27, 0x10, 0x00, 0x32,        # LED 1
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: # Timeout
00297                     #print "Activating connection."
00298                     ctrl.send("\x53\xf4\x42\x03\x00\x00") # Try activating the stream.
00299                 else: # Got a frame.
00300                     #print "Got a frame at ", curtime, 1 / (curtime - lastvalidtime)
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: # Orderly shutdown of socket
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                         #for when we can restart a rosnode
00322 #                        if self.deamon:
00323 #                            self.core_down= True
00324 #                        else:
00325 #                            print "The roscore shutdown, ps3joy shutting down. Run with --deamon if you want ps3joy to respawn"
00326 #                            return
00327 #                    if self.core_down == True:
00328 #                        try:
00329 #                            rosgraph.masterapi.is_online()
00330 #                            self.init_ros()
00331 #                            self.core_down = False
00332 #                            print "succeeded bringing node up"
00333 #                        except:
00334 #                            print "failed to bring node up"
00335 #                            pass
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: # Zero all outputs if we don't hear a valid frame for 0.1 to 0.2 seconds
00346                     self.fullstop()
00347                 if curtime - lastvalidtime >= 5: # Disconnect if we don't hear a valid frame for 5 seconds
00348                     print "No valid data for 5 seconds. Disconnecting. This should not happen, please report it."
00349                     return
00350                 time.sleep(0.005) # No need to blaze through the loop when there is an error
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         # limit to 1hz
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         #battery info
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         #battery info
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         #battery info
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         #publish
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     # Check if hci0 is up and pscanning, take action as necessary.
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 #deamon_string = "--deamon"
00511 
00512 def usage(errcode):
00513 #    print "usage: ps3joy.py ["+inactivity_timout_string+"=<n>] ["+no_disable_bluetoothd_string+"] ["+redirect_output_string+"]=<f> ["+deamon_string+"]=<d>"
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 #    print "<d>: runs in deamon mode respawning node when roscore goes down."
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:]: # Be very tolerant in case we are roslaunched.
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 #            elif arg == deamon_string:
00563 #                deamon = True
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) # Give the socket time to be available.
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 


ps3joy
Author(s): Blaise Gassend, pascal@pabr.org, Melonee Wise
autogenerated on Mon Oct 6 2014 01:06:36