def _handle_run_program(self, req):
print 'Running program...'
input = StringIO(req.code)
storage = SimpleFormat(input)
count = storage.deserialize_data()
assert(count > 0)
self._sequences = []
for index in range(count):
sequence = ActionSequence()
sequence.deserialize(storage)
print 'Loaded sequence %d with %d poses' % (index + 1, len(sequence.actions()))
sequence.executing_action_signal.connect(self._progress)
sequence.execute_sequence_finished_signal.connect(self._finished)
self._sequences.append(sequence)
self._event = Event()
# wait until default pose has finished
print 'Execute default pose'
self._default_pose.execute_finished_signal.connect(self._event.set)
self._default_pose.execute()
self._event.wait()
self._default_pose.execute_finished_signal.disconnect(self._event.set)
# enable interactive mode
print 'Enable interactive mode'
self._event.clear()
self._default_pose.execute_finished_signal.connect(self._finished)
self._ps3_subscriber.buttons_changed.connect(self._check_ps3_buttons)
self._event.wait()
self._default_pose.execute_finished_signal.disconnect(self._finished)
self._ps3_subscriber.buttons_changed.disconnect(self._check_ps3_buttons)
self._stop_current_sequence()
for sequence in self._sequences:
sequence.executing_action_signal.disconnect(self._progress)
sequence.execute_sequence_finished_signal.disconnect(self._finished)
self._sequences = []
print 'Running program finished'
return CallProgramResponse(0, "Slider Program Successful")
import roslib; roslib.load_manifest('puppet_gui')
from puppet_gui.actions.ActionSequence import ActionSequence
from puppet_gui.actions.ActionSet import ActionSet
from puppet_gui.actions.Pr2MoveHeadAction import Pr2MoveHeadAction
from puppet_gui.actions.Pr2MoveLeftArmAction import Pr2MoveLeftArmAction
from puppet_gui.actions.Pr2MoveLeftGripperAction import Pr2MoveLeftGripperAction
from puppet_gui.actions.Pr2MoveRightArmAction import Pr2MoveRightArmAction
from puppet_gui.actions.Pr2MoveTorsoAction import Pr2MoveTorsoAction
from puppet_gui.actions.WaitAction import WaitAction
import rospy
rospy.init_node('test_actions')
seq = ActionSequence()
action1 = Pr2MoveLeftArmAction()
action1.set_values([0, 0, 0, 0, 0, 0, 0])
seq.add_action(action1)
action2 = Pr2MoveLeftArmAction()
action2.set_values([0, 88, 0, -90, 0, 0, 90])
seq.add_action(action2)
action3 = Pr2MoveRightArmAction()
action3.set_values([0, 0, 0, 0, 0, 0, 0])
seq.add_action(action3)
action4 = Pr2MoveRightArmAction()
action4.set_values([0, 88, 0, -90, 0, 0, 90])
