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 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: # 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.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         ## timed gate: limit to 1 Hz
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         ## compose diagnostics message
00386         diag = DiagnosticArray()
00387         diag.header.stamp = curr_time
00388         # battery info
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         # connection info
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         # charging info
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         # publish message
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     # Check if hci0 is up and pscanning, take action as necessary.
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 #deamon_string = "--deamon"
00524 
00525 def usage(errcode):
00526 #    print "usage: ps3joy.py ["+inactivity_timout_string+"=<n>] ["+no_disable_bluetoothd_string+"] ["+redirect_output_string+"]=<f> ["+deamon_string+"]=<d>"
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 #    print "<d>: runs in deamon mode respawning node when roscore goes down."
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:]: # Be very tolerant in case we are roslaunched.
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 #            elif arg == deamon_string:
00576 #                deamon = True
00577             else:
00578                 print "Ignoring parameter: '%s'"%arg
00579         
00580         # If the user does not have HW permissions indicate that ps3joy must be run as root
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) # Give the socket time to be available.
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 


ps3joy
Author(s): Blaise Gassend, pascal@pabr.org, Melonee Wise
autogenerated on Sun Jul 9 2017 02:34:56