def __init__(self, editable=False):
super(PosesDataModel, self).__init__()
self._action_sequence = ActionSequence()
self._editable = editable
self._joint_columns = {}
action = DefaultAction()
self._add_joint(1, action, 'head_pan_joint', 'Hd Turn')
self._add_joint(2, action, 'head_tilt_joint', 'Hd Nod')
self._add_joint(3, action, 'torso_lift_joint', 'Torso', 2)
self._add_joint(4, action, 'l_shoulder_pan_joint', 'Lp1')
self._add_joint(5, action, 'l_shoulder_lift_joint', 'Ll2')
self._add_joint(6, action, 'l_upper_arm_roll_joint', 'Lu3')
self._add_joint(7, action, 'l_elbow_flex_joint', 'Le4')
self._add_joint(8, action, 'l_forearm_roll_joint', 'Lf5')
self._add_joint(9, action, 'l_wrist_flex_joint', 'Lw6')
self._add_joint(10, action, 'l_wrist_roll_joint', 'Lr7')
self._add_joint(11, action, 'l_gripper', 'LGrip', 3)
self._add_joint(12, action, 'r_shoulder_pan_joint', 'Rp1')
self._add_joint(13, action, 'r_shoulder_lift_joint', 'Rl2')
self._add_joint(14, action, 'r_upper_arm_roll_joint', 'Ru3')
self._add_joint(15, action, 'r_elbow_flex_joint', 'Re4')
self._add_joint(16, action, 'r_forearm_roll_joint', 'Rf5')
self._add_joint(17, action, 'r_wrist_flex_joint', 'Rw6')
self._add_joint(18, action, 'r_wrist_roll_joint', 'Rr7')
self._add_joint(19, action, 'r_gripper', 'RGrip', 3)
# keep reference to delegates to prevent being garbaged
self._delegates = []
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('slider_gui')
from actions.ActionSequence import ActionSequence
from actions.ActionSet import ActionSet
from actions.Pr2MoveHeadAction import Pr2MoveHeadAction
from actions.Pr2MoveLeftArmAction import Pr2MoveLeftArmAction
from actions.Pr2MoveLeftGripperAction import Pr2MoveLeftGripperAction
from actions.Pr2MoveRightArmAction import Pr2MoveRightArmAction
from actions.Pr2MoveTorsoAction import Pr2MoveTorsoAction
from 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])
