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)