def insert_find_face():
model = get_current_model()
action_set = ActionSet()
action = Pr2LookAtFace()
action_set.add_action(action)
action_set.set_duration(main_window.duration_doubleSpinBox.value())
model.add_action(action_set)
class JointObserver(QObject):
current_values_changed = Signal(str)
_update_current_value_signal = Signal()
def __init__(self):
super(JointObserver, self).__init__()
self.action_set = None
self._latest_joint_state = None
self._update_current_value_signal.connect(self._update_current_value)
self._subscriber = None
self._trajectory_locks = []
self._trajectory_locks.append(TrajectoryLock("head_traj_controller_loose", [0.08, 0.08]))
self._trajectory_locks.append(
TrajectoryLock("l_arm_controller_loose", [0.02, 0.02, 0.02, 0.02, 0.02, 0.06, 0.06])
)
self._trajectory_locks.append(
TrajectoryLock("r_arm_controller_loose", [0.02, 0.02, 0.02, 0.02, 0.02, 0.06, 0.06])
)
def start(self):
print "JointObserver.start()"
if self._subscriber is not None:
self._subscriber.unregister()
self._subscriber = rospy.Subscriber("/joint_states", JointState, self._receive_joint_states)
for trajectory_lock in self._trajectory_locks:
trajectory_lock.start()
def stop(self):
print "JointObserver.stop()"
if self._subscriber is not None:
self._subscriber.unregister()
self._subscriber = None
for trajectory_lock in self._trajectory_locks:
trajectory_lock.stop()
def _receive_joint_states(self, joint_state_msg):
self._latest_joint_state = joint_state_msg
self._update_current_value_signal.emit()
def _update_current_value(self):
self.action_set = ActionSet()
self.action_set.add_action(Pr2MoveHeadAction())
self.action_set.add_action(Pr2MoveLeftArmAction())
self.action_set.add_action(Pr2MoveRightArmAction())
labels = self.action_set.get_labels()
for label in labels:
value = self._latest_joint_state.position[self._latest_joint_state.name.index(label)]
value *= 180.0 / math.pi
try:
self.action_set.update_value(label, value)
except KeyError, e:
print 'JointObserver._update_current_value() label "%s" not found' % label
pass
value = self.action_set.to_string()
self.current_values_changed.emit(value)
def add_default_pose():
model = get_current_model()
action_set = ActionSet()
action = DefaultAction()
action_set.add_action(action)
action_set.set_duration(main_window.duration_doubleSpinBox.value())
model.add_action(action_set)
table_view = get_table_view(get_current_tab_index())
rows = len(model.action_sequence().actions())
table_view.resizeColumnsToContents()
table_view.selectRow(rows - 1)
def insert_speak(whatToSay, voiceToUse, ttsEngine):
model = get_current_model()
action_set = ActionSet()
shouldBlock = main_window.blockingSpeechCheckbox.isChecked()
try:
action = Pr2SpeakAction(
Toolbox.getInstance(), whatToSay, voice=voiceToUse, ttsEngine=ttsEngine, waitForSpeechDone=shouldBlock
)
except ValueError as e:
self.dialogService.showErrorMsg(str(e))
return
action_set.add_action(action)
model.add_action(action_set)
def _update_current_value(self):
self.action_set = ActionSet()
self.action_set.add_action(Pr2MoveHeadAction())
self.action_set.add_action(Pr2MoveLeftArmAction())
self.action_set.add_action(Pr2MoveRightArmAction())
labels = self.action_set.get_labels()
for label in labels:
value = self._latest_joint_state.position[self._latest_joint_state.name.index(label)]
value *= 180.0 / math.pi
try:
self.action_set.update_value(label, value)
except KeyError, e:
print 'JointObserver._update_current_value() label "%s" not found' % label
pass
action5 = WaitAction(3.0) seq.add_action(action5) action6 = Pr2MoveLeftGripperAction() action6.set_value(0.0) seq.add_action(action6) action7 = Pr2MoveLeftGripperAction() action7.set_value(0.08) seq.add_action(action7) action8 = Pr2MoveHeadAction() action8.set_values([-60, -30]) action9 = Pr2MoveTorsoAction() action9.set_values([0.0]) set1 = ActionSet() set1.add_action(action8) set1.add_action(action9) seq.add_action(set1) action10 = Pr2MoveHeadAction() action10.set_values([60, 30]) action11 = Pr2MoveTorsoAction() action11.set_values([0.3]) set2 = ActionSet() set2.add_action(action10) set2.add_action(action11) seq.add_action(set2) action5 = WaitAction(3.0) seq.add_action(action5)
