class DemoProcessor(object):
global p
def __init__(self, goal_process):
self.goal_process = goal_process
self.x = YamlExtractor(self.goal_process)
self.topics = self.x.get_topics()
self.ids = self.x.get_bond_ids()
self.bond = None
self.bond_list = []
self.gen_bonds()
return
def get_demo_cmd(self):
return self.x.get_command()
def gen_bonds(self):
# Generate bond instances and store in a list
for i in range(len(self.topics)):
self.bond = bondpy.Bond(self.topics[i], self.ids[i])
self.bond_list.append(self.bond)
#Start each bond and set callbacks
for j in range(len(self.bond_list)):
self.bond_list[j].start()
self.bond_list[j].set_broken_callback(self.on_broken_cb)
self.bond_list[j].set_formed_callback(self.on_formed_cb)
#self.bond_list[j].wait_until_formed()
print "Done with bond settings"
return
def on_formed_cb(self):
print "Bond has been formed"
def on_broken_cb(self):
print "Bond has broken"
if p is not None:
nxr.terminate_process_and_children(self.p)
self.termination_publisher()
else:
print "There is something wrong with p!!!"
return
def termination_publisher(self):
pub = rospy.Publisher("termination_status", String, queue_size=3)
r = rospy.Rate(10)
start_time = rospy.Time.now().secs
while rospy.Time.now().secs - start_time < 1:
msg = 'Demo terminated!'
pub.publish(msg)
r.sleep()
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 __init__(self): self.goal_process = 'test1' self.x = YamlExtractor(self.goal_process) #self.bond = None self.bond_id = self.x.get_bond_ids() self.topic = self.x.get_topics() print self.topic, self.bond_id self.cmd = self.x.get_command() self.gen_infinity_bond(subprocess.Popen(self.cmd))
def __init__(self, goal_process): self.goal_process = goal_process self.x = YamlExtractor(self.goal_process) self.topics = self.x.get_topics() self.ids = self.x.get_bond_ids() self.bond = None self.bond_list = [] self.gen_bonds() return
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
class InfinityBonder(object): def __init__(self): self.goal_process = 'test1' self.x = YamlExtractor(self.goal_process) #self.bond = None self.bond_id = self.x.get_bond_ids() self.topic = self.x.get_topics() print self.topic, self.bond_id self.cmd = self.x.get_command() self.gen_infinity_bond(subprocess.Popen(self.cmd)) def gen_infinity_bond(self, p): self.bond = bondpy.Bond(self.topic, self.bond_id) self.bond.start() self.bond.set_broken_callback(self.on_broken_cb) self.bond.set_formed_callback(self.on_formed_cb) self.bond.wait_until_formed() def on_formed_cb(self): print "Bond has been formed" def on_broken_cb(self): print "Bond has been broken"
