# 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)
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 == 'localize' and len(args) >= 1:
if args[0] in [ 'shelf', 'order_bin', 'both' ]:
print master._localize_shelf_and_order_bin()
elif args[0] == 'object':
print master._localize_object('unused')
elif cmd == 'robot' and len(args) == 1:
if args[0] == 'enable':
os.system('rosrun baxter_tools enable_robot.py -e')
elif args[0] == 'disable':
os.system('rosrun baxter_tools enable_robot.py -d')
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)
