148 lines
3.5 KiB
Python
Executable File
148 lines
3.5 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
|
|
import get_specs
|
|
import traceback
|
|
import logging
|
|
import yaml
|
|
from multiprocessing import Process, Manager, Pool, TimeoutError, active_children, log_to_stderr
|
|
from multiprocessing.pool import Pool
|
|
import multiprocessing
|
|
from time import sleep
|
|
from util import fprint
|
|
from util import run_cmd
|
|
import sys
|
|
import ur5_control
|
|
import os
|
|
import signal
|
|
|
|
|
|
|
|
config = None
|
|
keeprunning = True
|
|
arm_ready = False
|
|
led_ready = False
|
|
killme = None
|
|
#pool = None
|
|
|
|
|
|
def arm_start_callback(res):
|
|
global arm_ready
|
|
arm_ready = True
|
|
|
|
def led_start_callback(res):
|
|
global led_ready
|
|
led_ready = True
|
|
|
|
def cam_start_callback(res):
|
|
global cam_ready
|
|
cam_ready = True
|
|
|
|
def wait_for(val, name):
|
|
if val is False:
|
|
fprint("waiting for " + name + " to complete...")
|
|
while val is False:
|
|
sleep(0.1)
|
|
|
|
def setup(pool):
|
|
global config
|
|
global counter
|
|
fprint("Starting Jukebox control system...")
|
|
pool.apply_async(ur5_control.init, (config["arm"]["ip"],), callback=arm_start_callback)
|
|
#pool.apply_async(led_control.init, callback=led_start_callback)
|
|
#pool.apply_async(cam_control.init, callback=led_start_callback)
|
|
|
|
wait_for(led_ready, "LED controller initialization")
|
|
wait_for(arm_ready, "UR5 initilization")
|
|
wait_for(cam_ready, "Camera initilization")
|
|
|
|
return True
|
|
|
|
|
|
|
|
def mainloop(pool):
|
|
global config
|
|
global counter
|
|
global killme
|
|
|
|
if killme.value > 0:
|
|
killall()
|
|
counter = counter + 1
|
|
|
|
class Logger(object):
|
|
def __init__(self, filename="output.log"):
|
|
self.log = open(filename, "a")
|
|
self.terminal = sys.stdout
|
|
|
|
def write(self, message):
|
|
self.log.write(message)
|
|
#close(filename)
|
|
#self.log = open(filename, "a")
|
|
try:
|
|
self.terminal.write(message)
|
|
except:
|
|
sleep(0)
|
|
|
|
def flush(self):
|
|
print("",end="")
|
|
|
|
|
|
|
|
|
|
def killall():
|
|
procs = active_children()
|
|
for proc in procs:
|
|
proc.kill()
|
|
fprint("All child processes killed")
|
|
os.kill(os.getpid(), 9) # dirty kill of self
|
|
|
|
|
|
def killall_signal(a, b):
|
|
killall()
|
|
|
|
def error(msg, *args):
|
|
return multiprocessing.get_logger().error(msg, *args)
|
|
|
|
class LogExceptions(object):
|
|
def __init__(self, callable):
|
|
self.__callable = callable
|
|
|
|
def __call__(self, *args, **kwargs):
|
|
try:
|
|
result = self.__callable(*args, **kwargs)
|
|
|
|
except Exception as e:
|
|
# Here we add some debugging help. If multiprocessing's
|
|
# debugging is on, it will arrange to log the traceback
|
|
error(traceback.format_exc())
|
|
# Re-raise the original exception so the Pool worker can
|
|
# clean up
|
|
raise
|
|
|
|
# It was fine, give a normal answer
|
|
return result
|
|
|
|
class LoggingPool(Pool):
|
|
def apply_async(self, func, args=(), kwds={}, callback=None):
|
|
return Pool.apply_async(self, LogExceptions(func), args, kwds, callback)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
#sys.stdout = Logger(filename="output.log")
|
|
#sys.stderr = Logger(filename="output.log")
|
|
log_to_stderr(logging.DEBUG)
|
|
|
|
with open('config.yml', 'r') as fileread:
|
|
#global config
|
|
config = yaml.safe_load(fileread)
|
|
|
|
with Manager() as manager:
|
|
pool = LoggingPool(processes=10)
|
|
counter = 0
|
|
killme = manager.Value('d', 0)
|
|
signal.signal(signal.SIGINT, killall_signal)
|
|
if setup(pool):
|
|
while(keeprunning):
|
|
mainloop(pool)
|
|
|
|
|