# 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__), 'all_bins_order.json')))
master = Master(apc_order)
from time import sleep
for request in master.order:
print ' {}: {}'.format(request['bin'], request['item'])
task = master.manager.control.update()
while not task.done: sleep(0.1)
master._move_initial()
while master.order:
request = master.order.pop(0)
master._set_target(request['bin'], request['item'])
master._move_vantage_point(master.knowledge_base.bin_vantage_points[request['bin']][0])
master._localize_object(master.knowledge_base.bin_vantage_points[request['bin']][0])
task = master.manager.control.update()
while not task.done: sleep(0.1)
cmd = parts[0]
args = parts[1:]
if cmd == 'active' and len(args) == 1:
master.knowledge_base.active_limb = args[0]
elif cmd == 'target':
if len(args) == 2:
print master._set_target(args[0], args[1])
elif len(args) == 1:
print master._set_target(args[0], filter(lambda o: o['bin'] == args[0], master.order)[0]['item'])
elif cmd == 'move' and len(args) >= 1:
if args[0] == 'initial':
print master._move_initial()
sleep(2)
elif args[0] == 'vantage' and len(args) >= 2:
print master._move_vantage_point(args[1])
sleep(2)
elif args[0] == 'withdraw' and len(args) >= 1:
name = args[1] if len(args) >= 2 else master.knowledge_base.target_bin
limb = args[2] if len(args) >= 3 else master.knowledge_base.active_limb
print master.manager.moveToXform(se3.mul(master.knowledge_base.shelf_xform, master.knowledge_base.withdraw_point_xforms[name]), limb)
sleep(2)
elif args[0] == 'pickTray':
print master.manager.PickUpOrderTray(True)
elif args[0] == 'placeTray':
print master.manager.PickUpOrderTray(False)
elif args[0] == 'moveTray' and len(args) > 1:
if(args[1].startswith('bin')):