ps3joy_node.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 #***********************************************************
3 #* Software License Agreement (BSD License)
4 #*
5 #* Copyright (c) 2009, Willow Garage, Inc.
6 #* All rights reserved.
7 #*
8 #* Redistribution and use in source and binary forms, with or without
9 #* modification, are permitted provided that the following conditions
10 #* are met:
11 #*
12 #* * Redistributions of source code must retain the above copyright
13 #* notice, this list of conditions and the following disclaimer.
14 #* * Redistributions in binary form must reproduce the above
15 #* copyright notice, this list of conditions and the following
16 #* disclaimer in the documentation and/or other materials provided
17 #* with the distribution.
18 #* * Neither the name of the Willow Garage nor the names of its
19 #* contributors may be used to endorse or promote products derived
20 #* from this software without specific prior written permission.
21 #*
22 #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 #* POSSIBILITY OF SUCH DAMAGE.
34 #***********************************************************
35 import roslib; roslib.load_manifest('ps3joy')
36 import rospy
37 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
38 
39 from bluetooth import *
40 import select
41 import fcntl
42 import os
43 import time
44 import sys
45 import traceback
46 import subprocess
47 from array import array
48 import sensor_msgs.msg
49 import rosgraph.masterapi
50 
51 L2CAP_PSM_HIDP_CTRL = 17
52 L2CAP_PSM_HIDP_INTR = 19
53 
54 class uinput:
55  EV_KEY = 1
56  EV_REL = 2
57  EV_ABS = 3
58  BUS_USB = 3
59  ABS_MAX = 0x3f
60 
61 
62 class uinputjoy:
63  def open_uinput(self):
64  for name in ["/dev/input/uinput", "/dev/misc/uinput", "/dev/uinput"]:
65  try:
66  return os.open(name, os.O_WRONLY)
67  break
68  except Exception, e:
69  #print >> sys.stderr, "Error opening uinput: %s"%str(e)
70  pass
71  return None
72 
73  def __init__(self, buttons, axes, axmin, axmax, axfuzz, axflat):
74  self.file = self.open_uinput()
75  if self.file == None:
76  print >> sys.stderr, "Trying to modprobe uinput."
77  os.system("modprobe uinput > /dev/null 2>&1")
78  time.sleep(1) # uinput isn't ready to go right away.
79  self.file = self.open_uinput()
80  if self.file == None:
81  print >> sys.stderr, "Can't open uinput device. Is it accessible by this user? Did you mean to run as root?"
82  raise IOError
83  #id = uinput.input_id()
84  #id.bustype = uinput.BUS_USB
85  #id.vendor = 0x054C
86  #id.product = 0x0268
87  #id.version = 0
88  #info = uinput.uinput_user_dev()
89  #info.name = "Sony Playstation SixAxis/DS3"
90  #info.id = id
91 
92  UI_SET_EVBIT = 0x40045564
93  UI_SET_KEYBIT = 0x40045565
94  UI_SET_RELBIT = 0x40045566
95  UI_DEV_CREATE = 0x5501
96  UI_SET_RELBIT = 0x40045566
97  UI_SET_ABSBIT = 0x40045567
98  uinput_user_dev = "80sHHHHi" + (uinput.ABS_MAX+1)*4*'i'
99 
100  if len(axes) != len(axmin) or len(axes) != len(axmax):
101  raise Exception("uinputjoy.__init__: axes, axmin and axmax should have same length")
102  absmin = [0] * (uinput.ABS_MAX+1)
103  absmax = [0] * (uinput.ABS_MAX+1)
104  absfuzz = [2] * (uinput.ABS_MAX+1)
105  absflat = [4] * (uinput.ABS_MAX+1)
106  for i in range(0, len(axes)):
107  absmin[axes[i]] = axmin[i]
108  absmax[axes[i]] = axmax[i]
109  absfuzz[axes[i]] = axfuzz[i]
110  absflat[axes[i]] = axflat[i]
111 
112  os.write(self.file, struct.pack(uinput_user_dev, "Sony Playstation SixAxis/DS3",
113  uinput.BUS_USB, 0x054C, 0x0268, 0, 0, *(absmax + absmin + absfuzz + absflat)))
114 
115  fcntl.ioctl(self.file, UI_SET_EVBIT, uinput.EV_KEY)
116 
117  for b in buttons:
118  fcntl.ioctl(self.file, UI_SET_KEYBIT, b)
119 
120  for a in axes:
121  fcntl.ioctl(self.file, UI_SET_EVBIT, uinput.EV_ABS)
122  fcntl.ioctl(self.file, UI_SET_ABSBIT, a)
123 
124  fcntl.ioctl(self.file, UI_DEV_CREATE)
125 
126  self.value = [None] * (len(buttons) + len(axes))
127  self.type = [uinput.EV_KEY] * len(buttons) + [uinput.EV_ABS] * len(axes)
128  self.code = buttons + axes
129 
130  def update(self, value):
131  input_event = "LLHHi"
132  t = time.time()
133  th = int(t)
134  tl = int((t - th) * 1000000)
135  if len(value) != len(self.value):
136  print >> sys.stderr, "Unexpected length for value in update (%i instead of %i). This is a bug."%(len(value), len(self.value))
137  for i in range(0, len(value)):
138  if value[i] != self.value[i]:
139  os.write(self.file, struct.pack(input_event, th, tl, self.type[i], self.code[i], value[i]))
140  self.value = list(value)
141 
142 class BadJoystickException(Exception):
143  def __init__(self):
144  Exception.__init__(self, "Unsupported joystick.")
145 
146 class decoder:
147  def __init__(self, deamon, inactivity_timeout = float(1e3000)):
148  #buttons=[uinput.BTN_SELECT, uinput.BTN_THUMBL, uinput.BTN_THUMBR, uinput.BTN_START,
149  # uinput.BTN_FORWARD, uinput.BTN_RIGHT, uinput.BTN_BACK, uinput.BTN_LEFT,
150  # uinput.BTN_TL, uinput.BTN_TR, uinput.BTN_TL2, uinput.BTN_TR2,
151  # uinput.BTN_X, uinput.BTN_A, uinput.BTN_B, uinput.BTN_Y,
152  # uinput.BTN_MODE]
153  #axes=[uinput.ABS_X, uinput.ABS_Y, uinput.ABS_Z, uinput.ABS_RX,
154  # uinput.ABS_RX, uinput.ABS_RY, uinput.ABS_PRESSURE, uinput.ABS_DISTANCE,
155  # uinput.ABS_THROTTLE, uinput.ABS_RUDDER, uinput.ABS_WHEEL, uinput.ABS_GAS,
156  # uinput.ABS_HAT0Y, uinput.ABS_HAT1Y, uinput.ABS_HAT2Y, uinput.ABS_HAT3Y,
157  # uinput.ABS_TILT_X, uinput.ABS_TILT_Y, uinput.ABS_MISC, uinput.ABS_RZ,
158  # ]
159  buttons = range(0x100,0x111)
160  axes = range(0, 20)
161  axmin = [0] * 20
162  axmax = [255] * 20
163  axfuzz = [2] * 20
164  axflat = [4] * 20
165  for i in range(-4,0): # Gyros have more bits than other axes
166  axmax[i] = 1023
167  axfuzz[i] = 4
168  axflat[i] = 4
169  for i in range(4,len(axmin)-4): # Buttons should be zero when not pressed
170  axmin[i] = -axmax[i]
171  self.joy = uinputjoy(buttons, axes, axmin, axmax, axfuzz, axflat)
172  self.axmid = [sum(pair)/2 for pair in zip(axmin, axmax)]
173  self.fullstop() # Probably useless because of uinput startup bug
174  self.outlen = len(buttons) + len(axes)
175  self.inactivity_timeout = inactivity_timeout
176  self.deamon = deamon
177  self.init_ros()
178  step_active = 1
179  step_idle = 2
180  step_error = 3
181 
182  def init_ros(self):
183  try:
184  rospy.init_node('ps3joy', anonymous=True, disable_signals=True)
185  except:
186  print "rosnode init failed"
187  rospy.Subscriber("joy/set_feedback",sensor_msgs.msg.JoyFeedbackArray,self.set_feedback)
189  self.led_values = [1,0,0,0]
190  self.rumble_cmd = [0, 255]
191  self.led_cmd = 2
192  self.core_down = False
193 
194  #********************************************************************************
195  #Raw Data Format
196  #unsigned char ReportType; //Report Type 01
197  #unsigned char Reserved1; // Unknown
198  #unsigned int ButtonState; // Main buttons
199  #unsigned char PSButtonState; // PS button
200  #unsigned char Reserved2; // Unknown
201  #unsigned char LeftStickX; // left Joystick X axis 0 - 255, 128 is mid
202  #unsigned char LeftStickY; // left Joystick Y axis 0 - 255, 128 is mid
203  #unsigned char RightStickX; // right Joystick X axis 0 - 255, 128 is mid
204  #unsigned char RightStickY; // right Joystick Y axis 0 - 255, 128 is mid
205  #unsigned char Reserved3[4]; // Unknown
206  #unsigned char PressureUp; // digital Pad Up button Pressure 0 - 255
207  #unsigned char PressureRight; // digital Pad Right button Pressure 0 - 255
208  #unsigned char PressureDown; // digital Pad Down button Pressure 0 - 255
209  #unsigned char PressureLeft; // digital Pad Left button Pressure 0 - 255
210  #unsigned char PressureL2; // digital Pad L2 button Pressure 0 - 255
211  #unsigned char PressureR2; // digital Pad R2 button Pressure 0 - 255
212  #unsigned char PressureL1; // digital Pad L1 button Pressure 0 - 255
213  #unsigned char PressureR1; // digital Pad R1 button Pressure 0 - 255
214  #unsigned char PressureTriangle; // digital Pad Triangle button Pressure 0 - 255
215  #unsigned char PressureCircle; // digital Pad Circle button Pressure 0 - 255
216  #unsigned char PressureCross; // digital Pad Cross button Pressure 0 - 255
217  #unsigned char PressureSquare; // digital Pad Square button Pressure 0 - 255
218  #unsigned char Reserved4[3]; // Unknown
219  #unsigned char Charge; // charging status ? 02 = charge, 03 = normal
220  #unsigned char Power; // Battery status
221  #unsigned char Connection; // Connection Type
222  #unsigned char Reserved5[9]; // Unknown
223  #unsigned int AccelerometerX; // X axis accelerometer Big Endian 0 - 1023
224  #unsigned int Accelero // Y axis accelerometer Big Endian 0 - 1023
225  #unsigned int AccelerometerZ; // Z axis accelerometer Big Endian 0 - 1023
226  #unsigned int GyrometerX; // Z axis Gyro Big Endian 0 - 1023
227  #*********************************************************************************
228  def step(self, rawdata): # Returns true if the packet was legal
229  if len(rawdata) == 50:
230  joy_coding = "!1B2x3B1x4B4x12B3x1B1B1B9x4H"
231  all_data = list(struct.unpack(joy_coding, rawdata)) #removing power data
232  state_data = all_data[20:23]
233  data = all_data[0:20]+all_data[23:]
234  prefix = data.pop(0)
235  self.diagnostics.publish(state_data)
236  if prefix != 161:
237  print >> sys.stderr, "Unexpected prefix (%i). Is this a PS3 Dual Shock or Six Axis?"%prefix
238  return self.step_error
239  out = []
240  for j in range(0,2): # Split out the buttons.
241  curbyte = data.pop(0)
242  for k in range(0,8):
243  out.append(int((curbyte & (1 << k)) != 0))
244  out = out + data
245  self.joy.update(out)
246  axis_motion = [abs(out[17:][i] - self.axmid[i]) > 20 for i in range(0,len(out)-17-4)]
247  # 17 buttons, 4 inertial sensors
248  if any(out[0:17]) or any(axis_motion):
249  return self.step_active
250  return self.step_idle
251  elif len(rawdata) == 13:
252  #print list(rawdata)
253  print >> sys.stderr, "Your bluetooth adapter is not supported. Does it support Bluetooth 2.0? Please report its model to blaise@willowgarage.com"
254  raise BadJoystickException()
255  else:
256  print >> sys.stderr, "Unexpected packet length (%i). Is this a PS3 Dual Shock or Six Axis?"%len(rawdata)
257  return self.step_error
258 
259  def fullstop(self):
260  self.joy.update([0] * 17 + self.axmid)
261 
262 
263  def set_feedback(self,msg):
264  for feedback in msg.array:
265  if feedback.type == sensor_msgs.msg.JoyFeedback.TYPE_LED and feedback.id < 4:
266  self.led_values[feedback.id] = int(round(feedback.intensity))
267  elif feedback.type == sensor_msgs.msg.JoyFeedback.TYPE_RUMBLE and feedback.id < 2:
268  self.rumble_cmd[feedback.id] = int(feedback.intensity*255)
269  else:
270  rospy.logwarn("Feedback %s of type %s does not exist for this joystick.",feedback.id, feedback.type)
271  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)
272  self.new_msg = True
273 
274  def send_cmd(self, ctrl):
275  command = [0x52,
276  0x01,
277  0x00, 0xfe, self.rumble_cmd[1], 0xfe, self.rumble_cmd[0], # rumble values
278  0x00, 0x00, 0x00, 0x00, self.led_cmd,
279  0xff, 0x27, 0x10, 0x00, 0x32, # LED 4
280  0xff, 0x27, 0x10, 0x00, 0x32, # LED 3
281  0xff, 0x27, 0x10, 0x00, 0x32, # LED 2
282  0xff, 0x27, 0x10, 0x00, 0x32, # LED 1
283  0x00, 0x00, 0x00, 0x00, 0x00
284  ]
285  ctrl.send(array('B', command).tostring())
286  self.new_msg = False
287 
288  def run(self, intr, ctrl):
289  activated = False
290  try:
291  self.fullstop()
292  lastactivitytime = lastvalidtime = time.time()
293  while not rospy.is_shutdown():
294  (rd, wr, err) = select.select([intr], [], [], 0.1)
295  curtime = time.time()
296  if len(rd) + len(wr) + len(err) == 0: # Timeout
297  #print "Activating connection."
298  ctrl.send("\x53\xf4\x42\x03\x00\x00") # Try activating the stream.
299  else: # Got a frame.
300  #print "Got a frame at ", curtime, 1 / (curtime - lastvalidtime)
301  if not activated:
302  self.send_cmd(ctrl)
303  time.sleep(0.5)
304  self.rumble_cmd[1] = 0
305  self.send_cmd(ctrl)
306  print "Connection activated"
307  activated = True
308  try:
309  if(self.new_msg):
310  self.send_cmd(ctrl)
311  rawdata = intr.recv(128)
312  except BluetoothError, s:
313  print "Got Bluetooth error %s. Disconnecting."%s
314  return
315  if len(rawdata) == 0: # Orderly shutdown of socket
316  print "Joystick shut down the connection, battery may be discharged."
317  return
318  if not rosgraph.masterapi.is_online():
319  print "The roscore or node shutdown, ps3joy shutting down."
320  return
321  #for when we can restart a rosnode
322 # if self.deamon:
323 # self.core_down= True
324 # else:
325 # print "The roscore shutdown, ps3joy shutting down. Run with --deamon if you want ps3joy to respawn"
326 # return
327 # if self.core_down == True:
328 # try:
329 # rosgraph.masterapi.is_online()
330 # self.init_ros()
331 # self.core_down = False
332 # print "succeeded bringing node up"
333 # except:
334 # print "failed to bring node up"
335 # pass
336 
337  stepout = self.step(rawdata)
338  if stepout != self.step_error:
339  lastvalidtime = curtime
340  if stepout == self.step_active:
341  lastactivitytime = curtime
342  if curtime - lastactivitytime > self.inactivity_timeout:
343  print "Joystick inactive for %.0f seconds. Disconnecting to save battery."%self.inactivity_timeout
344  return
345  if curtime - lastvalidtime >= 0.1: # Zero all outputs if we don't hear a valid frame for 0.1 to 0.2 seconds
346  self.fullstop()
347  if curtime - lastvalidtime >= 5: # Disconnect if we don't hear a valid frame for 5 seconds
348  print "No valid data for 5 seconds. Disconnecting. This should not happen, please report it."
349  return
350  time.sleep(0.005) # No need to blaze through the loop when there is an error
351  finally:
352  self.fullstop()
353 
354 class Diagnostics():
355  def __init__(self):
357  0:"Charging",
358  1:"Not Charging"}
360  18:"USB Connection",
361  20:"Rumbling",
362  22:"Bluetooth Connection"}
364  0:"No Charge",
365  1:"20% Charge",
366  2:"40% Charge",
367  3:"60% Charge",
368  4:"80% Charge",
369  5:"100% Charge",
370  238:"Charging"}
371  self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
372  self.last_diagnostics_time = rospy.get_rostime()
373 
374  def publish(self, state):
375  STATE_INDEX_CHARGING = 0
376  STATE_INDEX_BATTERY = 1
377  STATE_INDEX_CONNECTION = 2
378 
379  ## timed gate: limit to 1 Hz
380  curr_time = rospy.get_rostime()
381  if (curr_time - self.last_diagnostics_time).to_sec() < 1.0:
382  return
383  self.last_diagnostics_time = curr_time
384 
385  ## compose diagnostics message
386  diag = DiagnosticArray()
387  diag.header.stamp = curr_time
388  # battery info
389  stat = DiagnosticStatus(name="Battery", level=DiagnosticStatus.OK, message="OK")
390  try:
391  battery_state_code = state[STATE_INDEX_BATTERY]
392  stat.message = self.STATE_TEXTS_BATTERY[battery_state_code]
393  if battery_state_code < 3:
394  stat.level = DiagnosticStatus.WARN
395  if battery_state_code < 1:
396  stat.level = DiagnosticStatus.ERROR
397  stat.message = "Please Recharge Battery (%s)." % self.STATE_TEXTS_BATTERY[battery_state_code]
398  except KeyError as ex:
399  stat.message = "Invalid Battery State %s"%ex
400  rospy.logwarn("Invalid Battery State %s"%ex)
401  stat.level = DiagnosticStatus.ERROR
402  diag.status.append(stat)
403  # connection info
404  stat = DiagnosticStatus(name='ps3joy'": Connection Type", level=DiagnosticStatus.OK, message="OK")
405  try:
406  stat.message = self.STATE_TEXTS_CONNECTION[state[STATE_INDEX_CONNECTION]]
407  except KeyError as ex:
408  stat.message = "Invalid Connection State %s"%ex
409  rospy.logwarn("Invalid Connection State %s"%ex)
410  stat.level = DiagnosticStatus.ERROR
411  diag.status.append(stat)
412  # charging info
413  stat = DiagnosticStatus(name='ps3joy'": Charging State", level=DiagnosticStatus.OK, message="OK")
414  try:
415  stat.message = self.STATE_TEXTS_CHARGING[state[STATE_INDEX_CHARGING]]
416  except KeyError as ex:
417  stat.message = "Invalid Charging State %s"%ex
418  rospy.logwarn("Invalid Charging State %s"%ex)
419  stat.level = DiagnosticStatus.ERROR
420  diag.status.append(stat)
421  # publish message
422  self.diag_pub.publish(diag)
423 
424 class Quit(Exception):
425  def __init__(self, errorcode):
426  Exception.__init__(self)
427  self.errorcode = errorcode
428 
430  # Check if hci0 is up and pscanning, take action as necessary.
431  proc = subprocess.Popen(['hciconfig'], stdout=subprocess.PIPE, stderr=subprocess.PIPE)
432  (out, err) = proc.communicate()
433  if out.find('UP') == -1:
434  os.system("hciconfig hci0 up > /dev/null 2>&1")
435  if out.find('PSCAN') == -1:
436  os.system("hciconfig hci0 pscan > /dev/null 2>&1")
437 
439  def __init__(self, decoder):
440  self.decoder = decoder
441 
442  def prepare_bluetooth_socket(self, port):
443  sock = BluetoothSocket(L2CAP)
444  return self.prepare_socket(sock, port)
445 
446  def prepare_net_socket(self, port):
447  sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
448  return self.prepare_socket(sock, port)
449 
450  def prepare_socket(self, sock, port):
451  first_loop = True
452  while True:
453  try:
454  sock.bind(("", port))
455  except Exception, e:
456  print repr(e)
457  if first_loop:
458  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."
459  first_loop = False
460  time.sleep(0.5)
461  continue
462  sock.listen(1)
463  return sock
464 
465  def listen_net(self,intr_port, ctrl_port):
466  intr_sock = self.prepare_net_socket(intr_port)
467  ctrl_sock = self.prepare_net_socket(ctrl_port)
468  self.listen(intr_sock, ctrl_sock)
469 
470  def listen_bluetooth(self):
471  intr_sock = self.prepare_bluetooth_socket(L2CAP_PSM_HIDP_INTR)
472  ctrl_sock = self.prepare_bluetooth_socket(L2CAP_PSM_HIDP_CTRL)
473  self.listen(intr_sock, ctrl_sock)
474 
475  def listen(self, intr_sock, ctrl_sock):
476  self.n = 0
477  while not rospy.is_shutdown():
478  print "Waiting for connection. Disconnect your PS3 joystick from USB and press the pairing button."
479  try:
480  intr_sock.settimeout(5)
481  ctrl_sock.settimeout(1)
482  while True:
483  try:
484  (intr, (idev, iport)) = intr_sock.accept();
485  break
486  except Exception, e:
487  if str(e) == 'timed out':
489  else:
490  raise
491 
492  try:
493  try:
494  (ctrl, (cdev, cport)) = ctrl_sock.accept();
495  except Exception, e:
496  print >> sys.stderr, "Got interrupt connection without control connection. Giving up on it."
497  continue
498  try:
499  if idev == cdev:
500  self.decoder.run(intr, ctrl)
501  print "Connection terminated."
502  quit(0)
503  else:
504  print >> sys.stderr, "Simultaneous connection from two different devices. Ignoring both."
505  finally:
506  ctrl.close()
507  finally:
508  intr.close()
509  except BadJoystickException:
510  pass
511  except KeyboardInterrupt:
512  print "\nCTRL+C detected. Exiting."
513  rospy.signal_shutdown("\nCTRL+C detected. Exiting.")
514  quit(0)
515  except Exception, e:
516  traceback.print_exc()
517  print >> sys.stderr, "Caught exception: %s"%str(e)
518  time.sleep(1)
519 
520 inactivity_timout_string = "--inactivity-timeout"
521 no_disable_bluetoothd_string = "--no-disable-bluetoothd"
522 redirect_output_string = "--redirect-output"
523 #deamon_string = "--deamon"
524 
525 def usage(errcode):
526 # print "usage: ps3joy.py ["+inactivity_timout_string+"=<n>] ["+no_disable_bluetoothd_string+"] ["+redirect_output_string+"]=<f> ["+deamon_string+"]=<d>"
527  print "usage: ps3joy.py ["+inactivity_timout_string+"=<n>] ["+no_disable_bluetoothd_string+"] ["+redirect_output_string+"]=<f>"
528  print "<n>: inactivity timeout in seconds (saves battery life)."
529  print "<f>: file name to redirect output to."
530 # print "<d>: runs in deamon mode respawning node when roscore goes down."
531  print "Unless "+no_disable_bluetoothd_string+" is specified, bluetoothd will be stopped."
532  raise Quit(errcode)
533 
534 def is_arg_with_param(arg, prefix):
535  if not arg.startswith(prefix):
536  return False
537  if not arg.startswith(prefix+"="):
538  print "Expected '=' after "+prefix
539  print
540  usage(1)
541  return True
542 
543 if __name__ == "__main__":
544  errorcode = 0
545  try:
546  inactivity_timeout = float(1e3000)
547  disable_bluetoothd = True
548  deamon = False
549  for arg in sys.argv[1:]: # Be very tolerant in case we are roslaunched.
550  if arg == "--help":
551  usage(0)
552  elif is_arg_with_param(arg, inactivity_timout_string):
553  str_value = arg[len(inactivity_timout_string)+1:]
554  try:
555  inactivity_timeout = float(str_value)
556  if inactivity_timeout < 0:
557  print "Inactivity timeout must be positive."
558  print
559  usage(1)
560  except ValueError:
561  print "Error parsing inactivity timeout: "+str_value
562  print
563  usage(1)
564  elif arg == no_disable_bluetoothd_string:
565  disable_bluetoothd = False
566  elif is_arg_with_param(arg, redirect_output_string):
567  str_value = arg[len(redirect_output_string)+1:]
568  try:
569  print "Redirecting output to:", str_value
570  sys.stdout = open(str_value, "a", 1)
571  except IOError, e:
572  print "Error opening file to redirect output:", str_value
573  raise Quit(1)
574  sys.stderr = sys.stdout
575 # elif arg == deamon_string:
576 # deamon = True
577  else:
578  print "Ignoring parameter: '%s'"%arg
579 
580  # If the user does not have HW permissions indicate that ps3joy must be run as root
581  if os.getuid() != 0:
582  print >> sys.stderr, "ps3joy.py must be run as root."
583  quit(1)
584  if disable_bluetoothd:
585  os.system("/etc/init.d/bluetooth stop > /dev/null 2>&1")
586  time.sleep(1) # Give the socket time to be available.
587  try:
588  while os.system("hciconfig hci0 > /dev/null 2>&1") != 0:
589  print >> sys.stderr, "No bluetooth dongle found or bluez rosdep not installed. Will retry in 5 seconds."
590  time.sleep(5)
591  if inactivity_timeout == float(1e3000):
592  print "No inactivity timeout was set. (Run with --help for details.)"
593  else:
594  print "Inactivity timeout set to %.0f seconds."%inactivity_timeout
595  cm = connection_manager(decoder(deamon, inactivity_timeout = inactivity_timeout))
596  cm.listen_bluetooth()
597  finally:
598  if disable_bluetoothd:
599  os.system("/etc/init.d/bluetooth start > /dev/null 2>&1")
600  except Quit, e:
601  errorcode = e.errorcode
602  except KeyboardInterrupt:
603  print "\nCTRL+C detected. Exiting."
604  rospy.signal_shutdown("\nCTRL+C detected. Exiting.")
605  exit(errorcode)
606 
def __init__(self, buttons, axes, axmin, axmax, axfuzz, axflat)
Definition: ps3joy_node.py:73
def __init__(self, deamon, inactivity_timeout=float(1e3000))
Definition: ps3joy_node.py:147
def update(self, value)
Definition: ps3joy_node.py:130
def usage(errcode)
Definition: ps3joy_node.py:525
def step(self, rawdata)
Definition: ps3joy_node.py:228
def send_cmd(self, ctrl)
Definition: ps3joy_node.py:274
def listen_net(self, intr_port, ctrl_port)
Definition: ps3joy_node.py:465
def __init__(self, decoder)
Definition: ps3joy_node.py:439
def check_hci_status()
Definition: ps3joy_node.py:429
def listen(self, intr_sock, ctrl_sock)
Definition: ps3joy_node.py:475
def publish(self, state)
Definition: ps3joy_node.py:374
def is_arg_with_param(arg, prefix)
Definition: ps3joy_node.py:534
def run(self, intr, ctrl)
Definition: ps3joy_node.py:288
def prepare_net_socket(self, port)
Definition: ps3joy_node.py:446
def set_feedback(self, msg)
Definition: ps3joy_node.py:263
def prepare_bluetooth_socket(self, port)
Definition: ps3joy_node.py:442
last_diagnostics_time
timed gate: limit to 1 Hz
Definition: ps3joy_node.py:372
def prepare_socket(self, sock, port)
Definition: ps3joy_node.py:450
def __init__(self, errorcode)
Definition: ps3joy_node.py:425


ps3joy
Author(s): Blaise Gassend, pascal@pabr.org, Melonee Wise
autogenerated on Mon Jun 10 2019 13:42:41