# 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)