ps3joysim.py
Go to the documentation of this file.
00001 #!/usr/bin/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 from bluetooth import *
00036 import select
00037 import fcntl
00038 import os
00039 import time
00040 import sys                    
00041 import traceback
00042 import threading
00043 import ps3joy
00044 import socket
00045 import signal
00046 
00047 def mk_in_socket():
00048     sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
00049     sock.bind(("127.0.0.1",0))
00050     sock.listen(1)
00051     return sock,sock.getsockname()[1]
00052 
00053 # Class to spawn the ps3joy.py infrastructure in its own thread
00054 class driversim(threading.Thread):
00055     def __init__(self, intr, ctrl):
00056         threading.Thread.__init__(self)
00057         self.intr = intr
00058         self.ctrl = ctrl
00059         self.start()
00060 
00061     def run(self):
00062         self.cm = ps3joy.connection_manager(ps3joy.decoder())
00063         self.cm.listen(self.intr, self.ctrl)
00064         print "driversim exiting"
00065 
00066     def shutdown(self):
00067         self.cm.shutdown = True
00068                     
00069 class joysim(threading.Thread):
00070     def __init__(self, intr, ctrl):
00071         threading.Thread.__init__(self)
00072         print "Starting joystick simulator on ports", intr, "and", ctrl
00073         self.intr = socket.socket()
00074         self.intr.connect(("127.0.0.1", intr))
00075         if self.intr == -1:
00076             raise "Error creating interrput socket."
00077         self.ctrl = socket.socket()
00078         self.ctrl.connect(("127.0.0.1", ctrl))
00079         if self.ctrl == -1:
00080             raise "Error creating control socket."
00081         self.active = False
00082         self.shutdown = False
00083         self.start()
00084 
00085     def run(self):
00086         while not self.active and not self.shutdown:
00087             (rd, wr, err) = select.select([self.ctrl], [], [], 1)
00088             if len(rd) == 1:
00089                 cmd = self.ctrl.recv(128)
00090                 if cmd == "\x53\xf4\x42\x03\x00\x00":
00091                     self.active = True
00092                     print "Got activate command"
00093                 else:
00094                     print "Got unknown command (len=%i)"%len(cmd),
00095                     time.sleep(1);
00096                     for c in cmd:
00097                         print "%x"%ord(c),
00098                     print
00099         print "joyactivate exiting"
00100 
00101     def publishstate(self, ax, butt):
00102         if self.active:
00103             ranges = [255] * 17 + [8191] * 20
00104             axval = [ int((v + 1) * s / 2) for (v,s) in zip(ax, ranges)]
00105             buttout = []
00106             for i in range(0,2):
00107                 newval = 0
00108                 for j in range(0,8):
00109                     newval = (newval << 1)
00110                     if butt[i * 8 + j]:
00111                         newval = newval + 1
00112                 buttout.append(newval)
00113             joy_coding = "!1B2x3B1x4B4x12B15x4H"
00114             #print buttout, axval
00115             self.intr.send(struct.pack(joy_coding, 161, *(buttout + [0] + axval)))
00116         else:
00117             print "Tried to publish while inactive"
00118 
00119 if __name__ == "__main__":
00120     def stop_all_threads(a, b):
00121       #ds.shutdown()
00122       #js.shutdown = True
00123       #shutdown = True
00124       exit(0)
00125 
00126     signal.signal(signal.SIGINT, stop_all_threads)
00127 
00128     # Create sockets for the driver side and pass them to the driver
00129     (intr_in, intr_port) = mk_in_socket()
00130     (ctrl_in, ctrl_port) = mk_in_socket()
00131 
00132     ds = driversim(intr_in, ctrl_in)
00133    
00134     # Give the simulator a chance to get going
00135     time.sleep(2)
00136     
00137     # Call up the simulator telling it which ports to connect to.
00138     js = joysim(intr_port, ctrl_port)
00139     buttons1 = [True] * 16
00140     axes1 = [1, 0, -1, .5] * 5
00141     buttons2 = [False] * 16
00142     axes2 = [-1] * 20
00143     buttons3 = [False] * 16
00144     axes3 = [0] * 20
00145     shutdown = False
00146     while not js.active and not shutdown:
00147         time.sleep(0.2)
00148     #js.publishstate(axes2, buttons2)
00149     time.sleep(0.01)
00150     #js.publishstate(axes3, buttons3)
00151     time.sleep(0.01)
00152     while not shutdown:
00153         js.publishstate(axes1, buttons2)
00154         time.sleep(0.01)
00155         axes1[0] = -axes1[0]
00156         js.publishstate(axes2, buttons2)
00157         time.sleep(0.01)
00158        # js.publishstate(axes3, buttons3)
00159        # time.sleep(0.01)
00160     
00161     print "main exiting"


ps3joy
Author(s): Blaise Gassend, pascal@pabr.org, Melonee Wise
autogenerated on Sat Jun 8 2019 20:54:40