xform[1]
)
print so3.matrix(master.knowledge_base.shelf_xform[0]), master.knowledge_base.shelf_xform[1]
elif cmd == 'reset' and len(args) == 1:
if args[0] == 'control':
print master.manager._restart_control()
elif args[0] == 'order':
print master._load_apc_order(apc_order)
elif args[0] == 'kb':
print master._create_knowledge_base()
print master._load_apc_order(apc_order)
elif cmd == 'run' and len(args) == 2:
print master.run(args[0], args[1])
for request in master.order:
print ' {}: {}'.format(request['bin'], request['item'])
elif cmd in [ 'quit', 'exit' ]:
break
else:
print "Invalid command",cmd
except Exception as e:
print 'excution error:', e
print traceback.format_exc()
task = master.manager.control.update()
while not task.done: sleep(0.1)
logging.getLogger('integration.io.pcd').setLevel(logging.WARNING)
logging.getLogger('integration.camera.client').setLevel(logging.WARNING)
logging.getLogger('integration.camera.packet').setLevel(logging.WARNING)
logging.getLogger('integration.interface.fake').setLevel(logging.WARNING)
logging.getLogger('integration.control_server').setLevel(logging.WARNING)
logging.getLogger('integration.master').setLevel(logging.INFO)
logging.getLogger('perception.shelf').setLevel(logging.WARNING)
logging.getLogger('perception.icp').setLevel(logging.WARNING)
logging.getLogger('perception.blob').setLevel(logging.WARNING)
# allow importing from the repository root
import sys, os
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from integration.master import Master
import json
apc_order = json.load(open(os.path.join(os.path.dirname(__file__), 'test_shelf.json')))
# speed up the tests
from integration.interface import fake
fake.delay_scale = 1
fake.fail_scale = 100
m = Master(apc_order)
while m.order:
request = m.order.pop(0)
if not m.run(request['bin'], request['item']):
m.order.append(request)
