def execute(self, userdata): rd_goal = userdata.goal_from_ui rd_result = userdata.result_for_ui if rd_goal.ui_command.command == bdm.GetUserCommand.MOVE_TO_HOME_POSITION or \ rd_goal.ui_command.command == bdm.GetUserCommand.MOVE_TO_IDLE_MODE or \ rd_goal.ui_command.command == bdm.GetUserCommand.CANCEL_DEMO: return 'different_request' else: global p x = YamlExtractor(rd_goal.ui_command.command) if x.get_name() is not None: topics = x.get_topics() ids = x.get_bond_ids() bond_list = [] # Generate bond instances for i in range(len(topics)): bond = bondpy.Bond(topics[i], ids[i]) bond_list.append(bond) ui_cmd = x.get_command() if p is None: p = subprocess.Popen(ui_cmd) rd_result.sys_result.result = bdm.GetResult.DEMO_EXECUTED def on_formed_cb(): print "Bond has been formed" def on_broken_cb(): print "Bond has been broken" if p is not None: nxr.terminate_process_and_children(p) #termination_publisher() for j in range(len(bond_list)): bond_list[j].start() bond_list[j].set_formed_callback(on_formed_cb) bond_list[j].set_broken_callback(on_broken_cb) bond_list[j].wait_until_formed() return 'demo_executed' else: rd_result.sys_result.result = bdm.GetResult.IS_ABORTED return 'aborted'
def get_selection_cb(userdata, goal): print " d1 : joint velocity wobbler \n d2 : joint velocity puppet\n d3 : joint position keyboard\n d4 : head wobbler\n d5 : gripper cuff control\n d6: joint position joystick \n test1 : turtle_sim infinity drawer\n test2 : turtle_sim circle drawer\n test3 : dummy demo \n" print "Select a demo to run (e.g. d1): " inp, out, err = select.select([sys.stdin], [], [], 15) if inp: usr_inp = sys.stdin.readline().strip() x = YamlExtractor(usr_inp) demo_req = x.get_name() if demo_req is None: print "Undefined input!\n" return get_selection_cb(userdata, goal) else: goal.ui_command.command = demo_req else: goal.ui_command.command = bdm.GetUserCommand.MOVE_TO_IDLE_MODE print "Baxter is moving to idle mode...\n" return goal