ps3joy.py
Go to the documentation of this file.
1 #!/usr/bin/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 
36 from bluetooth import *
37 import select
38 import fcntl
39 import os
40 import time
41 import sys
42 import traceback
43 import subprocess
44 
45 L2CAP_PSM_HIDP_CTRL = 17
46 L2CAP_PSM_HIDP_INTR = 19
47 
48 class uinput:
49  EV_KEY = 1
50  EV_REL = 2
51  EV_ABS = 3
52  BUS_USB = 3
53  ABS_MAX = 0x3f
54 
55 class uinputjoy:
56  def open_uinput(self):
57  for name in ["/dev/input/uinput", "/dev/misc/uinput", "/dev/uinput"]:
58  try:
59  return os.open(name, os.O_WRONLY)
60  break
61  except Exception, e:
62  #print >> sys.stderr, "Error opening uinput: %s"%str(e)
63  pass
64  return None
65 
66  def __init__(self, buttons, axes, axmin, axmax, axfuzz, axflat):
67  self.file = self.open_uinput()
68  if self.file == None:
69  print >> sys.stderr, "Trying to modprobe uinput."
70  os.system("modprobe uinput > /dev/null 2>&1")
71  time.sleep(1) # uinput isn't ready to go right away.
72  self.file = self.open_uinput()
73  if self.file == None:
74  print >> sys.stderr, "Can't open uinput device. Is it accessible by this user? Did you mean to run as root?"
75  raise IOError
76  #id = uinput.input_id()
77  #id.bustype = uinput.BUS_USB
78  #id.vendor = 0x054C
79  #id.product = 0x0268
80  #id.version = 0
81  #info = uinput.uinput_user_dev()
82  #info.name = "Sony Playstation SixAxis/DS3"
83  #info.id = id
84 
85  UI_SET_EVBIT = 0x40045564
86  UI_SET_KEYBIT = 0x40045565
87  UI_SET_RELBIT = 0x40045566
88  UI_DEV_CREATE = 0x5501
89  UI_SET_RELBIT = 0x40045566
90  UI_SET_ABSBIT = 0x40045567
91  uinput_user_dev = "80sHHHHi" + (uinput.ABS_MAX+1)*4*'i'
92 
93  if len(axes) != len(axmin) or len(axes) != len(axmax):
94  raise Exception("uinputjoy.__init__: axes, axmin and axmax should have same length")
95  absmin = [0] * (uinput.ABS_MAX+1)
96  absmax = [0] * (uinput.ABS_MAX+1)
97  absfuzz = [2] * (uinput.ABS_MAX+1)
98  absflat = [4] * (uinput.ABS_MAX+1)
99  for i in range(0, len(axes)):
100  absmin[axes[i]] = axmin[i]
101  absmax[axes[i]] = axmax[i]
102  absfuzz[axes[i]] = axfuzz[i]
103  absflat[axes[i]] = axflat[i]
104 
105  os.write(self.file, struct.pack(uinput_user_dev, "Sony Playstation SixAxis/DS3",
106  uinput.BUS_USB, 0x054C, 0x0268, 0, 0, *(absmax + absmin + absfuzz + absflat)))
107 
108  fcntl.ioctl(self.file, UI_SET_EVBIT, uinput.EV_KEY)
109 
110  for b in buttons:
111  fcntl.ioctl(self.file, UI_SET_KEYBIT, b)
112 
113  for a in axes:
114  fcntl.ioctl(self.file, UI_SET_EVBIT, uinput.EV_ABS)
115  fcntl.ioctl(self.file, UI_SET_ABSBIT, a)
116 
117  fcntl.ioctl(self.file, UI_DEV_CREATE)
118 
119  self.value = [None] * (len(buttons) + len(axes))
120  self.type = [uinput.EV_KEY] * len(buttons) + [uinput.EV_ABS] * len(axes)
121  self.code = buttons + axes
122 
123  def update(self, value):
124  input_event = "LLHHi"
125  t = time.time()
126  th = int(t)
127  tl = int((t - th) * 1000000)
128  if len(value) != len(self.value):
129  print >> sys.stderr, "Unexpected length for value in update (%i instead of %i). This is a bug."%(len(value), len(self.value))
130  for i in range(0, len(value)):
131  if value[i] != self.value[i]:
132  os.write(self.file, struct.pack(input_event, th, tl, self.type[i], self.code[i], value[i]))
133  self.value = list(value)
134 
135 class BadJoystickException(Exception):
136  def __init__(self):
137  Exception.__init__(self, "Unsupported joystick.")
138 
139 class decoder:
140  def __init__(self, inactivity_timeout = float(1e3000), continuous_motion_output = False):
141  #buttons=[uinput.BTN_SELECT, uinput.BTN_THUMBL, uinput.BTN_THUMBR, uinput.BTN_START,
142  # uinput.BTN_FORWARD, uinput.BTN_RIGHT, uinput.BTN_BACK, uinput.BTN_LEFT,
143  # uinput.BTN_TL, uinput.BTN_TR, uinput.BTN_TL2, uinput.BTN_TR2,
144  # uinput.BTN_X, uinput.BTN_A, uinput.BTN_B, uinput.BTN_Y,
145  # uinput.BTN_MODE]
146  #axes=[uinput.ABS_X, uinput.ABS_Y, uinput.ABS_Z, uinput.ABS_RX,
147  # uinput.ABS_RX, uinput.ABS_RY, uinput.ABS_PRESSURE, uinput.ABS_DISTANCE,
148  # uinput.ABS_THROTTLE, uinput.ABS_RUDDER, uinput.ABS_WHEEL, uinput.ABS_GAS,
149  # uinput.ABS_HAT0Y, uinput.ABS_HAT1Y, uinput.ABS_HAT2Y, uinput.ABS_HAT3Y,
150  # uinput.ABS_TILT_X, uinput.ABS_TILT_Y, uinput.ABS_MISC, uinput.ABS_RZ,
151  # ]
152  buttons = range(0x100,0x111)
153  axes = range(0, 20)
154  axmin = [0] * 20
155  axmax = [255] * 20
156  axfuzz = [2] * 20
157  axflat = [4] * 20
158  for i in range(-4,0): # Gyros have more bits than other axes
159  axmax[i] = 1023
160  axfuzz[i] = 4
161  axflat[i] = 4
162  if continuous_motion_output:
163  axfuzz[i] = 0
164  axflat[i] = 0
165  for i in range(4,len(axmin)-4): # Buttons should be zero when not pressed
166  axmin[i] = -axmax[i]
167  self.joy = uinputjoy(buttons, axes, axmin, axmax, axfuzz, axflat)
168  self.axmid = [sum(pair)/2 for pair in zip(axmin, axmax)]
169  self.fullstop() # Probably useless because of uinput startup bug
170  self.outlen = len(buttons) + len(axes)
171  self.inactivity_timeout = inactivity_timeout
172 
173  step_active = 1
174  step_idle = 2
175  step_error = 3
176 
177  def step(self, rawdata): # Returns true if the packet was legal
178  if len(rawdata) == 50:
179  joy_coding = "!1B2x3B1x4B4x12B15x4H"
180  data = list(struct.unpack(joy_coding, rawdata))
181  prefix = data.pop(0)
182  if prefix != 161:
183  print >> sys.stderr, "Unexpected prefix (%i). Is this a PS3 Dual Shock or Six Axis?"%prefix
184  return self.step_error
185  out = []
186  for j in range(0,2): # Split out the buttons.
187  curbyte = data.pop(0)
188  for k in range(0,8):
189  out.append(int((curbyte & (1 << k)) != 0))
190  out = out + data
191  self.joy.update(out)
192  axis_motion = [abs(out[17:][i] - self.axmid[i]) > 20 for i in range(0,len(out)-17-4)]
193  # 17 buttons, 4 inertial sensors
194  if any(out[0:17]) or any(axis_motion):
195  return self.step_active
196  return self.step_idle
197  elif len(rawdata) == 13:
198  #print list(rawdata)
199  print >> sys.stderr, "Your bluetooth adapter is not supported. Does it support Bluetooth 2.0? Please report its model to blaise@willowgarage.com"
200  raise BadJoystickException()
201  else:
202  print >> sys.stderr, "Unexpected packet length (%i). Is this a PS3 Dual Shock or Six Axis?"%len(rawdata)
203  return self.step_error
204 
205  def fullstop(self):
206  self.joy.update([0] * 17 + self.axmid)
207 
208  def run(self, intr, ctrl):
209  activated = False
210  try:
211  self.fullstop()
212  lastactivitytime = lastvalidtime = time.time()
213  while True:
214  (rd, wr, err) = select.select([intr], [], [], 0.1)
215  curtime = time.time()
216  if len(rd) + len(wr) + len(err) == 0: # Timeout
217  #print "Activating connection."
218  ctrl.send("\x53\xf4\x42\x03\x00\x00") # Try activating the stream.
219  else: # Got a frame.
220  #print "Got a frame at ", curtime, 1 / (curtime - lastvalidtime)
221  if not activated:
222  print "Connection activated"
223  activated = True
224  try:
225  rawdata = intr.recv(128)
226  except BluetoothError, s:
227  print "Got Bluetooth error %s. Disconnecting."%s
228  return
229  if len(rawdata) == 0: # Orderly shutdown of socket
230  print "Joystick shut down the connection, battery may be discharged."
231  return
232  stepout = self.step(rawdata)
233  if stepout != self.step_error:
234  lastvalidtime = curtime
235  if stepout == self.step_active:
236  lastactivitytime = curtime
237  if curtime - lastactivitytime > self.inactivity_timeout:
238  print "Joystick inactive for %.0f seconds. Disconnecting to save battery."%self.inactivity_timeout
239  return
240  if curtime - lastvalidtime >= 0.1: # Zero all outputs if we don't hear a valid frame for 0.1 to 0.2 seconds
241  self.fullstop()
242  if curtime - lastvalidtime >= 5: # Disconnect if we don't hear a valid frame for 5 seconds
243  print "No valid data for 5 seconds. Disconnecting. This should not happen, please report it."
244  return
245  time.sleep(0.005) # No need to blaze through the loop when there is an error
246  finally:
247  self.fullstop()
248 
249 class Quit(Exception):
250  def __init__(self, errorcode):
251  Exception.__init__(self)
252  self.errorcode = errorcode
253 
255  # Check if hci0 is up and pscanning, take action as necessary.
256  proc = subprocess.Popen(['hciconfig'], stdout=subprocess.PIPE, stderr=subprocess.PIPE)
257  (out, err) = proc.communicate()
258  if out.find('UP') == -1:
259  os.system("hciconfig hci0 up > /dev/null 2>&1")
260  if out.find('PSCAN') == -1:
261  os.system("hciconfig hci0 pscan > /dev/null 2>&1")
262 
264  def __init__(self, decoder):
265  self.decoder = decoder
266  self.shutdown = False
267 
268  def prepare_bluetooth_socket(self, port):
269  sock = BluetoothSocket(L2CAP)
270  return self.prepare_socket(sock, port)
271 
272  def prepare_net_socket(self, port):
273  sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
274  return self.prepare_socket(sock, port)
275 
276  def prepare_socket(self, sock, port):
277  first_loop = True
278  while True:
279  try:
280  sock.bind(("", port))
281  except Exception, e:
282  print repr(e)
283  if first_loop:
284  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."
285  first_loop = False
286  time.sleep(0.5)
287  continue
288  sock.listen(1)
289  return sock
290 
291  def listen_net(self,intr_port, ctrl_port):
292  intr_sock = self.prepare_net_socket(intr_port)
293  ctrl_sock = self.prepare_net_socket(ctrl_port)
294  self.listen(intr_sock, ctrl_sock)
295 
296  def listen_bluetooth(self):
297  intr_sock = self.prepare_bluetooth_socket(L2CAP_PSM_HIDP_INTR)
298  ctrl_sock = self.prepare_bluetooth_socket(L2CAP_PSM_HIDP_CTRL)
299  self.listen(intr_sock, ctrl_sock)
300 
301  def listen(self, intr_sock, ctrl_sock):
302  self.n = 0
303  while not self.shutdown:
304  print "Waiting for connection. Disconnect your PS3 joystick from USB and press the pairing button."
305  try:
306  intr_sock.settimeout(5)
307  ctrl_sock.settimeout(1)
308  while True:
309  try:
310  (intr, (idev, iport)) = intr_sock.accept();
311  break
312  except Exception, e:
313  if str(e) == 'timed out':
315  else:
316  raise
317 
318  try:
319  try:
320  (ctrl, (cdev, cport)) = ctrl_sock.accept();
321  except Exception, e:
322  print >> sys.stderr, "Got interrupt connection without control connection. Giving up on it."
323  continue
324  try:
325  if idev == cdev:
326  self.decoder.run(intr, ctrl)
327  print "Connection terminated."
328  else:
329  print >> sys.stderr, "Simultaneous connection from two different devices. Ignoring both."
330  finally:
331  ctrl.close()
332  finally:
333  intr.close()
334  except BadJoystickException:
335  pass
336  except KeyboardInterrupt:
337  print "CTRL+C detected. Exiting."
338  quit(0)
339  except Exception, e:
340  traceback.print_exc()
341  print >> sys.stderr, "Caught exception: %s"%str(e)
342  time.sleep(1)
343  print
344 
345 inactivity_timout_string = "--inactivity-timeout"
346 no_disable_bluetoothd_string = "--no-disable-bluetoothd"
347 redirect_output_string = "--redirect-output"
348 continuous_motion_output_string = "--continuous-output"
349 
350 def usage(errcode):
351  print "usage: ps3joy.py ["+inactivity_timout_string+"=<n>] ["+no_disable_bluetoothd_string+"] ["+redirect_output_string+"] ["+continuous_motion_output_string+"]=<f>"
352  print "<n>: inactivity timeout in seconds (saves battery life)."
353  print "<f>: file name to redirect output to."
354  print "Unless "+no_disable_bluetoothd_string+" is specified, bluetoothd will be stopped."
355  raise Quit(errcode)
356 
357 def is_arg_with_param(arg, prefix):
358  if not arg.startswith(prefix):
359  return False
360  if not arg.startswith(prefix+"="):
361  print "Expected '=' after "+prefix
362  print
363  usage(1)
364  return True
365 
366 if __name__ == "__main__":
367  errorcode = 0
368  try:
369  # Get Root Privileges
370  euid = os.geteuid()
371  if euid != 0:
372  args = ['sudo', sys.executable] + sys.argv + [os.environ]
373  os.execlpe('sudo', *args)
374  if euid != 0:
375  raise SystemExit("Root Privlages Required.")
376 
377 
378  inactivity_timeout = float(1e3000)
379  disable_bluetoothd = True
380  continuous_output = False
381  for arg in sys.argv[1:]: # Be very tolerant in case we are roslaunched.
382  if arg == "--help":
383  usage(0)
384  elif is_arg_with_param(arg, inactivity_timout_string):
385  str_value = arg[len(inactivity_timout_string)+1:]
386  try:
387  inactivity_timeout = float(str_value)
388  if inactivity_timeout < 0:
389  print "Inactivity timeout must be positive."
390  print
391  usage(1)
392  except ValueError:
393  print "Error parsing inactivity timeout: "+str_value
394  print
395  usage(1)
396  elif arg == no_disable_bluetoothd_string:
397  disable_bluetoothd = False
398  elif arg == continuous_motion_output_string:
399  continuous_output = True
400  elif is_arg_with_param(arg, redirect_output_string):
401  str_value = arg[len(redirect_output_string)+1:]
402  try:
403  print "Redirecting output to:", str_value
404  sys.stdout = open(str_value, "a", 1)
405  except IOError, e:
406  print "Error opening file to redirect output:", str_value
407  raise Quit(1)
408  sys.stderr = sys.stdout
409  else:
410  print "Ignoring parameter: '%s'"%arg
411 
412  if disable_bluetoothd:
413  os.system("/etc/init.d/bluetooth stop > /dev/null 2>&1")
414  time.sleep(1) # Give the socket time to be available.
415  try:
416  while os.system("hciconfig hci0 > /dev/null 2>&1") != 0:
417  print >> sys.stderr, "No bluetooth dongle found or bluez rosdep not installed. Will retry in 5 seconds."
418  time.sleep(5)
419  if inactivity_timeout == float(1e3000):
420  print "No inactivity timeout was set. (Run with --help for details.)"
421  else:
422  print "Inactivity timeout set to %.0f seconds."%inactivity_timeout
423  cm = connection_manager(decoder(inactivity_timeout = inactivity_timeout, continuous_motion_output = continuous_output))
424  cm.listen_bluetooth()
425  finally:
426  if disable_bluetoothd:
427  os.system("/etc/init.d/bluetooth start > /dev/null 2>&1")
428  except Quit, e:
429  errorcode = e.errorcode
430  except KeyboardInterrupt:
431  print "CTRL+C detected. Exiting."
432  exit(errorcode)
def listen_bluetooth(self)
Definition: ps3joy.py:296
def is_arg_with_param(arg, prefix)
Definition: ps3joy.py:357
def prepare_socket(self, sock, port)
Definition: ps3joy.py:276
int step_active
Definition: ps3joy.py:173
def __init__(self, decoder)
Definition: ps3joy.py:264
int step_idle
Definition: ps3joy.py:174
def __init__(self, errorcode)
Definition: ps3joy.py:250
def step(self, rawdata)
Definition: ps3joy.py:177
def prepare_net_socket(self, port)
Definition: ps3joy.py:272
def prepare_bluetooth_socket(self, port)
Definition: ps3joy.py:268
def update(self, value)
Definition: ps3joy.py:123
def listen(self, intr_sock, ctrl_sock)
Definition: ps3joy.py:301
def usage(errcode)
Definition: ps3joy.py:350
def open_uinput(self)
Definition: ps3joy.py:56
def check_hci_status()
Definition: ps3joy.py:254
def __init__(self, inactivity_timeout=float(1e3000), continuous_motion_output=False)
Definition: ps3joy.py:140
def listen_net(self, intr_port, ctrl_port)
Definition: ps3joy.py:291
def fullstop(self)
Definition: ps3joy.py:205
def run(self, intr, ctrl)
Definition: ps3joy.py:208
def __init__(self, buttons, axes, axmin, axmax, axfuzz, axflat)
Definition: ps3joy.py:66
int step_error
Definition: ps3joy.py:175


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