def main_loop(self):
# Set up a shared queue to put human angles into
input_queue = Queue()
# Get the process for the input method and start it
input_proc = self.joint_input.launch(input_queue)
input_proc.start()
# Set up the device queue to push data into
device_queue = Queue()
device_proq = self.device.launch(device_queue)
device_proq.start()
# Set the initial
joints = {}
joint_angles = [1, 1]
while True:
# Check if there's a new human joint inputs ready
# if not input_queue.empty():
joints = input_queue.get()
if joints['Lwri']:
joint_angles = self.solver.solve(joints['Lwri']['pc'])
joint = ArmJoints(joint_angles[0], joint_angles[1], 0.0)
device_queue.push(joint)
class PQueue:
def __init__(task: Callable):
self._lock = RLock()
self._active = 0
self._done = False
self._q_task = Queue()
self._q_result = Queue()
self._pool = None
self._task = task
def push(item):
with self._lock:
self._q_task.push(item)
def generate():
self._q_result.clear()
self._done = False
self._active = 0
cpu = cpu_count()
self._pool = Pool(cpu)
def run_tasks():
while not self._done:
t = None
with self._lock:
try:
t = self._q_task.get_nowait()
except Queue.Empty:
t = None
if t is not None:
self._active += 1
if t is not None:
for r in self._task(t):
self._q_result.put(r, 100)
with self._lock:
self._active -= 1
for _ in range(cpu):
self._pool.apply_async(run_tasks, ())
while not self._done:
with self._lock:
if not self._active and self._q_task.empty():
self._done = True
while not self._q_result.empty():
r = self._q_result.get(True, 100)
if r is not None:
yield r
self._pool.join()
self._pool.close()
self._pool = None
def get_prices(self):
mgr = Manager()
q = Queue()
processes = []
filenames = os.listdir(self.prices_dir)
count = len(filenames)
thread = threading.Thread(target=self.listener, args=(q, count))
thread.daemon = True # Daemonize thread
thread.start() # Start the execution
for filename in filenames:
p = Process(target=self.worker, args=(q, filename))
p.start()
processes.append(p)
for p in processes:
p.join()
q.push('kill')
return self.prices
class Controller(object):
def __init__(self):
motor = get_motor(motor_debug)
trigger = get_trigger(debug)
self.motor = motor
self.video_tracker = None
print "CONNECTING Motor"
motor.connect()
print "CONNECTING Trigger"
self.trigger = trigger.Trigger()
print "CONNECTING Joystick"
self.init_joystick()
self.manual = False
self.x = None
self.y = None
self.fire_duration = 200
self.video_tracker = None
def _unused_init_detect_process(self):
self.video_in = Queue()
self.video_out = Queue()
self.detect_process = DetectProcess(inq=self.video_in, outq=self.video_out)
self.detect_process.start()
def on_frame(self, b):
ret = inp = numpy.fromstring(b.data, dtype=numpy.uint8).reshape(480, 720, 3)
if not self.video_tracker:
self.video_tracker = VideoTracker(initial_frame=inp, on_cx_cy=self.on_cx_cy)
else:
ret = self.video_tracker.on_frame(inp)
return gst.Buffer(ret)
def on_frame_process(self, b):
ret = inp = numpy.fromstring(b.data, dtype=numpy.uint8).reshape(480, 720, 3)
print "pushing"
self.video_in.push("test")
print "popping"
ret_unused = self.video_out.pop()
assert ret_unused == "testout"
print "popped"
return gst.Buffer(ret)
def init_joystick(self):
self.j = joystick.Joystick()
self.j.connect("axis", self.on_axis)
self.j.connect("button", self.on_button)
def on_axis(self, signal, code, value):
print "on_axis %s %s" % (code, value)
# print "on_axis %d, %d" % (code, value)
if code not in [0, 1, 2]:
return
if code == 0:
self.x = value
if self.manual:
self.motor.theta.set(state.x - 127)
elif code == 1:
self.y = value
if self.manual:
self.motor.pitch.set(state.y - 127)
else:
fire_duration = 100 + (float(value) / 255.0) * 900
print "joy: %d => fire dur %d" % (value, fire_duration)
self.fire_duration = int(fire_duration)
def on_button(self, signal, code, value):
print "on_button: %d, %d" % (code, value)
if value != 1 or code != 0:
return
print " **** FIRE ****"
self.trigger.fire(self.fire_duration)
def on_cx_cy(self, cx, cy):
# print "controller: got (%d, %d)" % (cx, cy)
cx -= 720 / 2
cy -= 480 / 2
for axis, val in [(self.motor.theta, cx), (self.motor.pitch, cy)]:
if abs(val) > 10:
print "action %r" % axis
axis.set(200 if cx > 0 else -200)
