class ArmGUI(Plugin): joint_sig = Signal(JointState) def __init__(self, context): super(ArmGUI, self).__init__(context) self.setObjectName('ArmGUI') self._widget = QWidget() self._widget.setFixedSize(525, 300) self.arm_db = ArmDB() # Action/service/message clients or servers switch_srv_name = 'pr2_controller_manager/switch_controller' rospy.loginfo('Waiting for switch controller service...') rospy.wait_for_service(switch_srv_name) self.switch_service_client = rospy.ServiceProxy(switch_srv_name, SwitchController) self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] self.all_joint_names = [] self.all_joint_poses = [] self.saved_r_arm_pose = None self.saved_l_arm_pose = None self.lock = threading.Lock() rospy.Subscriber('joint_states', JointState, self.joint_states_cb) # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...') self.l_traj_action_client.wait_for_server() QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.joint_sig.connect(self.joint_sig_cb) large_box = QtGui.QVBoxLayout() arm_box = QtGui.QHBoxLayout() right_arm_box = QtGui.QVBoxLayout() left_arm_box = QtGui.QVBoxLayout() left_arm_box.addItem(QtGui.QSpacerItem(50,50)) right_arm_box.addItem(QtGui.QSpacerItem(50,50)) right_arm_box.addWidget(self.create_button('Relax right arm')) right_arm_box.addWidget(self.create_button('Freeze right arm')) left_arm_box.addWidget(self.create_button('Relax left arm')) left_arm_box.addWidget(self.create_button('Freeze left arm')) left_arm_box.addItem(QtGui.QSpacerItem(50,20)) right_arm_box.addItem(QtGui.QSpacerItem(50,20)) left_pose_saver = PoseSaver(PoseSaver.LEFT, self) right_pose_saver = PoseSaver(PoseSaver.RIGHT, self) left_arm_box.addWidget(self.create_button("Create left arm pose", left_pose_saver.create_closure())) right_arm_box.addWidget(self.create_button("Create right arm pose", right_pose_saver.create_closure())) left_arm_box.addItem(QtGui.QSpacerItem(50,20)) right_arm_box.addItem(QtGui.QSpacerItem(50,20)) # Dropdown boxes for saved poses left_pose_loader = PoseLoader(PoseLoader.LEFT, self) right_pose_loader = PoseLoader(PoseLoader.RIGHT, self) self.combo_box_left = left_pose_loader.create_button() self.combo_box_right = right_pose_loader.create_button() left_arm_box.addWidget(self.combo_box_left) right_arm_box.addWidget(self.combo_box_right) left_pose_option_box = QtGui.QHBoxLayout() right_pose_option_box = QtGui.QHBoxLayout() right_pose_option_box.addWidget(self.create_button('Move to pose (R)')) left_pose_option_box.addWidget(self.create_button('Move to pose (L)')) # Buttons for deleting poses for left/right arms left_pose_option_box.addWidget(self.create_button('Delete pose (L)')) right_pose_option_box.addWidget(self.create_button('Delete pose (R)')) left_arm_box.addLayout(left_pose_option_box) right_arm_box.addLayout(right_pose_option_box) left_arm_box.addItem(QtGui.QSpacerItem(50,50)) right_arm_box.addItem(QtGui.QSpacerItem(50,50)) arm_box.addLayout(left_arm_box) arm_box.addItem(QtGui.QSpacerItem(20, 20)) arm_box.addLayout(right_arm_box) large_box.addLayout(arm_box) # Initialize state of saved arm poses for selected dropdowns self.update_saved_l_arm_pose() self.update_saved_r_arm_pose() # Update saved arm pose data on the changing of selected pose self.combo_box_left.connect(self.combo_box_left, QtCore.SIGNAL("currentIndexChanged(QString)"), self.update_saved_l_arm_pose) self.combo_box_right.connect(self.combo_box_right, QtCore.SIGNAL("currentIndexChanged(QString)"), self.update_saved_r_arm_pose) self._widget.setObjectName('ArmGUI') self._widget.setLayout(large_box) context.add_widget(self._widget) self._widget.setStyleSheet("QWidget { image: url(%s) }" % (str(os.path.dirname(os.path.realpath(__file__))) + "/../../arm_gui_bg_large.png")) rospy.loginfo('GUI initialization complete.') def create_button(self, name, method=None): if method == None: method = self.command_cb btn = QtGui.QPushButton(name, self._widget) btn.clicked.connect(method) return btn def command_cb(self): button_name = self._widget.sender().text() if (button_name == 'Relax right arm'): self.relax_arm('r') elif (button_name == 'Freeze right arm'): self.freeze_arm('r') elif (button_name == 'Relax left arm'): self.relax_arm('l') elif (button_name == 'Freeze left arm'): self.freeze_arm('l') elif (button_name == 'Move to pose (R)'): self.move_arm('r') elif (button_name == 'Move to pose (L)'): self.move_arm('l') elif (button_name == 'Delete pose (L)'): self.delete_pose_left() elif (button_name == 'Delete pose (R)'): self.delete_pose_right() def update_saved_l_arm_pose(self): selected_index = self.combo_box_left.currentIndex() if(selected_index == -1): self.saved_l_arm_pose = None else: self.saved_l_arm_pose = self.combo_box_left.itemData(selected_index) def update_saved_r_arm_pose(self): selected_index = self.combo_box_right.currentIndex() if(selected_index == -1): self.saved_r_arm_pose = None else: self.saved_r_arm_pose = self.combo_box_right.itemData(selected_index) def delete_pose_left(self): selected_index = self.combo_box_left.currentIndex() if (selected_index != -1): self.arm_db.rmPos('l', self.combo_box_left.itemText(selected_index)) self.combo_box_left.removeItem(selected_index) def delete_pose_right(self): selected_index = self.combo_box_right.currentIndex() if (selected_index != -1): self.arm_db.rmPos('r', self.combo_box_right.itemText(selected_index)) self.combo_box_right.removeItem(selected_index) def move_arm(self, side_prefix): if (side_prefix == 'r'): if self.saved_r_arm_pose is None: rospy.logerr('Target pose for right arm is None, cannot move.') else: self.freeze_arm('r') self.move_to_joints('r', self.saved_r_arm_pose, 2.0) else: if self.saved_l_arm_pose is None: rospy.logerr('Target pose for left arm is None, cannot move.') else: self.freeze_arm('l') self.move_to_joints('l', self.saved_l_arm_pose, 2.0) pass def move_to_joints(self, side_prefix, positions, time_to_joint): '''Moves the arm to the desired joints''' velocities = [0] * len(positions) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) if (side_prefix == 'r'): traj_goal.trajectory.joint_names = self.r_joint_names self.r_traj_action_client.send_goal(traj_goal) else: traj_goal.trajectory.joint_names = self.l_joint_names self.l_traj_action_client.send_goal(traj_goal) def relax_arm(self, side_prefix): controller_name = side_prefix + '_arm_controller' start_controllers = [] stop_controllers = [controller_name] self.set_arm_mode(start_controllers, stop_controllers) def freeze_arm(self, side_prefix): controller_name = side_prefix + '_arm_controller' start_controllers = [controller_name] stop_controllers = [] self.set_arm_mode(start_controllers, stop_controllers) def set_arm_mode(self, start_controllers, stop_controllers): try: self.switch_service_client(start_controllers, stop_controllers, 1) except rospy.ServiceException: rospy.logerr('Could not change arm mode.') def joint_states_cb(self, msg): '''Callback function that saves the joint positions when a joint_states message is received''' self.lock.acquire() self.all_joint_names = msg.name self.all_joint_poses = msg.position self.joint_sig.emit(msg) self.lock.release() def joint_sig_cb(self, msg): pass def get_joint_state(self, side_prefix): '''Returns position for arm joints on the requested side (r/l)''' if side_prefix == 'r': joint_names = self.r_joint_names else: joint_names = self.l_joint_names if self.all_joint_names == []: rospy.logerr("No robot_state messages received yet!\n") return None positions = [] self.lock.acquire() for joint_name in joint_names: if joint_name in self.all_joint_names: index = self.all_joint_names.index(joint_name) position = self.all_joint_poses[index] positions.append(position) else: rospy.logerr("Joint %s not found!", joint_name) self.lock.release() return None self.lock.release() return positions def shutdown_plugin(self): # TODO unregister all publishers here # Leave both arm controllers on start_controllers = ['r_arm_controller', 'l_arm_controller'] stop_controllers = [] self.set_arm_mode(start_controllers, stop_controllers) def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class PbDGUI(Plugin): exp_state_sig = Signal(ExperimentState) def __init__(self, context): super(PbDGUI, self).__init__(context) self.setObjectName('PbDGUI') self._widget = QWidget() self.speech_cmd_publisher = rospy.Publisher('recognized_command', Command) self.gui_cmd_publisher = rospy.Publisher('gui_command', GuiCommand) rospy.Subscriber('experiment_state', ExperimentState, self.exp_state_cb) rospy.Subscriber('robotsound', SoundRequest, self.robotSoundReceived) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.exp_state_sig.connect(self.update_state) self.commands = dict() self.commands[Command.TEST_MICROPHONE] = 'Test microphone' self.commands[Command.NEW_DEMONSTRATION] = 'New demonstration' self.commands[Command.START_RECORDING] = 'Start recording' self.commands[Command.STOP_RECORDING] = 'Stop recording' self.commands[Command.REPLAY_DEMONSTRATION] = 'Replay demonstration' self.commands[Command.DETECT_SURFACE] = 'Detect surface' self.commands[Command.TAKE_TOOL] = 'Take tool' self.commands[Command.RELEASE_TOOL] = 'Release tool' self.commands[Command.SAVE_ARM_POSE] = 'Save arm pose' self.currentAction = -1 self.currentStep = -1 allWidgetsBox = QtGui.QVBoxLayout() actionBox = QGroupBox('Demonstrations', self._widget) self.actionGrid = QtGui.QGridLayout() self.actionGrid.setHorizontalSpacing(0) for i in range(6): self.actionGrid.addItem(QtGui.QSpacerItem(90, 90), 0, i, QtCore.Qt.AlignCenter) self.actionGrid.setColumnStretch(i, 0) self.actionIcons = dict() actionBoxLayout = QtGui.QHBoxLayout() actionBoxLayout.addLayout(self.actionGrid) actionBox.setLayout(actionBoxLayout) actionButtonGrid = QtGui.QHBoxLayout() actionButtonGrid.addWidget( self.create_button(Command.NEW_DEMONSTRATION)) self.stepsBox = QGroupBox('No demonstrations', self._widget) self.stepsGrid = QtGui.QGridLayout() self.l_model = QtGui.QStandardItemModel(self) self.l_view = self._create_table_view(self.l_model, self.l_row_clicked_cb) self.r_model = QtGui.QStandardItemModel(self) self.r_view = self._create_table_view(self.r_model, self.r_row_clicked_cb) self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 0, 2, 3) self.stepsGrid.addItem(QtGui.QSpacerItem(10, 10), 0, 1, 2, 3) self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 2, 2, 3) self.stepsGrid.addWidget(QtGui.QLabel('Left Arm'), 0, 0) self.stepsGrid.addWidget(QtGui.QLabel('Right Arm'), 0, 2) self.stepsGrid.addWidget(self.l_view, 1, 0) self.stepsGrid.addWidget(self.r_view, 1, 2) stepsBoxLayout = QtGui.QHBoxLayout() stepsBoxLayout.addLayout(self.stepsGrid) self.stepsBox.setLayout(stepsBoxLayout) stepsButtonGrid = QtGui.QHBoxLayout() stepsButtonGrid.addWidget( self.create_button(Command.REPLAY_DEMONSTRATION)) motionButtonGrid = QtGui.QHBoxLayout() motionButtonGrid.addWidget(self.create_button(Command.START_RECORDING)) motionButtonGrid.addWidget(self.create_button(Command.STOP_RECORDING)) misc_grid = QtGui.QHBoxLayout() misc_grid.addWidget(self.create_button(Command.TEST_MICROPHONE)) misc_grid.addWidget(self.create_button(Command.DETECT_SURFACE)) misc_grid.addStretch(1) misc_grid2 = QtGui.QHBoxLayout() misc_grid2.addWidget(self.create_button(Command.TAKE_TOOL)) misc_grid2.addWidget(self.create_button(Command.RELEASE_TOOL)) misc_grid2.addStretch(1) misc_grid3 = QtGui.QHBoxLayout() misc_grid3.addWidget(self.create_button(Command.SAVE_ARM_POSE)) misc_grid3.addStretch(1) speechGroupBox = QGroupBox('Robot Speech', self._widget) speechGroupBox.setObjectName('RobotSpeechGroup') speechBox = QtGui.QHBoxLayout() self.speechLabel = QtGui.QLabel('Robot has not spoken yet') palette = QtGui.QPalette() palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue) self.speechLabel.setPalette(palette) speechBox.addWidget(self.speechLabel) speechGroupBox.setLayout(speechBox) allWidgetsBox.addWidget(actionBox) allWidgetsBox.addLayout(actionButtonGrid) allWidgetsBox.addWidget(self.stepsBox) allWidgetsBox.addLayout(motionButtonGrid) allWidgetsBox.addLayout(stepsButtonGrid) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addLayout(misc_grid) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addLayout(misc_grid2) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 40)) allWidgetsBox.addLayout(misc_grid3) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addWidget(speechGroupBox) allWidgetsBox.addStretch(1) # Fix layout and add main widget to the user interface QtGui.QApplication.setStyle(QtGui.QStyleFactory.create('plastique')) vAllBox = QtGui.QVBoxLayout() vAllBox.addLayout(allWidgetsBox) vAllBox.addStretch(1) hAllBox = QtGui.QHBoxLayout() hAllBox.addLayout(vAllBox) hAllBox.addStretch(1) self._widget.setObjectName('PbDGUI') self._widget.setLayout(hAllBox) context.add_widget(self._widget) rospy.loginfo('Will wait for the experiment state service...') rospy.wait_for_service('get_experiment_state') exp_state_srv = rospy.ServiceProxy('get_experiment_state', GetExperimentState) rospy.loginfo('Got response from the experiment state service...') response = exp_state_srv() self.update_state(response.state) def _create_table_view(self, model, row_click_cb): proxy = QtGui.QSortFilterProxyModel(self) proxy.setSourceModel(model) view = QtGui.QTableView(self._widget) verticalHeader = view.verticalHeader() verticalHeader.sectionClicked.connect(row_click_cb) view.setModel(proxy) view.setMaximumWidth(250) view.setSortingEnabled(False) view.setCornerButtonEnabled(False) return view def get_uid(self, arm_index, index): '''Returns a unique id of the marker''' return (2 * (index + 1) + arm_index) def get_arm_and_index(self, uid): '''Returns a unique id of the marker''' arm_index = uid % 2 index = (uid - arm_index) / 2 return (arm_index, (index - 1)) def r_row_clicked_cb(self, logicalIndex): self.step_pressed(self.get_uid(0, logicalIndex)) def l_row_clicked_cb(self, logicalIndex): self.step_pressed(self.get_uid(1, logicalIndex)) def create_button(self, command): btn = QtGui.QPushButton(self.commands[command], self._widget) btn.clicked.connect(self.command_cb) return btn def update_state(self, state): qWarning('Received new state') n_actions = len(self.actionIcons.keys()) if n_actions < state.n_actions: for i in range(n_actions, state.n_actions): self.new_action() if (self.currentAction != (state.i_current_action - 1)): self.delete_all_steps() self.action_pressed(state.i_current_action - 1, False) n_steps = self.n_steps() if (n_steps < state.n_steps): for i in range(n_steps, state.n_steps): self.save_pose(frameType=ord(state.frame_types[i])) elif (n_steps > state.n_steps): n_to_remove = n_steps - state.n_steps self.r_model.invisibleRootItem().removeRows( state.n_steps, n_to_remove) self.l_model.invisibleRootItem().removeRows( state.n_steps, n_to_remove) ## TODO: DEAL with the following arrays!!! state.r_gripper_states state.l_gripper_states state.r_ref_frames state.l_ref_frames state.objects if (self.currentStep != state.i_current_step): if (self.n_steps() > 0): self.currentStep = state.i_current_step arm_index, index = self.get_arm_and_index(self.currentStep) if (arm_index == 0): self.r_view.selectRow(index) else: self.l_view.selectRow(index) def get_frame_type(self, fr_type): if (fr_type > 1): rospy.logwarn( "Invalid frame type @ save_pose -> get_frame_type: " + str(fr_type)) return ["Go to pose", "Maneuver"][fr_type] def save_pose(self, actionIndex=None, frameType=0): nColumns = 9 if actionIndex is None: actionIndex = self.currentAction stepIndex = self.n_steps(actionIndex) r_step = [ QtGui.QStandardItem('Step' + str(stepIndex + 1)), QtGui.QStandardItem(self.get_frame_type(frameType)), QtGui.QStandardItem('Absolute') ] l_step = [ QtGui.QStandardItem('Step' + str(stepIndex + 1)), QtGui.QStandardItem(self.get_frame_type(frameType)), QtGui.QStandardItem('Absolute') ] self.r_model.invisibleRootItem().appendRow(r_step) self.l_model.invisibleRootItem().appendRow(l_step) self.update_table_view() self.currentStep = stepIndex def update_table_view(self): self.l_view.setColumnWidth(0, 50) self.l_view.setColumnWidth(1, 100) self.l_view.setColumnWidth(2, 70) self.r_view.setColumnWidth(0, 50) self.r_view.setColumnWidth(1, 100) self.r_view.setColumnWidth(2, 70) def n_steps(self, actionIndex=None): return self.l_model.invisibleRootItem().rowCount() def delete_all_steps(self, actionIndex=None): if actionIndex is None: actionIndex = self.currentAction n_steps = self.n_steps() if (n_steps > 0): self.l_model.invisibleRootItem().removeRows(0, n_steps) self.r_model.invisibleRootItem().removeRows(0, n_steps) def n_actions(self): return len(self.actionIcons.keys()) def new_action(self): nColumns = 6 actionIndex = self.n_actions() for key in self.actionIcons.keys(): self.actionIcons[key].selected = False self.actionIcons[key].updateView() actIcon = ActionIcon(self._widget, actionIndex, self.action_pressed) self.actionGrid.addLayout(actIcon, int(actionIndex / nColumns), actionIndex % nColumns) self.actionIcons[actionIndex] = actIcon def step_pressed(self, step_index): gui_cmd = GuiCommand(GuiCommand.SELECT_ACTION_STEP, step_index) self.gui_cmd_publisher.publish(gui_cmd) def action_pressed(self, actionIndex, isPublish=True): for i in range(len(self.actionIcons.keys())): key = self.actionIcons.keys()[i] if key == actionIndex: self.actionIcons[key].selected = True else: self.actionIcons[key].selected = False self.actionIcons[key].updateView() self.currentAction = actionIndex self.stepsBox.setTitle('Steps for Action ' + str(self.currentAction + 1)) if isPublish: gui_cmd = GuiCommand(GuiCommand.SWITCH_TO_ACTION, (actionIndex + 1)) self.gui_cmd_publisher.publish(gui_cmd) def command_cb(self): clickedButtonName = self._widget.sender().text() for key in self.commands.keys(): if (self.commands[key] == clickedButtonName): qWarning('Sending speech command: ' + key) command = Command() command.command = key self.speech_cmd_publisher.publish(command) def robotSoundReceived(self, soundReq): if (soundReq.command == SoundRequest.SAY): qWarning('Robot said: ' + soundReq.arg) self.speechLabel.setText('Robot sound: ' + soundReq.arg) def exp_state_cb(self, state): qWarning('Received new experiment state.') self.exp_state_sig.emit(state) def shutdown_plugin(self): # TODO unregister all publishers here self.speech_cmd_publisher.unregister() self.gui_cmd_publisher.unregister() pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class HalloweenGUI(Plugin): joint_sig = Signal(JointState) def __init__(self, context): super(HalloweenGUI, self).__init__(context) self.setObjectName('HalloweenGUI') self._widget = QWidget() self._widget.setFixedSize(525, 300) self.arm_db = ArmDB() self._tf_listener = TransformListener() # Action/service/message clients or servers switch_srv_name = 'pr2_controller_manager/switch_controller' rospy.loginfo('Waiting for switch controller service...') rospy.wait_for_service(switch_srv_name) self.switch_service_client = rospy.ServiceProxy( switch_srv_name, SwitchController) self.r_joint_names = [ 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint' ] self.l_joint_names = [ 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint' ] self.all_joint_names = [] self.all_joint_poses = [] self.saved_r_arm_pose = None self.saved_l_arm_pose = None self.lock = threading.Lock() rospy.Subscriber('joint_states', JointState, self.joint_states_cb) # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server for RIGHT arm...' ) self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server for LEFT arm...' ) self.l_traj_action_client.wait_for_server() QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.joint_sig.connect(self.joint_sig_cb) large_box = QtGui.QVBoxLayout() arm_box = QtGui.QHBoxLayout() right_arm_box = QtGui.QVBoxLayout() left_arm_box = QtGui.QVBoxLayout() left_arm_box.addItem(QtGui.QSpacerItem(50, 50)) right_arm_box.addItem(QtGui.QSpacerItem(50, 50)) right_arm_box.addWidget(self.create_button('Relax right arm')) right_arm_box.addWidget(self.create_button('Freeze right arm')) left_arm_box.addWidget(self.create_button('Relax left arm')) left_arm_box.addWidget(self.create_button('Freeze left arm')) left_arm_box.addItem(QtGui.QSpacerItem(50, 20)) right_arm_box.addItem(QtGui.QSpacerItem(50, 20)) left_pose_saver = PoseSaver(PoseSaver.LEFT, self) right_pose_saver = PoseSaver(PoseSaver.RIGHT, self) left_arm_box.addWidget( self.create_button("Create left arm pose", left_pose_saver.create_closure())) right_arm_box.addWidget( self.create_button("Create right arm pose", right_pose_saver.create_closure())) left_arm_box.addItem(QtGui.QSpacerItem(50, 20)) right_arm_box.addItem(QtGui.QSpacerItem(50, 20)) # Dropdown boxes for saved poses left_pose_loader = PoseLoader(PoseLoader.LEFT, self) right_pose_loader = PoseLoader(PoseLoader.RIGHT, self) self.combo_box_left = left_pose_loader.create_button() self.combo_box_right = right_pose_loader.create_button() left_arm_box.addWidget(self.combo_box_left) right_arm_box.addWidget(self.combo_box_right) left_pose_option_box = QtGui.QHBoxLayout() right_pose_option_box = QtGui.QHBoxLayout() right_pose_option_box.addWidget(self.create_button('Move to pose (R)')) left_pose_option_box.addWidget(self.create_button('Move to pose (L)')) # Buttons for deleting poses for left/right arms left_pose_option_box.addWidget(self.create_button('Delete pose (L)')) right_pose_option_box.addWidget(self.create_button('Delete pose (R)')) left_arm_box.addLayout(left_pose_option_box) right_arm_box.addLayout(right_pose_option_box) left_arm_box.addItem(QtGui.QSpacerItem(50, 50)) right_arm_box.addItem(QtGui.QSpacerItem(50, 50)) arm_box.addLayout(left_arm_box) arm_box.addItem(QtGui.QSpacerItem(20, 20)) arm_box.addLayout(right_arm_box) large_box.addLayout(arm_box) # Initialize state of saved arm poses for selected dropdowns self.update_saved_l_arm_pose() self.update_saved_r_arm_pose() # Update saved arm pose data on the changing of selected pose self.combo_box_left.connect( self.combo_box_left, QtCore.SIGNAL("currentIndexChanged(QString)"), self.update_saved_l_arm_pose) self.combo_box_right.connect( self.combo_box_right, QtCore.SIGNAL("currentIndexChanged(QString)"), self.update_saved_r_arm_pose) self._widget.setObjectName('HalloweenGUI') self._widget.setLayout(large_box) context.add_widget(self._widget) self._widget.setStyleSheet( "QWidget { image: url(%s) }" % (str(os.path.dirname(os.path.realpath(__file__))) + "/../../arm_gui_bg_large.png")) rospy.loginfo('GUI initialization complete.') def create_button(self, name, method=None): if method == None: method = self.command_cb btn = QtGui.QPushButton(name, self._widget) btn.clicked.connect(method) return btn def command_cb(self): button_name = self._widget.sender().text() if (button_name == 'Relax right arm'): self.relax_arm('r') elif (button_name == 'Freeze right arm'): self.freeze_arm('r') elif (button_name == 'Relax left arm'): self.relax_arm('l') elif (button_name == 'Freeze left arm'): self.freeze_arm('l') elif (button_name == 'Move to pose (R)'): self.move_arm('r') elif (button_name == 'Move to pose (L)'): self.move_arm('l') elif (button_name == 'Delete pose (L)'): self.delete_pose_left() elif (button_name == 'Delete pose (R)'): self.delete_pose_right() def update_saved_l_arm_pose(self): selected_index = self.combo_box_left.currentIndex() if (selected_index == -1): self.saved_l_arm_pose = None else: self.saved_l_arm_pose = self.combo_box_left.itemData( selected_index) def update_saved_r_arm_pose(self): selected_index = self.combo_box_right.currentIndex() if (selected_index == -1): self.saved_r_arm_pose = None else: self.saved_r_arm_pose = self.combo_box_right.itemData( selected_index) def delete_pose_left(self): selected_index = self.combo_box_left.currentIndex() if (selected_index != -1): self.arm_db.rmPos('l', self.combo_box_left.itemText(selected_index)) self.combo_box_left.removeItem(selected_index) def delete_pose_right(self): selected_index = self.combo_box_right.currentIndex() if (selected_index != -1): self.arm_db.rmPos('r', self.combo_box_right.itemText(selected_index)) self.combo_box_right.removeItem(selected_index) def pose_distance(self, source, dest): dist = 0 dist += (source.position.x - dest.position.x) ^ 2 dist += (source.position.y - dest.position.y) ^ 2 dist += (source.position.z - dest.position.z) ^ 2 return dist def move_arm(self, side_prefix): # forward kinematics if (side_prefix == 'r'): if self.saved_r_arm_pose is None: rospy.logerr('Target pose for right arm is None, cannot move.') else: self.freeze_arm(side_prefix) time_to_joints = 2.0 self.move_to_joints(side_prefix, self.saved_r_arm_pose, time_to_joints) else: # side_prefix == 'l' if self.saved_l_arm_pose is None: rospy.logerr('Target pose for left arm is None, cannot move.') else: self.freeze_arm(side_prefix) time_to_joints = 2.0 self.move_to_joints(side_prefix, self.saved_l_arm_pose, time_to_joints) pass def move_to_joints(self, side_prefix, positions, time_to_joint): '''Moves the arm to the desired joints''' velocities = [0] * len(positions) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.points.append( JointTrajectoryPoint( positions=positions, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) if (side_prefix == 'r'): traj_goal.trajectory.joint_names = self.r_joint_names self.r_traj_action_client.send_goal(traj_goal) else: traj_goal.trajectory.joint_names = self.l_joint_names self.l_traj_action_client.send_goal(traj_goal) def relax_arm(self, side_prefix): controller_name = side_prefix + '_arm_controller' start_controllers = [] stop_controllers = [controller_name] self.set_arm_mode(start_controllers, stop_controllers) def freeze_arm(self, side_prefix): controller_name = side_prefix + '_arm_controller' start_controllers = [controller_name] stop_controllers = [] self.set_arm_mode(start_controllers, stop_controllers) def set_arm_mode(self, start_controllers, stop_controllers): try: self.switch_service_client(start_controllers, stop_controllers, 1) except rospy.ServiceException: rospy.logerr('Could not change arm mode.') def joint_states_cb(self, msg): '''Callback function that saves the joint positions when a joint_states message is received''' self.lock.acquire() self.all_joint_names = msg.name self.all_joint_poses = msg.position self.joint_sig.emit(msg) self.lock.release() def joint_sig_cb(self, msg): pass def get_joint_state(self, side_prefix): '''Returns position for arm joints on the requested side (r/l)''' if side_prefix == 'r': joint_names = self.r_joint_names else: joint_names = self.l_joint_names if self.all_joint_names == []: rospy.logerr("No robot_state messages received yet!\n") return None positions = [] self.lock.acquire() for joint_name in joint_names: if joint_name in self.all_joint_names: index = self.all_joint_names.index(joint_name) position = self.all_joint_poses[index] positions.append(position) else: rospy.logerr("Joint %s not found!", joint_name) self.lock.release() return None self.lock.release() return positions def shutdown_plugin(self): # TODO unregister all publishers here # Leave both arm controllers on start_controllers = ['r_arm_controller', 'l_arm_controller'] stop_controllers = [] self.set_arm_mode(start_controllers, stop_controllers) def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class SimpleGUI(Plugin): sound_sig = Signal(SoundRequest) def __init__(self, context): super(SimpleGUI, self).__init__(context) self.setObjectName('SimpleGUI') self._widget = QWidget() self._widget.setFixedSize(600, 600) self._sound_client = SoundClient() rospy.Subscriber('robotsound', SoundRequest, self.sound_cb) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.sound_sig.connect(self.sound_sig_cb) large_box = QtGui.QVBoxLayout() # Textbox to enter words for PR2 to say sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") # Default Text sound_textbox.setFixedWidth(450) self.marker_publisher = rospy.Publisher('visualization_marker', Marker) # Set a handler on the textbox to retrieve the text when button clicked self.sound_textbox = sound_textbox button_box = QtGui.QHBoxLayout() button_box.addItem(QtGui.QSpacerItem(15,20)) button_box.addWidget(self.create_button('Speak', self.command_cb)) button_box.addWidget(sound_textbox) button_box.addStretch(1) large_box.addLayout(button_box) speech_box = QtGui.QHBoxLayout() speech_box.addItem(QtGui.QSpacerItem(15, 20)) self.speech_label = QtGui.QLabel('Robot has not spoken yet') palette = QtGui.QPalette() palette.setColor(QtGui.QPalette.Foreground,QtCore.Qt.blue) self.speech_label.setPalette(palette) speech_box.addWidget(self.speech_label) large_box.addLayout(speech_box) large_box.addStretch(1) #large_box.addItem(QtGui.QSpacerItem(50,20)) # Buttons to move the PR2's head up_head = Head(Head.UP, self) head_box = QtGui.QVBoxLayout() up_head_box = QtGui.QHBoxLayout() up_head_button = self.create_button('^', up_head.create_closure()) down_head = Head(Head.DOWN, self) down_head_box = QtGui.QHBoxLayout() down_head_button = self.create_button('v', down_head.create_closure()) right_head = Head(Head.RIGHT, self) right_head_button = self.create_button('>', right_head.create_closure()) left_head = Head(Head.LEFT, self) left_head_button = self.create_button('<', left_head.create_closure()) left_right_head_box = QtGui.QHBoxLayout() up_head_box.addItem(QtGui.QSpacerItem(235,20)) up_head_box.addWidget(up_head_button) up_head_box.addItem(QtGui.QSpacerItem(275,20)) left_right_head_box.addItem(QtGui.QSpacerItem(160,20)) left_right_head_box.addWidget(left_head_button) left_right_head_box.addItem(QtGui.QSpacerItem(60,20)) left_right_head_box.addWidget(right_head_button) left_right_head_box.addItem(QtGui.QSpacerItem(225,20)) down_head_box.addItem(QtGui.QSpacerItem(235,20)) down_head_box.addWidget(down_head_button) down_head_box.addItem(QtGui.QSpacerItem(275,20)) head_box.addLayout(up_head_box) head_box.addLayout(left_right_head_box) head_box.addLayout(down_head_box) large_box.addLayout(head_box) # Buttons to move the grippers gripper = Gripper(Gripper.RIGHT, Gripper.OPEN, self) right_gripper = self.create_button('Right gripper', gripper.create_closure()) gripper = Gripper(Gripper.LEFT, Gripper.OPEN, self) left_gripper = self.create_button('Left gripper', gripper.create_closure()) large_box.addItem(QtGui.QSpacerItem(100,250)) gripper_box = QtGui.QHBoxLayout() gripper_box.addItem(QtGui.QSpacerItem(75,20)) gripper_box.addWidget(left_gripper) gripper_box.addItem(QtGui.QSpacerItem(450,20)) gripper_box.addWidget(right_gripper) gripper_box.addItem(QtGui.QSpacerItem(75,20)) large_box.addLayout(gripper_box) # Buttons to move the base base_box = QtGui.QVBoxLayout() large_box.addItem(QtGui.QSpacerItem(100,100)) forward_base_box = QtGui.QHBoxLayout() forward_base = Base(Base.FORWARD, self) forward_base_button = self.create_button('move forward', forward_base.create_closure()) forward_base_box.addItem(QtGui.QSpacerItem(400,20)) forward_base_box.addWidget(forward_base_button) forward_base_box.addItem(QtGui.QSpacerItem(400,20)) base_box.addLayout(forward_base_box) left_right_base_box = QtGui.QHBoxLayout() left_base= Base(Base.LEFT, self) left_base_button = self.create_button('move left', left_base.create_closure()) right_base= Base(Base.RIGHT, self) right_base_button= self.create_button('move right', right_base.create_closure()) left_right_base_box.addItem(QtGui.QSpacerItem(300,20)) left_right_base_box.addWidget(left_base_button) left_right_base_box.addItem(QtGui.QSpacerItem(50,20)) left_right_base_box.addWidget(right_base_button) left_right_base_box.addItem(QtGui.QSpacerItem(300,20)) base_box.addLayout(left_right_base_box) backward_base_box = QtGui.QHBoxLayout() backward_base= Base(Base.BACKWARD, self) backward_base_button = self.create_button('move backward', backward_base.create_closure()) backward_base_box.addItem(QtGui.QSpacerItem(400,20)) backward_base_box.addWidget(backward_base_button) backward_base_box.addItem(QtGui.QSpacerItem(400,20)) base_box.addLayout(backward_base_box) large_box.addLayout(base_box) turn_base_box = QtGui.QHBoxLayout() counter_base= Base(Base.COUNTER, self) counter_base_button = self.create_button('\\\n -->', counter_base.create_closure()) clockwise_base= Base(Base.CLOCKWISE, self) clockwise_base_button = self.create_button(' /\n<--', clockwise_base.create_closure()) turn_base_box.addItem(QtGui.QSpacerItem(75,20)) turn_base_box.addWidget(counter_base_button) turn_base_box.addItem(QtGui.QSpacerItem(225,20)) turn_base_box.addWidget(clockwise_base_button) turn_base_box.addItem(QtGui.QSpacerItem(100,20)) large_box.addLayout(turn_base_box) self._widget.setObjectName('SimpleGUI') self._widget.setLayout(large_box) context.add_widget(self._widget) self._widget.setStyleSheet("QWidget { image: url(%s) }" % (str(os.path.dirname(os.path.realpath(__file__))) + "/../../rosie_background.jpg")) def show_text_in_rviz(self, text): marker = Marker(type=Marker.TEXT_VIEW_FACING, id=0, lifetime=rospy.Duration(1.5), pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)), scale=Vector3(0.06, 0.06, 0.06), header=Header(frame_id='base_link'), color=ColorRGBA(0.0, 1.0, 0.0, 0.8), text=text) self.marker_publisher.publish(marker) def sound_cb(self, sound_request): qWarning('Received sound.') self.sound_sig.emit(sound_request) def create_button(self, name, method): btn = QtGui.QPushButton(name, self._widget) btn.clicked.connect(method) btn.setAutoRepeat(True) return btn def sound_sig_cb(self, sound_request): qWarning('Received sound signal.') #if (sound_request.command == SoundRequest.SAY): qWarning('Robot said: ' + sound_request.arg) self.speech_label.setText('Robot said: ' + sound_request.arg) def command_cb(self): button_name = self._widget.sender().text() if (button_name == 'Speak'): qWarning('Robot will say: ' + self.sound_textbox.text()) self._sound_client.say(self.sound_textbox.text()) def shutdown_plugin(self): # TODO unregister all publishers here pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class PbDGUI(Plugin): exp_state_sig = Signal(ExperimentState) def __init__(self, context): super(PbDGUI, self).__init__(context) self.setObjectName('PbDGUI') self._widget = QWidget() self.speech_cmd_publisher = rospy.Publisher('recognized_command', Command) self.gui_cmd_publisher = rospy.Publisher('gui_command', GuiCommand) rospy.Subscriber('experiment_state', ExperimentState, self.exp_state_cb) rospy.Subscriber('robotsound', SoundRequest, self.robotSoundReceived) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.exp_state_sig.connect(self.update_state) self.commands = dict() self.commands[Command.CREATE_NEW_ACTION] = 'New action' self.commands[Command.TEST_MICROPHONE] = 'Test microphone' self.commands[Command.NEXT_ACTION] = 'Next action' self.commands[Command.PREV_ACTION] = 'Previous action' self.commands[Command.SAVE_POSE] = 'Save pose' self.commands[Command.RELAX_RIGHT_ARM] = 'Relax right arm' self.commands[Command.RELAX_LEFT_ARM] = 'Relax left arm' self.commands[Command.FREEZE_RIGHT_ARM] = 'Freeze right arm' self.commands[Command.FREEZE_LEFT_ARM] = 'Freeze left arm' self.commands[Command.OPEN_RIGHT_HAND] = 'Open right hand' self.commands[Command.OPEN_LEFT_HAND] = 'Open left hand' self.commands[Command.CLOSE_RIGHT_HAND] = 'Close right hand' self.commands[Command.CLOSE_LEFT_HAND] = 'Close left hand' self.commands[Command.CLOSE_LEFT_HAND] = 'Close left hand' self.commands[Command.EXECUTE_ACTION] = 'Execute action' self.commands[Command.STOP_EXECUTION] = 'Stop execution' self.commands[Command.DELETE_ALL_STEPS] = 'Delete all' self.commands[Command.DELETE_LAST_STEP] = 'Delete last' self.commands[Command.RECORD_OBJECT_POSE] = 'Record object poses' self.currentAction = -1 self.currentStep = -1 allWidgetsBox = QtGui.QVBoxLayout() actionBox = QGroupBox('Actions', self._widget) self.actionGrid = QtGui.QGridLayout() self.actionGrid.setHorizontalSpacing(0) for i in range(6): self.actionGrid.addItem(QtGui.QSpacerItem(90, 90), 0, i, QtCore.Qt.AlignCenter) self.actionGrid.setColumnStretch(i, 0) self.actionIcons = dict() actionBoxLayout = QtGui.QHBoxLayout() actionBoxLayout.addLayout(self.actionGrid) actionBox.setLayout(actionBoxLayout) actionButtonGrid = QtGui.QHBoxLayout() actionButtonGrid.addWidget( self.create_button(Command.CREATE_NEW_ACTION)) self.stepsBox = QGroupBox('No actions created yet', self._widget) self.stepsGrid = QtGui.QGridLayout() self.l_model = QtGui.QStandardItemModel(self) self.l_view = self._create_table_view(self.l_model, self.l_row_clicked_cb) self.r_model = QtGui.QStandardItemModel(self) self.r_view = self._create_table_view(self.r_model, self.r_row_clicked_cb) self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 0, 2, 3) self.stepsGrid.addItem(QtGui.QSpacerItem(10, 10), 0, 1, 2, 3) self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 2, 2, 3) self.stepsGrid.addWidget(QtGui.QLabel('Left Arm'), 0, 0) self.stepsGrid.addWidget(QtGui.QLabel('Right Arm'), 0, 2) self.stepsGrid.addWidget(self.l_view, 1, 0) self.stepsGrid.addWidget(self.r_view, 1, 2) stepsBoxLayout = QtGui.QHBoxLayout() stepsBoxLayout.addLayout(self.stepsGrid) self.stepsBox.setLayout(stepsBoxLayout) stepsButtonGrid = QtGui.QHBoxLayout() stepsButtonGrid.addWidget(self.create_button(Command.SAVE_POSE)) stepsButtonGrid.addWidget(self.create_button(Command.EXECUTE_ACTION)) stepsButtonGrid.addWidget(self.create_button(Command.STOP_EXECUTION)) stepsButtonGrid.addWidget(self.create_button(Command.DELETE_ALL_STEPS)) stepsButtonGrid.addWidget(self.create_button(Command.DELETE_LAST_STEP)) misc_grid = QtGui.QHBoxLayout() misc_grid.addWidget(self.create_button(Command.TEST_MICROPHONE)) misc_grid.addWidget(self.create_button(Command.RECORD_OBJECT_POSE)) misc_grid.addStretch(1) misc_grid2 = QtGui.QHBoxLayout() misc_grid2.addWidget(self.create_button(Command.RELAX_RIGHT_ARM)) misc_grid2.addWidget(self.create_button(Command.RELAX_LEFT_ARM)) misc_grid2.addWidget(self.create_button(Command.FREEZE_RIGHT_ARM)) misc_grid2.addWidget(self.create_button(Command.FREEZE_LEFT_ARM)) misc_grid2.addStretch(1) misc_grid3 = QtGui.QHBoxLayout() misc_grid3.addWidget(self.create_button(Command.OPEN_RIGHT_HAND)) misc_grid3.addWidget(self.create_button(Command.OPEN_LEFT_HAND)) misc_grid3.addWidget(self.create_button(Command.CLOSE_RIGHT_HAND)) misc_grid3.addWidget(self.create_button(Command.CLOSE_LEFT_HAND)) misc_grid3.addStretch(1) misc_grid4 = QtGui.QHBoxLayout() misc_grid4.addWidget(self.create_button(Command.PREV_ACTION)) misc_grid4.addWidget(self.create_button(Command.NEXT_ACTION)) misc_grid4.addStretch(1) speechGroupBox = QGroupBox('Robot Speech', self._widget) speechGroupBox.setObjectName('RobotSpeechGroup') speechBox = QtGui.QHBoxLayout() self.speechLabel = QtGui.QLabel('Robot has not spoken yet') palette = QtGui.QPalette() palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue) self.speechLabel.setPalette(palette) speechBox.addWidget(self.speechLabel) speechGroupBox.setLayout(speechBox) allWidgetsBox.addWidget(actionBox) allWidgetsBox.addLayout(actionButtonGrid) allWidgetsBox.addWidget(self.stepsBox) allWidgetsBox.addLayout(stepsButtonGrid) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addLayout(misc_grid) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addLayout(misc_grid2) allWidgetsBox.addLayout(misc_grid3) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addLayout(misc_grid4) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addWidget(speechGroupBox) allWidgetsBox.addStretch(1) # Fix layout and add main widget to the user interface QtGui.QApplication.setStyle(QtGui.QStyleFactory.create('plastique')) vAllBox = QtGui.QVBoxLayout() vAllBox.addLayout(allWidgetsBox) vAllBox.addStretch(1) hAllBox = QtGui.QHBoxLayout() hAllBox.addLayout(vAllBox) hAllBox.addStretch(1) self._widget.setObjectName('PbDGUI') self._widget.setLayout(hAllBox) context.add_widget(self._widget) rospy.loginfo('Will wait for the experiment state service...') rospy.wait_for_service('get_experiment_state') exp_state_srv = rospy.ServiceProxy('get_experiment_state', GetExperimentState) rospy.loginfo('Got response from the experiment state service...') response = exp_state_srv() self.update_state(response.state) @staticmethod def loginfo(message): '''Because all other ROS logging has some kind of information about what's being emitted, and qWarning doesn't, we're going to wrap the function to provide a little bit of additional info for newbies. Args: message (str): The message to log ''' qWarning('[INFO] [pbd_gui.py] ' + message) def _create_table_view(self, model, row_click_cb): proxy = QtGui.QSortFilterProxyModel(self) proxy.setSourceModel(model) view = QtGui.QTableView(self._widget) verticalHeader = view.verticalHeader() verticalHeader.sectionClicked.connect(row_click_cb) view.setModel(proxy) view.setMaximumWidth(250) view.setSortingEnabled(False) view.setCornerButtonEnabled(False) return view def get_uid(self, arm_index, index): '''Returns a unique id of the marker''' return (2 * (index + 1) + arm_index) def get_arm_and_index(self, uid): '''Returns a unique id of the marker''' arm_index = uid % 2 index = (uid - arm_index) / 2 return (arm_index, (index - 1)) def r_row_clicked_cb(self, logicalIndex): self.step_pressed(self.get_uid(0, logicalIndex)) def l_row_clicked_cb(self, logicalIndex): self.step_pressed(self.get_uid(1, logicalIndex)) def create_button(self, command): btn = QtGui.QPushButton(self.commands[command], self._widget) btn.clicked.connect(self.command_cb) return btn def update_state(self, state): PbDGUI.loginfo('Received new state') n_actions = len(self.actionIcons.keys()) if n_actions < state.n_actions: for i in range(n_actions, state.n_actions): self.new_action() if (self.currentAction != (state.i_current_action - 1)): self.delete_all_steps() self.action_pressed(state.i_current_action - 1, False) n_steps = self.n_steps() if (n_steps < state.n_steps): for i in range(n_steps, state.n_steps): self.save_pose() elif (n_steps > state.n_steps): n_to_remove = n_steps - state.n_steps self.r_model.invisibleRootItem().removeRows( state.n_steps, n_to_remove) self.l_model.invisibleRootItem().removeRows( state.n_steps, n_to_remove) ## TODO: DEAL with the following arrays!!! state.r_gripper_states state.l_gripper_states state.r_ref_frames state.l_ref_frames state.objects if (self.currentStep != state.i_current_step): if (self.n_steps() > 0): self.currentStep = state.i_current_step arm_index, index = self.get_arm_and_index(self.currentStep) if (arm_index == 0): self.r_view.selectRow(index) else: self.l_view.selectRow(index) def save_pose(self, actionIndex=None): nColumns = 9 if actionIndex is None: actionIndex = self.currentAction stepIndex = self.n_steps(actionIndex) r_step = [ QtGui.QStandardItem('Step' + str(stepIndex + 1)), QtGui.QStandardItem('Go to pose'), QtGui.QStandardItem('Absolute') ] l_step = [ QtGui.QStandardItem('Step' + str(stepIndex + 1)), QtGui.QStandardItem('Go to pose'), QtGui.QStandardItem('Absolute') ] self.r_model.invisibleRootItem().appendRow(r_step) self.l_model.invisibleRootItem().appendRow(l_step) self.update_table_view() self.currentStep = stepIndex def update_table_view(self): self.l_view.setColumnWidth(0, 50) self.l_view.setColumnWidth(1, 100) self.l_view.setColumnWidth(2, 70) self.r_view.setColumnWidth(0, 50) self.r_view.setColumnWidth(1, 100) self.r_view.setColumnWidth(2, 70) def n_steps(self, actionIndex=None): return self.l_model.invisibleRootItem().rowCount() def delete_all_steps(self, actionIndex=None): if actionIndex is None: actionIndex = self.currentAction n_steps = self.n_steps() if (n_steps > 0): self.l_model.invisibleRootItem().removeRows(0, n_steps) self.r_model.invisibleRootItem().removeRows(0, n_steps) def n_actions(self): return len(self.actionIcons.keys()) def new_action(self): nColumns = 6 actionIndex = self.n_actions() for key in self.actionIcons.keys(): self.actionIcons[key].selected = False self.actionIcons[key].updateView() actIcon = ActionIcon(self._widget, actionIndex, self.action_pressed) self.actionGrid.addLayout(actIcon, int(actionIndex / nColumns), actionIndex % nColumns) self.actionIcons[actionIndex] = actIcon def step_pressed(self, step_index): gui_cmd = GuiCommand(GuiCommand.SELECT_ACTION_STEP, step_index) self.gui_cmd_publisher.publish(gui_cmd) def action_pressed(self, actionIndex, isPublish=True): for i in range(len(self.actionIcons.keys())): key = self.actionIcons.keys()[i] if key == actionIndex: self.actionIcons[key].selected = True else: self.actionIcons[key].selected = False self.actionIcons[key].updateView() self.currentAction = actionIndex self.stepsBox.setTitle('Steps for Action ' + str(self.currentAction + 1)) if isPublish: gui_cmd = GuiCommand(GuiCommand.SWITCH_TO_ACTION, (actionIndex + 1)) self.gui_cmd_publisher.publish(gui_cmd) def command_cb(self): clickedButtonName = self._widget.sender().text() for key in self.commands.keys(): if (self.commands[key] == clickedButtonName): PbDGUI.loginfo('Sending speech command: ' + key) command = Command() command.command = key self.speech_cmd_publisher.publish(command) def robotSoundReceived(self, soundReq): if (soundReq.command == SoundRequest.SAY): PbDGUI.loginfo('Robot said: ' + soundReq.arg) self.speechLabel.setText('Robot sound: ' + soundReq.arg) def exp_state_cb(self, state): self.exp_state_sig.emit(state) def shutdown_plugin(self): # TODO unregister all publishers here self.speech_cmd_publisher.unregister() self.gui_cmd_publisher.unregister() pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class ArmGUI(Plugin): joint_sig = Signal(JointState) def __init__(self, context): super(ArmGUI, self).__init__(context) self.setObjectName('ArmGUI') self._widget = QWidget() print "init" # Action/service/message clients or servers switch_srv_name = 'pr2_controller_manager/switch_controller' rospy.loginfo('Waiting for switch controller service...') rospy.wait_for_service(switch_srv_name) self.switch_service_client = rospy.ServiceProxy(switch_srv_name, SwitchController) self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] self.all_joint_names = [] self.all_joint_poses = [] # saving our poses self.saved_r_arm_poses = defaultdict(partial(list)) self.saved_l_arm_poses = defaultdict(partial(list)) self.saved_pose_sets = set() self.lock = threading.Lock() rospy.Subscriber('joint_states', JointState, self.joint_states_cb) # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...') self.l_traj_action_client.wait_for_server() QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.joint_sig.connect(self.joint_sig_cb) # controls time between poses self.time_between_poses = 2.0 large_box = QtGui.QVBoxLayout() button_box1 = QtGui.QHBoxLayout() button_box1.addWidget(self.create_button('Relax arms')) button_box1.addWidget(self.create_button('Freeze arms')) button_box1.addStretch(1) large_box.addLayout(button_box1) large_box.addItem(QtGui.QSpacerItem(100,20)) button_box2 = QtGui.QHBoxLayout() self.pose_set_text = QtGui.QLineEdit(self._widget) button_box2.addWidget(self.pose_set_text) button_box2.addWidget(self.create_button('Add to Pose Set')) button_box2.addStretch(1) large_box.addLayout(button_box2) large_box.addItem(QtGui.QSpacerItem(100,20)) # box with a slider controlling speed robot moves to pose with speed_box = QtGui.QHBoxLayout() speed_label = QtGui.QLabel('Pose speed: ') speed_box.addWidget(speed_label) speed_box.addWidget(QtGui.QLabel('Slow')) sldr = QtGui.QSlider(QtCore.Qt.Horizontal) sldr.setSliderPosition(50) speed_box.addWidget(sldr) #sldr.setGeometry(50, 50, 130, 30); sldr.valueChanged[int].connect(self.set_pose_speed) speed_box.addWidget(QtGui.QLabel('Fast')) speed_box.addStretch(1) large_box.addLayout(speed_box) button_box3 = QtGui.QHBoxLayout() self.pose_selector = QtGui.QComboBox() self.pose_selector.setSizeAdjustPolicy(QtGui.QComboBox.AdjustToContents) self.pose_selector.currentIndexChanged[str].connect(self.update_pose_set_length) button_box3.addWidget(self.pose_selector) self.pose_set_length_label = QtGui.QLabel() button_box3.addWidget(self.pose_set_length_label) self.update_pose_set_length() button_box3.addWidget(self.create_button('Do Pose Set')) button_box3.addItem(QtGui.QSpacerItem(50,20)) button_box3.addWidget(self.create_button('Delete Pose Set')) button_box3.addStretch(1) large_box.addLayout(button_box3) large_box.addItem(QtGui.QSpacerItem(100,20)) poses_box = QtGui.QVBoxLayout() self.status_message_label = QtGui.QLabel('No Saved Poses') poses_box.addWidget(self.status_message_label) large_box.addLayout(poses_box) large_box.addStretch(1) self._widget.setObjectName('ArmGUI') self._widget.setLayout(large_box) context.add_widget(self._widget) rospy.loginfo('GUI initialization complete.') # currently maps from a quarter second between poses to ~3.5 seconds; # pos comes in as the slider value which ranges from 0 to 100. it # seems like even with the delay set to 0, there are automatic # constraints, but to be safe the quarter second is added. there also # seem to be poses that are intrinsically hard for the robot to move # between, causing slow movement even with a low delay set. def set_pose_speed(self, pos): self.time_between_poses = (100 - pos) / 30 + .25 def create_button(self, name): btn = QtGui.QPushButton(name, self._widget) btn.clicked.connect(self.command_cb) return btn # This is a really janky way of doing this, you should check # self._widget.sender() to figure out where the event originated. def command_cb(self): button_name = self._widget.sender().text() if (button_name == 'Relax arms'): self.relax_arm('r') self.relax_arm('l') elif (button_name == 'Freeze arms'): self.freeze_arm('r') self.freeze_arm('l') elif (button_name == 'Add to Pose Set'): self.save_pose() elif (button_name == 'Do Pose Set'): self.move_arm() elif (button_name == 'Delete Pose Set'): self.delete_pose() self.update_pose_set_length() def save_pose(self): pose_set = self.pose_set_text.text() if len(pose_set) is 0 or pose_set is None: self.status_message_label.setText("Invalid pose name") return # if not already saved, make a new pose set if pose_set not in self.saved_pose_sets: self.pose_selector.addItem(pose_set) self.saved_pose_sets.add(pose_set) # auto select the saved pose index = self.pose_selector.findText(pose_set) self.pose_selector.setCurrentIndex(index) r_joint_state = self.get_joint_state('r') l_joint_state = self.get_joint_state('l') self.saved_r_arm_poses[pose_set].append(r_joint_state) self.saved_l_arm_poses[pose_set].append(l_joint_state) self.saved_pose_sets.add(pose_set) self.status_message_label.setText('Pose saved!') def update_pose_set_length(self): pose_set = self.pose_selector.currentText() text = "(%d)" % len(self.saved_r_arm_poses[pose_set]) self.pose_set_length_label.setText(text) def move_arm(self): pose_set = self.pose_selector.currentText() if not pose_set in self.saved_pose_sets: rospy.logerr("Target pose set does not exist, I cannot move my little arms!") self.status_message_label.setText("No pose set") return if pose_set in self.saved_r_arm_poses: self.freeze_arm('r') self.move_to_joints('r', self.saved_r_arm_poses[pose_set], self.time_between_poses) if pose_set in self.saved_l_arm_poses: self.freeze_arm('l') self.move_to_joints('l', self.saved_l_arm_poses[pose_set], self.time_between_poses) self.status_message_label.setText('Pose executing!') def delete_pose(self): pose_set = self.pose_selector.currentText() rospy.loginfo('Clearing pose set %s' % pose_set) # removing from the select box pose_set_len = len(self.saved_r_arm_poses[pose_set]) index = self.pose_selector.findText(pose_set) self.pose_selector.removeItem(index) # removing from the pose set maps self.saved_r_arm_poses.pop(pose_set, None) self.saved_l_arm_poses.pop(pose_set, None) self.saved_pose_sets.remove(pose_set) self.status_message_label.setText('Pose deleted!') # unsure if the time_to_joint input is still necessary # with self.time_between_poses def move_to_joints(self, side_prefix, positions, time_to_joint): '''Moves the arm to the desired joints''' traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) time_move = time_to_joint + 3 print "using following positions %s" % positions for pose in positions: velocities = [0] * len(pose) traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=pose, velocities=velocities, time_from_start=rospy.Duration(time_move))) time_move = time_move + time_to_joint if (side_prefix == 'r'): traj_goal.trajectory.joint_names = self.r_joint_names self.r_traj_action_client.send_goal(traj_goal) else: traj_goal.trajectory.joint_names = self.l_joint_names self.l_traj_action_client.send_goal(traj_goal) def relax_arm(self, side_prefix): controller_name = side_prefix + '_arm_controller' start_controllers = [] stop_controllers = [controller_name] self.set_arm_mode(start_controllers, stop_controllers) def freeze_arm(self, side_prefix): controller_name = side_prefix + '_arm_controller' start_controllers = [controller_name] stop_controllers = [] self.set_arm_mode(start_controllers, stop_controllers) def set_arm_mode(self, start_controllers, stop_controllers): try: self.switch_service_client(start_controllers, stop_controllers, 1) except rospy.ServiceException: rospy.logerr('Could not change arm mode.') def joint_states_cb(self, msg): '''Callback function that saves the joint positions when a joint_states message is received''' self.lock.acquire() self.all_joint_names = msg.name self.all_joint_poses = msg.position self.joint_sig.emit(msg) self.lock.release() def joint_sig_cb(self, msg): pass def get_joint_state(self, side_prefix): '''Returns position for arm joints on the requested side (r/l)''' if side_prefix == 'r': joint_names = self.r_joint_names else: joint_names = self.l_joint_names if self.all_joint_names == []: rospy.logerr("No robot_state messages received yet!\n") return None positions = [] self.lock.acquire() for joint_name in joint_names: if joint_name in self.all_joint_names: index = self.all_joint_names.index(joint_name) position = self.all_joint_poses[index] positions.append(position) else: rospy.logerr("Joint %s not found!", joint_name) self.lock.release() return None self.lock.release() return positions def shutdown_plugin(self): # TODO unregister all publishers here # Leave both arm controllers on start_controllers = ['r_arm_controller', 'l_arm_controller'] stop_controllers = [] self.set_arm_mode(start_controllers, stop_controllers) def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: instance_settings.set_value("r_arm_poses", self.saved_r_arm_poses) instance_settings.set_value("l_arm_poses", self.saved_l_arm_poses) def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: r_arm_poses = instance_settings.value("r_arm_poses") if not r_arm_poses is None: self.saved_r_arm_poses = r_arm_poses for pose in self.saved_r_arm_poses.keys(): if not pose is None and pose != "": self.pose_selector.addItem(pose) self.saved_pose_sets.add(pose) l_arm_poses = instance_settings.value("l_arm_poses") if not l_arm_poses is None: self.saved_l_arm_poses = l_arm_poses
class SimpleGUI(Plugin): sound_sig = Signal(SoundRequest) def __init__(self, context): super(SimpleGUI, self).__init__(context) self.setObjectName('SimpleGUI') self._widget = QWidget() self._widget.setFixedSize(600, 600) self._sound_client = SoundClient() rospy.Subscriber('robotsound', SoundRequest, self.sound_cb) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.sound_sig.connect(self.sound_sig_cb) large_box = QtGui.QVBoxLayout() #Sound textbox sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") #Default Text sound_textbox.setFixedWidth(450) #Set a handle on the textbox to retrieve the text when button clicked self.sound_textbox = sound_textbox button_box = QtGui.QHBoxLayout() button_box.addItem(QtGui.QSpacerItem(15, 20)) button_box.addWidget(self.create_button('Speak', self.command_cb)) button_box.addWidget(sound_textbox) button_box.addStretch(1) large_box.addLayout(button_box) speech_box = QtGui.QHBoxLayout() speech_box.addItem(QtGui.QSpacerItem(15, 20)) self.speech_label = QtGui.QLabel('Robot has not spoken yet.') palette = QtGui.QPalette() palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue) self.speech_label.setPalette(palette) speech_box.addWidget(self.speech_label) large_box.addLayout(speech_box) large_box.addStretch(1) #large_box.addItem(QtGui.QSpacerItem(50,20)) up_head = Head(Head.UP) head_box = QtGui.QVBoxLayout() up_head_box = QtGui.QHBoxLayout() up_head_button = self.create_button('^', up_head.create_closure()) #large_box.addWidget(up_head_button) down_head = Head(Head.DOWN) down_head_box = QtGui.QHBoxLayout() down_head_button = self.create_button('v', down_head.create_closure()) #large_box.addWidget(down_head_button) right_head = Head(Head.RIGHT) right_head_button = self.create_button('>', right_head.create_closure()) #large_box.addWidget(right_head_button) left_head = Head(Head.LEFT) left_head_button = self.create_button('<', left_head.create_closure()) left_right_head_box = QtGui.QHBoxLayout() up_head_box.addItem(QtGui.QSpacerItem(235, 20)) up_head_box.addWidget(up_head_button) up_head_box.addItem(QtGui.QSpacerItem(275, 20)) left_right_head_box.addItem(QtGui.QSpacerItem(160, 20)) left_right_head_box.addWidget(left_head_button) left_right_head_box.addItem(QtGui.QSpacerItem(60, 20)) left_right_head_box.addWidget(right_head_button) left_right_head_box.addItem(QtGui.QSpacerItem(225, 20)) down_head_box.addItem(QtGui.QSpacerItem(235, 20)) down_head_box.addWidget(down_head_button) down_head_box.addItem(QtGui.QSpacerItem(275, 20)) head_box.addLayout(up_head_box) head_box.addLayout(left_right_head_box) head_box.addLayout(down_head_box) large_box.addLayout(head_box) #large_box.addItem(QtGui.QSpacerItem(500,20)) #large_box.addWidget(left_head_button) gripper = Gripper(Gripper.RIGHT, Gripper.OPEN) right_gripper = self.create_button('Right gripper!', gripper.create_closure()) gripper = Gripper(Gripper.LEFT, Gripper.OPEN) left_gripper = self.create_button('Left gripper!', gripper.create_closure()) large_box.addItem(QtGui.QSpacerItem(100, 250)) gripper_box = QtGui.QHBoxLayout() gripper_box.addItem(QtGui.QSpacerItem(75, 20)) gripper_box.addWidget(left_gripper) gripper_box.addItem(QtGui.QSpacerItem(450, 20)) gripper_box.addWidget(right_gripper) gripper_box.addItem(QtGui.QSpacerItem(75, 20)) large_box.addLayout(gripper_box) base_box = QtGui.QVBoxLayout() large_box.addItem(QtGui.QSpacerItem(100, 100)) #forward forward_base_box = QtGui.QHBoxLayout() forward_base = Base(Base.FORWARD) forward_base_button = self.create_button('move forward', forward_base.create_closure()) forward_base_box.addItem(QtGui.QSpacerItem(400, 20)) forward_base_box.addWidget(forward_base_button) forward_base_box.addItem(QtGui.QSpacerItem(400, 20)) base_box.addLayout(forward_base_box) #large_box.addWidget(forward_base_button) #left left_right_base_box = QtGui.QHBoxLayout() left_base = Base(Base.LEFT) left_base_button = self.create_button('move left', left_base.create_closure()) #large_box.addWidget(left_base_button) #right right_base = Base(Base.RIGHT) right_base_button = self.create_button('move right', right_base.create_closure()) left_right_base_box.addItem(QtGui.QSpacerItem(300, 20)) left_right_base_box.addWidget(left_base_button) left_right_base_box.addItem(QtGui.QSpacerItem(50, 20)) left_right_base_box.addWidget(right_base_button) left_right_base_box.addItem(QtGui.QSpacerItem(300, 20)) base_box.addLayout(left_right_base_box) #large_box.addWidget(right_base_button) #backward backward_base_box = QtGui.QHBoxLayout() backward_base = Base(Base.BACKWARD) backward_base_button = self.create_button( 'move backward', backward_base.create_closure()) backward_base_box.addItem(QtGui.QSpacerItem(400, 20)) backward_base_box.addWidget(backward_base_button) backward_base_box.addItem(QtGui.QSpacerItem(400, 20)) base_box.addLayout(backward_base_box) #large_box.addWidget(backward_base_button) large_box.addLayout(base_box) turn_base_box = QtGui.QHBoxLayout() #turn left turnleft_base = Base(Base.TURNLEFT) turnleft_base_button = self.create_button( ' /\n<--', turnleft_base.create_closure()) #large_box.addWidget(turnleft_base_button) #turn right turnright_base = Base(Base.TURNRIGHT) turnright_base_button = self.create_button( '\\\n -->', turnright_base.create_closure()) turn_base_box.addItem(QtGui.QSpacerItem(75, 20)) turn_base_box.addWidget(turnright_base_button) turn_base_box.addItem(QtGui.QSpacerItem(225, 20)) turn_base_box.addWidget(turnleft_base_button) turn_base_box.addItem(QtGui.QSpacerItem(100, 20)) large_box.addLayout(turn_base_box) #large_box.addWidget(turnright_base_button) self._widget.setObjectName('SimpleGUI') self._widget.setLayout(large_box) context.add_widget(self._widget) self._widget.setStyleSheet( "QWidget { image: url(%s) }" % "/home/vjampala/catkin_ws/src/cse481/hw1/rqt_simplegui/rosie_background.jpg" ) def sound_cb(self, sound_request): qWarning('Received sound.') self.sound_sig.emit(sound_request) def create_button(self, name, method): btn = QtGui.QPushButton(name, self._widget) btn.clicked.connect(method) return btn def sound_sig_cb(self, sound_request): qWarning('Received sound signal.') #if (sound_request.command == SoundRequest.SAY): qWarning('Robot said: ' + sound_request.arg) self.speech_label.setText('Robot said: ' + sound_request.arg) def command_cb(self): button_name = self._widget.sender().text() if (button_name == 'Speak'): qWarning('Robot will say: ' + self.sound_textbox.text()) self._sound_client.say(self.sound_textbox.text()) def shutdown_plugin(self): # TODO unregister all publishers here pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class SimpleGUI(Plugin): # For sending speech sound_sig = Signal(SoundRequest) # Joints for arm poses joint_sig = Signal(JointState) def __init__(self, context): self.prompt_width = 170 self.input_width = 250 super(SimpleGUI, self).__init__(context) self.setObjectName('SimpleGUI') self._widget = QWidget() self._sound_client = SoundClient() #find relative path for files to load self.local_dir = os.path.dirname(__file__) self.dir = os.path.join(self.local_dir, './lib/rqt_simplegui/') if not os.path.isdir(self.dir): os.makedirs(self.dir) #need to add any additional subfolders as needed if not os.path.isdir(self.dir + 'animations/'): os.makedirs(self.dir + 'animations/') # Creates a subscriber to the ROS topic, having msg type SoundRequest rospy.Subscriber('robotsound', SoundRequest, self.sound_cb) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.sound_sig.connect(self.sound_sig_cb) # Code used for saving/ loading arm poses for the robot switch_srv_name = 'pr2_controller_manager/switch_controller' rospy.loginfo('Waiting for switch controller service...') rospy.wait_for_service(switch_srv_name) self.switch_service_client = rospy.ServiceProxy(switch_srv_name, SwitchController) self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] self.all_joint_names = [] self.all_joint_poses = [] # Hash tables storing the name of the pose and the # associated positions for that pose, initially empty self.saved_l_poses = {} self.saved_r_poses = {} # Hash tables for storing name of animations and the associated pose list self.saved_animations = {} self.lock = threading.Lock() rospy.Subscriber('joint_states', JointState, self.joint_states_cb) #parsing for animations dir = os.path.dirname(__file__) qWarning(dir) filename = os.path.join(self.dir, 'animations/') ani_path = filename ani_listing = os.listdir(ani_path) for infile in ani_listing: pose_left = [] pose_right = [] left_gripper_states = [] right_gripper_states = [] line_num = 1 for line in fileinput.input(ani_path + infile): if (line_num % 2 == 1): pose = [float(x) for x in line.split()] pose_left.append(pose[:len(pose)/2]) pose_right.append(pose[len(pose)/2:]) else: states = line.split() left_gripper_states.append(states[0]) right_gripper_states.append(states[1]) line_num += 1 self.saved_animations[os.path.splitext(infile)[0]] = Quad(pose_left, pose_right, left_gripper_states, right_gripper_states) # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...') self.l_traj_action_client.wait_for_server() # Navigation functionality initialization self.roomNav = RoomNavigator() self._tf_listener = TransformListener() self.animPlay = AnimationPlayer(None, None, None, None) # Detection and pickup functionality self.pap = PickAndPlaceManager(self._tf_listener, self.roomNav, self.animPlay) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.joint_sig.connect(self.joint_sig_cb) # Create a large vertical box that is top aligned large_box = QtGui.QVBoxLayout() large_box.setAlignment(QtCore.Qt.AlignTop) large_box.setMargin(0) large_box.addItem(QtGui.QSpacerItem(10,0)) # Buttons for controlling the head of the robot head_box = QtGui.QHBoxLayout() head_box.addItem(QtGui.QSpacerItem(230,0)) head_box.addWidget(self.create_pressed_button('Head Up')) head_box.addStretch(1) large_box.addLayout(head_box) button_box = QtGui.QHBoxLayout() button_box.addItem(QtGui.QSpacerItem(80,0)) button_box.addWidget(self.create_pressed_button('Head Turn Left')) button_box.addWidget(self.create_pressed_button('Head Down')) button_box.addWidget(self.create_pressed_button('Head Turn Right')) button_box.addStretch(1) button_box.setMargin(0) button_box.setSpacing(0) large_box.addLayout(button_box) # Shows what the robot says speech_box = QtGui.QHBoxLayout() self.speech_label = QtGui.QLabel('Robot has not spoken yet') palette = QtGui.QPalette() palette.setColor(QtGui.QPalette.Foreground,QtCore.Qt.blue) self.speech_label.setPalette(palette) # speech_box.addItem(QtGui.QSpacerItem(100,0)) #speech_box.addWidget(self.speech_label) # large_box.addLayout(speech_box) # Speak button speak_button_box = QtGui.QHBoxLayout(); speech_prompt = QtGui.QLabel('Enter Speech Text:') speech_prompt.setFixedWidth(self.prompt_width) speak_button_box.addWidget(speech_prompt) robot_says = QtGui.QLineEdit(self._widget) robot_says.setFixedWidth(self.input_width) robot_says.textChanged[str].connect(self.onChanged) # speak_button_box.addWidget(robot_says) speak_button_box.addWidget(self.create_button('Speak')) speak_button_box.addStretch(1) large_box.addLayout(speak_button_box) large_box.addItem(QtGui.QSpacerItem(0,50)) # Buttons for arm poses pose_button_box1 = QtGui.QHBoxLayout() pose_button_box1.addItem(QtGui.QSpacerItem(150,0)) pose_button_box1.addWidget(self.create_button('Relax Left Arm')) pose_button_box1.addWidget(self.create_button('Relax Right Arm')) pose_button_box1.addStretch(1) large_box.addLayout(pose_button_box1) # Buttons for grippers gripper_button_box = QtGui.QHBoxLayout() gripper_button_box.addItem(QtGui.QSpacerItem(150,0)) gripper_button_box.addWidget(self.create_button('Open Left Gripper')) gripper_button_box.addWidget(self.create_button('Open Right Gripper')) gripper_button_box.addStretch(1) large_box.addLayout(gripper_button_box) large_box.addItem(QtGui.QSpacerItem(0,25)) # Buttons for animation animation_box = QtGui.QHBoxLayout() play_anim_label = QtGui.QLabel('Select Animation:') play_anim_label.setFixedWidth(self.prompt_width) animation_box.addWidget(play_anim_label) self.saved_animations_list = QtGui.QComboBox(self._widget) animation_box.addWidget(self.saved_animations_list) pose_time_label = QtGui.QLabel('Duration(sec):') pose_time_label.setFixedWidth(100) animation_box.addWidget(pose_time_label) self.pose_time = QtGui.QLineEdit(self._widget) self.pose_time.setFixedWidth(50) self.pose_time.setText('2.0') animation_box.addWidget(self.pose_time) animation_box.addWidget(self.create_button('Play Animation')) animation_box.addStretch(1) large_box.addLayout(animation_box) animation_box2 = QtGui.QHBoxLayout() animation_name_label = QtGui.QLabel('Enter Animation Name:') animation_name_label.setFixedWidth(self.prompt_width) animation_box2.addWidget(animation_name_label) self.animation_name = QtGui.QLineEdit(self._widget) self.animation_name.setFixedWidth(self.input_width) animation_box2.addWidget(self.animation_name) animation_box2.addWidget(self.create_button('Save Animation')) animation_box2.addStretch(1) large_box.addLayout(animation_box2) animation_box3 = QtGui.QHBoxLayout() pose_name_label = QtGui.QLabel('Enter Pose Name:') pose_name_label.setFixedWidth(self.prompt_width) animation_box3.addWidget(pose_name_label) self.pose_name_temp = QtGui.QLineEdit(self._widget) self.pose_name_temp.setFixedWidth(self.input_width) animation_box3.addWidget(self.pose_name_temp) animation_box3.addWidget(self.create_button('Add Current Pose')) animation_box3.addStretch(1) large_box.addLayout(animation_box3) # Playing around with UI stuff play_box = QtGui.QHBoxLayout() pose_sequence_label = QtGui.QLabel('Current Pose Sequence:') pose_sequence_label.setFixedWidth(self.prompt_width) pose_sequence_label.setAlignment(QtCore.Qt.AlignTop) self.list_widget = QListWidget() self.list_widget.setDragDropMode(QAbstractItemView.InternalMove) self.list_widget.setMaximumSize(self.input_width, 200) play_box.addWidget(pose_sequence_label) play_box.addWidget(self.list_widget) play_box.addStretch(1) large_box.addLayout(play_box) large_box.addItem(QtGui.QSpacerItem(0,50)) # Buttons for first row of base controls first_base_button_box = QtGui.QHBoxLayout() first_base_button_box.addItem(QtGui.QSpacerItem(70,0)) first_base_button_box.addWidget(self.create_pressed_button('Rotate Left')) first_base_button_box.addWidget(self.create_pressed_button('^')) first_base_button_box.addWidget(self.create_pressed_button('Rotate Right')) first_base_button_box.addStretch(1) large_box.addLayout(first_base_button_box) # Buttons for second row of base controls second_base_button_box = QtGui.QHBoxLayout() second_base_button_box.addItem(QtGui.QSpacerItem(70,0)) second_base_button_box.addWidget(self.create_pressed_button('<')) second_base_button_box.addWidget(self.create_pressed_button('v')) second_base_button_box.addWidget(self.create_pressed_button('>')) second_base_button_box.addWidget(self.create_button('Move to Bin')) second_base_button_box.addWidget(self.create_button('Object Detect')) second_base_button_box.addWidget(self.create_button('Clean Room')) second_base_button_box.addStretch(1) large_box.addLayout(second_base_button_box) # Animation related items to store intermediate pose co-ordinates to save self.animation_map = {} self.create_state = False self._widget.setObjectName('SimpleGUI') self._widget.setLayout(large_box) context.add_widget(self._widget) # Look straight and down when launched self.head_x = 1.0 self.head_y = 0.0 self.head_z = 0.5 self.head_action(self.head_x, self.head_y, self.head_z) # Set grippers to closed on initialization self.left_gripper_state = '' self.right_gripper_state = '' self.gripper_action('l', 0.0) self.gripper_action('r', 0.0) # Lab 6 self.marker_publisher = rospy.Publisher('visualization_marker', Marker) # Saved states for poses saved_pose_box = QtGui.QHBoxLayout() self.saved_left_poses = QtGui.QLabel('') self.saved_right_poses = QtGui.QLabel('') saved_pose_box.addWidget(self.saved_left_poses) saved_pose_box.addWidget(self.saved_right_poses) large_box.addLayout(saved_pose_box) # Preload the map of animations self.saved_animations_list.addItems(self.saved_animations.keys()) # Move the torso all the way down # self.torso_down(True) # Autonomous navigation stuff ''' self.locations = [Pose(Point(2.48293590546, 3.90075874329, 0.000), Quaternion(0.000, 0.000, -0.783917630973, 0.620864838632)), Pose(Point(3.70106744766, 0.304672241211, 0.000), Quaternion(0.000, 0.000, 0.950186880196, -0.311680754463)), Pose(Point(2.57326722145, 1.51304531097, 0.000), Quaternion(0.000, 0.000, 0.96127194482, -0.275601611212)), Pose(Point(1.28060531616, 1.52380752563, 0.000), Quaternion(0.000, 0.000, 0.946345258806, -0.323157316388)), Pose(Point(2.11048603058, 0.420155525208, 0.000), Quaternion(0.000, 0.000, 0.945222393391, -0.326427062346)), Pose(Point(2.82733058929, -0.739856719971, 0.000), Quaternion(0.000, 0.000, 0.945473998362, -0.325697587373)), Pose(Point(1.29184818268, -1.90485572815, 0.000), Quaternion(0.000, 0.000, 0.942275557345, -0.334838429739)), Pose(Point(0.722846984863, -1.0921459198, 0.000), Quaternion(0.000, 0.000, 0.949330143731, -0.314280572424))] ''' self.locations = [Pose(Point(2.04748392105, 2.04748010635, 0.000), Quaternion(0.000, 0.000, -0.776968030817, 0.629540053601)), Pose(Point(2.34193611145, 1.43208932877, 0), Quaternion(0, 0, -0.841674385779, 0.539985396398)), Pose(Point(3.43202018738, -0.258297920227, 0.000), Quaternion(0.000, 0.000, 0.996656413122, -0.0817067572629)), Pose(Point(0.931655406952, -1.96435832977, 0.000), Quaternion(0.000, 0.000, 0.691187586713, 0.722675390458)), Pose(Point(-0.369112968445, 0.0330476760864, 0.000), Quaternion(0.000, 0.000, 0.0275340398899, 0.999620866453))] self.index = 1; rospy.loginfo("Completed GUI initialization") # Event for when text box is changed def onChanged(self, text): self.speech_label.setText(text) self.speech_label.adjustSize() def sound_cb(self, sound_request): qWarning('Received sound.') self.sound_sig.emit(sound_request) def create_button(self, name): btn = QtGui.QPushButton(name, self._widget) btn.setFixedWidth(150) btn.clicked.connect(self.command_cb) return btn def create_pressed_button(self, name): btn = QtGui.QPushButton(name, self._widget) btn.setFixedWidth(150) btn.pressed.connect(self.command_cb) btn.setAutoRepeat(True) # To make sure the movement repeats itself return btn def sound_sig_cb(self, sound_request): qWarning('Received sound signal.') qWarning('Robot said: ' + sound_request.arg) self.speech_label.setText(sound_request.arg) #'Robot said: ' + #a button was clicked def command_cb(self): button_name = self._widget.sender().text() #robot talk button clicked if (button_name == 'Speak'): qWarning('Robot will say: ' + self.speech_label.text() ) self._sound_client.say(self.speech_label.text()) self.show_text_in_rviz("Robot is Speaking") #gripper button selected elif ('Gripper' in button_name): self.gripper_click(button_name) # Move forward elif (button_name == '^'): self.base_action(0.25, 0.0, 0.0, 0.0, 0.0, 0.0) # Move left elif (button_name == '<'): self.base_action(0.0, 0.25, 0.0, 0.0, 0.0, 0.0) # Move right elif (button_name == '>'): self.base_action(0.0, -0.25, 0.0, 0.0, 0.0, 0.0) # Move back elif (button_name == 'v'): self.base_action(-0.25, 0.0, 0.0, 0.0, 0.0, 0.0) #Rotate Left elif (button_name == 'Rotate Left'): self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, 0.50) # Rotate Right elif (button_name == 'Rotate Right'): self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, -0.50) # A head button selected elif ('Head' in button_name): self.rotate_head(button_name) #An arm button selected #third param unused in freeze/relax #Second word in button should be side elif ('Arm' in button_name): arm_side = button_name.split()[1] if ('Freeze' in button_name or 'Relax' in button_name): new_arm_state = button_name.split()[0] self.toggle_arm(arm_side[0].lower(), new_arm_state, True) old_arm_state = '' if (new_arm_state == 'Relax'): old_arm_state = 'Freeze' else: old_arm_state = 'Relax' self._widget.sender().setText('%s %s Arm' % (old_arm_state, arm_side)) elif('Play Animation' == button_name): self.animPlay.left_poses = self.saved_animations[self.saved_animations_list.currentText()].left self.animPlay.right_poses = self.saved_animations[self.saved_animations_list.currentText()].right self.animPlay.left_gripper_states = self.saved_animations[self.saved_animations_list.currentText()].left_gripper self.animPlay.right_gripper_states = self.saved_animations[self.saved_animations_list.currentText()].right_gripper if self.pose_time.text() == '': self.show_warning('Please enter a duration in seconds.') else: self.animPlay.play(self.pose_time.text()) elif('Animation' in button_name): if ('Save' in button_name): if self.animation_name.text() == '': self.show_warning('Please enter name for animation') else: self.save_animation(self.animation_name.text()) self.list_widget.clear() self.animation_name.setText('') elif('Add Current Pose' == button_name): if self.pose_name_temp.text() == '': self.show_warning('Insert name for pose') else: self.animation_map[self.pose_name_temp.text()] = Quad(self.get_joint_state('l'), self.get_joint_state('r'), self.left_gripper_state, self.right_gripper_state) list_item = QListWidgetItem() list_item.setText(self.pose_name_temp.text()) self.list_widget.addItem(list_item) self.pose_name_temp.setText('') elif('Move to Bin' == button_name): self.move_to_bin_action() elif('Clean Room' == button_name): rospy.loginfo("STARTING AUTONOMOUS MODE") self.tuck_arms() while(self.index < len(self.locations)): self.roomNav.move_to_trash_location(self.locations[self.index]) # self.index += 1 self.head_action(1.0, 0, -0.50, True) # Returns Nonce if nothing, and the point of the first object it sees otherwise map_point = self.pap.detect_objects() if(map_point == None): self.index += 1 else: self.pick_and_move_trash_action() rospy.loginfo("FINISHED AUTONOMOUS MODE") self.index = 1 elif('Object Detect' == button_name): self.pick_and_move_trash_action() def pick_and_move_trash_action(self, map_point = None): if map_point == None: self.head_action(1.0, 0, -0.50, True) map_point = self.pap.detect_objects() if map_point == None: return # Convert to base link and move towards the object 0.50m away map_point = Transformer.transform(self._tf_listener, map_point.pose, map_point.header.frame_id, '/base_link') rospy.loginfo("Object is " + str (map_point.pose.position.x) + " away") ''' if(map_point.pose.position.x < 0.8): self.roomNav.move_to_trash_location(self.locations[self.index - 1]) ''' move_back_first = False; if(map_point.pose.position.x < 0.7): move_back_first = True; map_point.pose.position.x -= 0.450 map_point = Transformer.transform(self._tf_listener, map_point.pose, '/base_link', '/map') if(move_back_first): self.roomNav.move_to_trash_location(self.locations[self.index - 1]) success = self.roomNav.move_to_trash_location(map_point.pose) '''This part of the code strafes the robot left to get closer to the object''' ''' rate = rospy.Rate(10) position = Point() move_cmd = Twist() move_cmd.linear.y = 0.25 odom_frame = '/odom_combined' # Find out if the robot uses /base_link or /base_footprint try: self._tf_listener.waitForTransform(odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_footprint' except (tf.Exception, tf.ConnectivityException, tf.LookupException): try: self._tf_listener.waitForTransform(odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("Cannot find transform between " + odom_frame + " and /base_link or /base_footprint") rospy.signal_shutdown("tf Exception") # Get current position position = self.get_odom() x_start = position.x y_start = position.y # Distance travelled distance = 0 goal_distance = 0.25 rospy.loginfo("Strafing left") # Enter the loop to move along a side while distance < goal_distance: rospy.loginfo("Distance is at " + str(distance)) # Publish the Twist message and sleep 1 cycle self.base_action(0, 0.25, 0, 0, 0, 0) rate.sleep() # Get the current position position = self.get_odom() # Compute the Euclidean distance from the start distance = abs(position.y - y_start) ''' if(success): # Move head to look at the object, this will wait for a result self.head_action(0, 0.4, 0.55, True) # Move arms to ready pickup position, this will wait for a result before trying to detect and pick up object self.animPlay.left_poses = self.saved_animations['ready_pickup'].left self.animPlay.right_poses = self.saved_animations['ready_pickup'].right self.animPlay.left_gripper_states = self.saved_animations['ready_pickup'].left_gripper self.animPlay.right_gripper_states = self.saved_animations['ready_pickup'].right_gripper self.animPlay.play('3.0') for i in range (0,3): success = self.pap.detect_and_pickup() # Move head back to look forward # Move head to look at the object, this will wait for a result self.head_action(1.0, 0.0, 0.55, True) if success: break # Move to bin self.move_to_bin_action() def get_odom(self): # Get the current transform between the odom and base frames try: trans = self._tf_listener.lookupTransform('/odom_combined', self.base_frame, rospy.Time(0)) except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("TF Exception") return return Point(trans[0][0],trans[0][1],trans[0][2]) # gripper_type is either 'l' for left or 'r' for right # gripper position is the position as a parameter to the gripper goal def gripper_action(self, gripper_type, gripper_position): name_space = '/' + gripper_type + '_gripper_controller/gripper_action' gripper_client = SimpleActionClient(name_space, GripperCommandAction) gripper_client.wait_for_server() gripper_goal = GripperCommandGoal() gripper_goal.command.position = gripper_position gripper_goal.command.max_effort = 30.0 gripper_client.send_goal(gripper_goal) # update the state of the current gripper being modified if (gripper_type == 'l'): if (gripper_position == 0.0): self.left_gripper_state = 'closed' else: self.left_gripper_state = 'open' else: if (gripper_position == 0.0): self.right_gripper_state = 'closed' else: self.right_gripper_state = 'open' def base_action(self, x, y, z, theta_x, theta_y, theta_z): topic_name = '/base_controller/command' base_publisher = rospy.Publisher(topic_name, Twist) twist_msg = Twist() twist_msg.linear = Vector3(x, y, z) twist_msg.angular = Vector3(theta_x, theta_y, theta_z) base_publisher.publish(twist_msg) # Moves the torso of the robot down to its maximum def torso_down(self, wait = False): self.torso_client = SimpleActionClient('/torso_controller/position_joint_action', SingleJointPositionAction) torso_goal = SingleJointPositionGoal() torso_goal.position = 0.0 torso_goal.min_duration = rospy.Duration(5.0) torso_goal.max_velocity = 1.0 self.torso_client.send_goal(torso_goal) if wait: # wait for the head movement to finish before we try to detect and pickup an object finished_within_time = self.torso_client.wait_for_result(rospy.Duration(15)) # Check for success or failure if not finished_within_time: self.torso_client.cancel_goal() rospy.loginfo("Timed out achieving torso movement goal") else: state = self.torso_client.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Torso movement goal succeeded!") rospy.loginfo("State:" + str(state)) else: rospy.loginfo("Torso movement goal failed with error code: " + str(state)) def head_action(self, x, y, z, wait = False): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_goal = PointHeadGoal() head_goal.target.header.frame_id = 'base_link' head_goal.min_duration = rospy.Duration(1.0) head_goal.target.point = Point(x, y, z) head_client.send_goal(head_goal) if wait: # wait for the head movement to finish before we try to detect and pickup an object finished_within_time = head_client.wait_for_result(rospy.Duration(5)) # Check for success or failure if not finished_within_time: head_client.cancel_goal() rospy.loginfo("Timed out achieving head movement goal") else: state = head_client.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Head movement goal succeeded!") rospy.loginfo("State:" + str(state)) else: rospy.loginfo("Head movement goal failed with error code: " + str(self.goal_states[state])) def tuck_arms(self): rospy.loginfo('Left tuck arms') self.animPlay.left_poses = self.saved_animations['left_tuck'].left self.animPlay.right_poses = self.saved_animations['left_tuck'].right self.animPlay.left_gripper_states = self.saved_animations['left_tuck'].left_gripper self.animPlay.right_gripper_states = self.saved_animations['left_tuck'].right_gripper self.animPlay.play('3.0') def move_to_bin_action(self): # First tuck arms self.tuck_arms() # Move to the bin rospy.loginfo('Clicked the move to bin button') self.roomNav.move_to_bin() # Throw the trash away rospy.loginfo('Throwing trash away with left arm') self.animPlay.left_poses = self.saved_animations['l_dispose'].left self.animPlay.right_poses = self.saved_animations['l_dispose'].right self.animPlay.left_gripper_states = self.saved_animations['l_dispose'].left_gripper self.animPlay.right_gripper_states = self.saved_animations['l_dispose'].right_gripper self.animPlay.play('2.0') # Tuck arms again self.tuck_arms() def shutdown_plugin(self): # TODO unregister all publishers here pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass def show_text_in_rviz(self, text): marker = Marker(type=Marker.TEXT_VIEW_FACING, id=0, lifetime=rospy.Duration(1.5), pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)), scale=Vector3(0.06, 0.06, 0.06), header=Header(frame_id='base_link'), color=ColorRGBA(0.0, 1.0, 0.0, 0.8), text=text) self.marker_publisher.publish(marker) def show_arrow_in_rviz(self, arrow): marker = Marker(type=Marker.ARROW, id=0, lifetime=rospy.Duration(1.5), pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)), scale=Vector3(0.06, 0.06, 0.06), header=Header(frame_id='base_link'), color=ColorRGBA(0.0, 1.0, 0.0, 0.8), arrow=arrow) self.marker_publisher.publish(marker) def save_animation(self, animation_name): if animation_name != '': filename = os.path.join(self.dir, 'animations/') anim_file = open(filename + animation_name + '.txt', 'w') l_animation_queue = [] r_animation_queue = [] l_gripper_queue = [] r_gripper_queue = [] for i in range(self.list_widget.count()): item = self.list_widget.item(i) pose_name = item.text() anim_file.write(re.sub(',' , '', str(self.animation_map[pose_name].left).strip('[]') + ' ' + str(self.animation_map[pose_name].right).strip('[]'))) anim_file.write('\n') anim_file.write(str(self.animation_map[pose_name].left_gripper) + ' ' + str(self.animation_map[pose_name].right_gripper)) l_animation_queue.append(self.animation_map[pose_name].left) r_animation_queue.append(self.animation_map[pose_name].right) l_gripper_queue.append(self.animation_map[pose_name].left_gripper) r_gripper_queue.append(self.animation_map[pose_name].right_gripper) anim_file.write('\n') anim_file.close() self.saved_animations[animation_name] = Quad(l_animation_queue, r_animation_queue, l_gripper_queue, r_gripper_queue) self.saved_animations_list.addItem(animation_name) # Bug? Multiple entries # Reset the pending queue self.l_animation_queue = [] self.r_animation_queue = [] else: self.show_warning('Please insert name for animation') def gripper_click(self, button_name): grip_side = '' grip_side_text = '' if ('Left' in button_name): grip_side = 'l' grip_side_text = 'left' else: grip_side = 'r' grip_side_text = 'right' if ('Open' in button_name): grip_action = 20.0 grip_action_text = 'close' qWarning('Robot opened %s gripper' % (grip_side_text)) else: grip_action = 0.0 grip_action_text = 'open' qWarning('Robot closed %s gripper' % (grip_side_text)) self.show_text_in_rviz("%sing %s Gripper" % (grip_action_text.capitalize(), grip_side_text.capitalize())) self.gripper_action(grip_side, grip_action) self._widget.sender().setText('%s %s Gripper' % (grip_action_text.capitalize(), grip_side_text.capitalize())) def rotate_head(self, button_name): if('Left' in button_name): #qWarning('x: %s, y: %s' % (self.head_x, self.head_y)) if (self.head_x < -0.8 and self.head_y > 0.0): self.show_warning('Can\'t rotate anymore') elif (self.head_y < 0.0): self.head_x += 0.1 self.head_y = -((1.0 - self.head_x ** 2.0) ** 0.5) self.show_text_in_rviz("Turning Head Left") else: self.head_x -= 0.1 self.head_y = (1.0 - self.head_x ** 2.0) ** 0.5 self.show_text_in_rviz("Turning Head Left") qWarning('x: %s, y: %s' % (self.head_x, self.head_y)) self.head_action(self.head_x, self.head_y, self.head_z) elif('Up' in button_name): if (self.head_z <= 1.6): self.head_z += 0.1 self.show_text_in_rviz("Moving Head Up") self.head_action(self.head_x, self.head_y, self.head_z) else: self.show_warning('Can\'t look up anymore') elif('Down' in button_name): if (self.head_z >= -2.2): self.head_z -= 0.1 self.show_text_in_rviz("Moving Head Down") self.head_action(self.head_x, self.head_y, self.head_z) else: self.show_warning('Can\'t look down anymore') else: #qWarning('x: %s, y: %s' % (self.head_x, self.head_y)) if (self.head_x < -0.8 and self.head_y < 0.0): self.show_warning('Can\'t rotate anymore') elif (self.head_y > 0.0): self.head_x += 0.1 self.head_y = (1.0 - self.head_x ** 2.0) ** 0.5 self.show_text_in_rviz("Turning Head Right") else: self.head_x -= 0.1 self.head_y = -((1.0 - self.head_x ** 2.0) ** 0.5) self.show_text_in_rviz("Turning Head Right") #qWarning('x: %s, y: %s' % (self.head_x, self.head_y)) self.head_action(self.head_x, self.head_y, self.head_z) def toggle_arm(self, side, toggle, button): controller_name = side + '_arm_controller' start_controllers = [] stop_controllers = [] if (toggle == 'Relax'): stop_controllers.append(controller_name) else: start_controllers.append(controller_name) self.set_arm_mode(start_controllers, stop_controllers) def set_arm_mode(self, start_controllers, stop_controllers): try: self.switch_service_client(start_controllers, stop_controllers, 1) except rospy.ServiceException: rospy.logerr('Could not change arm mode.') def joint_states_cb(self, msg): '''Callback function that saves the joint positions when a joint_states message is received''' self.lock.acquire() self.all_joint_names = msg.name self.all_joint_poses = msg.position self.joint_sig.emit(msg) self.lock.release() def joint_sig_cb(self, msg): pass def get_joint_state(self, side_prefix): '''Returns position for arm joints on the requested side (r/l)''' if side_prefix == 'r': joint_names = self.r_joint_names else: joint_names = self.l_joint_names if self.all_joint_names == []: rospy.logerr("No robot_state messages received yet!\n") return None positions = [] self.lock.acquire() for joint_name in joint_names: if joint_name in self.all_joint_names: index = self.all_joint_names.index(joint_name) position = self.all_joint_poses[index] positions.append(position) else: rospy.logerr("Joint %s not found!", joint_name) self.lock.release() return None self.lock.release() return positions def show_warning(self, text): qWarning(text) msgBox = QMessageBox() msgBox.setText(text) msgBox.exec_()
class SimpleGUI(Plugin): sound_sig = Signal(SoundRequest) def __init__(self, context): super(SimpleGUI, self).__init__(context) self.setObjectName('SimpleGUI') self._widget = QWidget() self._widget.setFixedSize(600, 600) self._sound_client = SoundClient() rospy.Subscriber('robotsound', SoundRequest, self.sound_cb) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.sound_sig.connect(self.sound_sig_cb) large_box = QtGui.QVBoxLayout() #Sound textbox sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") #Default Text sound_textbox.setFixedWidth(450) #Set a handle on the textbox to retrieve the text when button clicked self.sound_textbox = sound_textbox button_box = QtGui.QHBoxLayout() button_box.addItem(QtGui.QSpacerItem(15,20)) button_box.addWidget(self.create_button('Speak', self.command_cb)) button_box.addWidget(sound_textbox) button_box.addStretch(1) large_box.addLayout(button_box) speech_box = QtGui.QHBoxLayout() speech_box.addItem(QtGui.QSpacerItem(15, 20)) self.speech_label = QtGui.QLabel('Robot has not spoken yet.') palette = QtGui.QPalette() palette.setColor(QtGui.QPalette.Foreground,QtCore.Qt.blue) self.speech_label.setPalette(palette) speech_box.addWidget(self.speech_label) large_box.addLayout(speech_box) large_box.addStretch(1) #large_box.addItem(QtGui.QSpacerItem(50,20)) up_head = Head(Head.UP) head_box = QtGui.QVBoxLayout() up_head_box = QtGui.QHBoxLayout() up_head_button = self.create_button('^', up_head.create_closure()) #large_box.addWidget(up_head_button) down_head = Head(Head.DOWN) down_head_box = QtGui.QHBoxLayout() down_head_button = self.create_button('v', down_head.create_closure()) #large_box.addWidget(down_head_button) right_head = Head(Head.RIGHT) right_head_button = self.create_button('>', right_head.create_closure()) #large_box.addWidget(right_head_button) left_head = Head(Head.LEFT) left_head_button = self.create_button('<', left_head.create_closure()) left_right_head_box = QtGui.QHBoxLayout() up_head_box.addItem(QtGui.QSpacerItem(235,20)) up_head_box.addWidget(up_head_button) up_head_box.addItem(QtGui.QSpacerItem(275,20)) left_right_head_box.addItem(QtGui.QSpacerItem(160,20)) left_right_head_box.addWidget(left_head_button) left_right_head_box.addItem(QtGui.QSpacerItem(60,20)) left_right_head_box.addWidget(right_head_button) left_right_head_box.addItem(QtGui.QSpacerItem(225,20)) down_head_box.addItem(QtGui.QSpacerItem(235,20)) down_head_box.addWidget(down_head_button) down_head_box.addItem(QtGui.QSpacerItem(275,20)) head_box.addLayout(up_head_box) head_box.addLayout(left_right_head_box) head_box.addLayout(down_head_box) large_box.addLayout(head_box) #large_box.addItem(QtGui.QSpacerItem(500,20)) #large_box.addWidget(left_head_button) gripper = Gripper(Gripper.RIGHT, Gripper.OPEN) right_gripper = self.create_button('Right gripper!', gripper.create_closure()) gripper = Gripper(Gripper.LEFT, Gripper.OPEN) left_gripper = self.create_button('Left gripper!', gripper.create_closure()) large_box.addItem(QtGui.QSpacerItem(100,250)) gripper_box = QtGui.QHBoxLayout() gripper_box.addItem(QtGui.QSpacerItem(75,20)) gripper_box.addWidget(left_gripper) gripper_box.addItem(QtGui.QSpacerItem(450,20)) gripper_box.addWidget(right_gripper) gripper_box.addItem(QtGui.QSpacerItem(75,20)) large_box.addLayout(gripper_box) base_box = QtGui.QVBoxLayout() large_box.addItem(QtGui.QSpacerItem(100,100)) #forward forward_base_box = QtGui.QHBoxLayout() forward_base = Base(Base.FORWARD) forward_base_button = self.create_button('move forward', forward_base.create_closure()) forward_base_box.addItem(QtGui.QSpacerItem(400,20)) forward_base_box.addWidget(forward_base_button) forward_base_box.addItem(QtGui.QSpacerItem(400,20)) base_box.addLayout(forward_base_box) #large_box.addWidget(forward_base_button) #left left_right_base_box = QtGui.QHBoxLayout() left_base= Base(Base.LEFT) left_base_button = self.create_button('move left', left_base.create_closure()) #large_box.addWidget(left_base_button) #right right_base= Base(Base.RIGHT) right_base_button= self.create_button('move right', right_base.create_closure()) left_right_base_box.addItem(QtGui.QSpacerItem(300,20)) left_right_base_box.addWidget(left_base_button) left_right_base_box.addItem(QtGui.QSpacerItem(50,20)) left_right_base_box.addWidget(right_base_button) left_right_base_box.addItem(QtGui.QSpacerItem(300,20)) base_box.addLayout(left_right_base_box) #large_box.addWidget(right_base_button) #backward backward_base_box = QtGui.QHBoxLayout() backward_base= Base(Base.BACKWARD) backward_base_button = self.create_button('move backward', backward_base.create_closure()) backward_base_box.addItem(QtGui.QSpacerItem(400,20)) backward_base_box.addWidget(backward_base_button) backward_base_box.addItem(QtGui.QSpacerItem(400,20)) base_box.addLayout(backward_base_box) #large_box.addWidget(backward_base_button) large_box.addLayout(base_box) turn_base_box = QtGui.QHBoxLayout() #turn left turnleft_base= Base(Base.TURNLEFT) turnleft_base_button = self.create_button(' /\n<--', turnleft_base.create_closure()) #large_box.addWidget(turnleft_base_button) #turn right turnright_base= Base(Base.TURNRIGHT) turnright_base_button = self.create_button('\\\n -->', turnright_base.create_closure()) turn_base_box.addItem(QtGui.QSpacerItem(75,20)) turn_base_box.addWidget(turnright_base_button) turn_base_box.addItem(QtGui.QSpacerItem(225,20)) turn_base_box.addWidget(turnleft_base_button) turn_base_box.addItem(QtGui.QSpacerItem(100,20)) large_box.addLayout(turn_base_box) #large_box.addWidget(turnright_base_button) self._widget.setObjectName('SimpleGUI') self._widget.setLayout(large_box) context.add_widget(self._widget) self._widget.setStyleSheet("QWidget { image: url(%s) }" % "/home/vjampala/catkin_ws/src/cse481/hw1/rqt_simplegui/rosie_background.jpg") def sound_cb(self, sound_request): qWarning('Received sound.') self.sound_sig.emit(sound_request) def create_button(self, name, method): btn = QtGui.QPushButton(name, self._widget) btn.clicked.connect(method) return btn def sound_sig_cb(self, sound_request): qWarning('Received sound signal.') #if (sound_request.command == SoundRequest.SAY): qWarning('Robot said: ' + sound_request.arg) self.speech_label.setText('Robot said: ' + sound_request.arg) def command_cb(self): button_name = self._widget.sender().text() if (button_name == 'Speak'): qWarning('Robot will say: ' + self.sound_textbox.text()) self._sound_client.say(self.sound_textbox.text()) def shutdown_plugin(self): # TODO unregister all publishers here pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class SimpleGUI(Plugin): # For sending speech sound_sig = Signal(SoundRequest) # Joints for arm poses joint_sig = Signal(JointState) def __init__(self, context): self.prompt_width = 170 self.input_width = 250 super(SimpleGUI, self).__init__(context) self.setObjectName('SimpleGUI') self._widget = QWidget() self._sound_client = SoundClient() #find relative path for files to load self.local_dir = os.path.dirname(__file__) self.dir = os.path.join(self.local_dir, './lib/rqt_simplegui/') if not os.path.isdir(self.dir): os.makedirs(self.dir) #need to add any additional subfolders as needed if not os.path.isdir(self.dir + 'animations/'): os.makedirs(self.dir + 'animations/') # Creates a subscriber to the ROS topic, having msg type SoundRequest rospy.Subscriber('robotsound', SoundRequest, self.sound_cb) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.sound_sig.connect(self.sound_sig_cb) # Code used for saving/ loading arm poses for the robot switch_srv_name = 'pr2_controller_manager/switch_controller' rospy.loginfo('Waiting for switch controller service...') rospy.wait_for_service(switch_srv_name) self.switch_service_client = rospy.ServiceProxy( switch_srv_name, SwitchController) self.r_joint_names = [ 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint' ] self.l_joint_names = [ 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint' ] self.all_joint_names = [] self.all_joint_poses = [] # Hash tables storing the name of the pose and the # associated positions for that pose, initially empty self.saved_l_poses = {} self.saved_r_poses = {} # Hash tables for storing name of animations and the associated pose list self.saved_animations = {} self.lock = threading.Lock() rospy.Subscriber('joint_states', JointState, self.joint_states_cb) #parsing for animations dir = os.path.dirname(__file__) qWarning(dir) filename = os.path.join(self.dir, 'animations/') ani_path = filename ani_listing = os.listdir(ani_path) for infile in ani_listing: pose_left = [] pose_right = [] left_gripper_states = [] right_gripper_states = [] line_num = 1 for line in fileinput.input(ani_path + infile): if (line_num % 2 == 1): pose = [float(x) for x in line.split()] pose_left.append(pose[:len(pose) / 2]) pose_right.append(pose[len(pose) / 2:]) else: states = line.split() left_gripper_states.append(states[0]) right_gripper_states.append(states[1]) line_num += 1 self.saved_animations[os.path.splitext(infile)[0]] = Quad( pose_left, pose_right, left_gripper_states, right_gripper_states) # Create a trajectory action client r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server for RIGHT arm...' ) self.r_traj_action_client.wait_for_server() l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo( 'Waiting for a response from the trajectory action server for LEFT arm...' ) self.l_traj_action_client.wait_for_server() # Navigation functionality initialization self.roomNav = RoomNavigator() self._tf_listener = TransformListener() self.animPlay = AnimationPlayer(None, None, None, None) # Detection and pickup functionality self.pap = PickAndPlaceManager(self._tf_listener, self.roomNav, self.animPlay) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.joint_sig.connect(self.joint_sig_cb) # Create a large vertical box that is top aligned large_box = QtGui.QVBoxLayout() large_box.setAlignment(QtCore.Qt.AlignTop) large_box.setMargin(0) large_box.addItem(QtGui.QSpacerItem(10, 0)) # Buttons for controlling the head of the robot head_box = QtGui.QHBoxLayout() head_box.addItem(QtGui.QSpacerItem(230, 0)) head_box.addWidget(self.create_pressed_button('Head Up')) head_box.addStretch(1) large_box.addLayout(head_box) button_box = QtGui.QHBoxLayout() button_box.addItem(QtGui.QSpacerItem(80, 0)) button_box.addWidget(self.create_pressed_button('Head Turn Left')) button_box.addWidget(self.create_pressed_button('Head Down')) button_box.addWidget(self.create_pressed_button('Head Turn Right')) button_box.addStretch(1) button_box.setMargin(0) button_box.setSpacing(0) large_box.addLayout(button_box) # Shows what the robot says speech_box = QtGui.QHBoxLayout() self.speech_label = QtGui.QLabel('Robot has not spoken yet') palette = QtGui.QPalette() palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue) self.speech_label.setPalette(palette) # speech_box.addItem(QtGui.QSpacerItem(100, 0)) #speech_box.addWidget(self.speech_label) # large_box.addLayout(speech_box) # Speak button speak_button_box = QtGui.QHBoxLayout() speech_prompt = QtGui.QLabel('Enter Speech Text:') speech_prompt.setFixedWidth(self.prompt_width) speak_button_box.addWidget(speech_prompt) robot_says = QtGui.QLineEdit(self._widget) robot_says.setFixedWidth(self.input_width) robot_says.textChanged[str].connect(self.onChanged) # speak_button_box.addWidget(robot_says) speak_button_box.addWidget(self.create_button('Speak')) speak_button_box.addStretch(1) large_box.addLayout(speak_button_box) large_box.addItem(QtGui.QSpacerItem(0, 50)) # Buttons for arm poses pose_button_box1 = QtGui.QHBoxLayout() pose_button_box1.addItem(QtGui.QSpacerItem(150, 0)) pose_button_box1.addWidget(self.create_button('Relax Left Arm')) pose_button_box1.addWidget(self.create_button('Relax Right Arm')) pose_button_box1.addStretch(1) large_box.addLayout(pose_button_box1) # Buttons for grippers gripper_button_box = QtGui.QHBoxLayout() gripper_button_box.addItem(QtGui.QSpacerItem(150, 0)) gripper_button_box.addWidget(self.create_button('Open Left Gripper')) gripper_button_box.addWidget(self.create_button('Open Right Gripper')) gripper_button_box.addStretch(1) large_box.addLayout(gripper_button_box) large_box.addItem(QtGui.QSpacerItem(0, 25)) # Buttons for animation animation_box = QtGui.QHBoxLayout() play_anim_label = QtGui.QLabel('Select Animation:') play_anim_label.setFixedWidth(self.prompt_width) animation_box.addWidget(play_anim_label) self.saved_animations_list = QtGui.QComboBox(self._widget) animation_box.addWidget(self.saved_animations_list) pose_time_label = QtGui.QLabel('Duration(sec):') pose_time_label.setFixedWidth(100) animation_box.addWidget(pose_time_label) self.pose_time = QtGui.QLineEdit(self._widget) self.pose_time.setFixedWidth(50) self.pose_time.setText('2.0') animation_box.addWidget(self.pose_time) animation_box.addWidget(self.create_button('Play Animation')) animation_box.addStretch(1) large_box.addLayout(animation_box) animation_box2 = QtGui.QHBoxLayout() animation_name_label = QtGui.QLabel('Enter Animation Name:') animation_name_label.setFixedWidth(self.prompt_width) animation_box2.addWidget(animation_name_label) self.animation_name = QtGui.QLineEdit(self._widget) self.animation_name.setFixedWidth(self.input_width) animation_box2.addWidget(self.animation_name) animation_box2.addWidget(self.create_button('Save Animation')) animation_box2.addStretch(1) large_box.addLayout(animation_box2) animation_box3 = QtGui.QHBoxLayout() pose_name_label = QtGui.QLabel('Enter Pose Name:') pose_name_label.setFixedWidth(self.prompt_width) animation_box3.addWidget(pose_name_label) self.pose_name_temp = QtGui.QLineEdit(self._widget) self.pose_name_temp.setFixedWidth(self.input_width) animation_box3.addWidget(self.pose_name_temp) animation_box3.addWidget(self.create_button('Add Current Pose')) animation_box3.addStretch(1) large_box.addLayout(animation_box3) # Playing around with UI stuff play_box = QtGui.QHBoxLayout() pose_sequence_label = QtGui.QLabel('Current Pose Sequence:') pose_sequence_label.setFixedWidth(self.prompt_width) pose_sequence_label.setAlignment(QtCore.Qt.AlignTop) self.list_widget = QListWidget() self.list_widget.setDragDropMode(QAbstractItemView.InternalMove) self.list_widget.setMaximumSize(self.input_width, 200) play_box.addWidget(pose_sequence_label) play_box.addWidget(self.list_widget) play_box.addStretch(1) large_box.addLayout(play_box) large_box.addItem(QtGui.QSpacerItem(0, 50)) # Buttons for first row of base controls first_base_button_box = QtGui.QHBoxLayout() first_base_button_box.addItem(QtGui.QSpacerItem(70, 0)) first_base_button_box.addWidget( self.create_pressed_button('Rotate Left')) first_base_button_box.addWidget(self.create_pressed_button('^')) first_base_button_box.addWidget( self.create_pressed_button('Rotate Right')) first_base_button_box.addStretch(1) large_box.addLayout(first_base_button_box) # Buttons for second row of base controls second_base_button_box = QtGui.QHBoxLayout() second_base_button_box.addItem(QtGui.QSpacerItem(70, 0)) second_base_button_box.addWidget(self.create_pressed_button('<')) second_base_button_box.addWidget(self.create_pressed_button('v')) second_base_button_box.addWidget(self.create_pressed_button('>')) second_base_button_box.addWidget(self.create_button('Move to Bin')) second_base_button_box.addWidget(self.create_button('Object Detect')) second_base_button_box.addWidget(self.create_button('Clean Room')) second_base_button_box.addStretch(1) large_box.addLayout(second_base_button_box) # Animation related items to store intermediate pose co-ordinates to save self.animation_map = {} self.create_state = False self._widget.setObjectName('SimpleGUI') self._widget.setLayout(large_box) context.add_widget(self._widget) # Look straight and down when launched self.head_x = 1.0 self.head_y = 0.0 self.head_z = 0.5 self.head_action(self.head_x, self.head_y, self.head_z) # Set grippers to closed on initialization self.left_gripper_state = '' self.right_gripper_state = '' self.gripper_action('l', 0.0) self.gripper_action('r', 0.0) # Lab 6 self.marker_publisher = rospy.Publisher('visualization_marker', Marker) # Saved states for poses saved_pose_box = QtGui.QHBoxLayout() self.saved_left_poses = QtGui.QLabel('') self.saved_right_poses = QtGui.QLabel('') saved_pose_box.addWidget(self.saved_left_poses) saved_pose_box.addWidget(self.saved_right_poses) large_box.addLayout(saved_pose_box) # Preload the map of animations self.saved_animations_list.addItems(self.saved_animations.keys()) # Move the torso all the way down # self.torso_down(True) # Autonomous navigation stuff ''' self.locations = [Pose(Point(2.48293590546, 3.90075874329, 0.000), Quaternion(0.000, 0.000, -0.783917630973, 0.620864838632)), Pose(Point(3.70106744766, 0.304672241211, 0.000), Quaternion(0.000, 0.000, 0.950186880196, -0.311680754463)), Pose(Point(2.57326722145, 1.51304531097, 0.000), Quaternion(0.000, 0.000, 0.96127194482, -0.275601611212)), Pose(Point(1.28060531616, 1.52380752563, 0.000), Quaternion(0.000, 0.000, 0.946345258806, -0.323157316388)), Pose(Point(2.11048603058, 0.420155525208, 0.000), Quaternion(0.000, 0.000, 0.945222393391, -0.326427062346)), Pose(Point(2.82733058929, -0.739856719971, 0.000), Quaternion(0.000, 0.000, 0.945473998362, -0.325697587373)), Pose(Point(1.29184818268, -1.90485572815, 0.000), Quaternion(0.000, 0.000, 0.942275557345, -0.334838429739)), Pose(Point(0.722846984863, -1.0921459198, 0.000), Quaternion(0.000, 0.000, 0.949330143731, -0.314280572424))] ''' self.locations = [ Pose(Point(2.04748392105, 2.04748010635, 0.000), Quaternion(0.000, 0.000, -0.776968030817, 0.629540053601)), Pose(Point(2.34193611145, 1.43208932877, 0), Quaternion(0, 0, -0.841674385779, 0.539985396398)), Pose(Point(3.43202018738, -0.258297920227, 0.000), Quaternion(0.000, 0.000, 0.996656413122, -0.0817067572629)), Pose(Point(0.931655406952, -1.96435832977, 0.000), Quaternion(0.000, 0.000, 0.691187586713, 0.722675390458)), Pose(Point(-0.369112968445, 0.0330476760864, 0.000), Quaternion(0.000, 0.000, 0.0275340398899, 0.999620866453)) ] self.index = 1 rospy.loginfo("Completed GUI initialization") # Event for when text box is changed def onChanged(self, text): self.speech_label.setText(text) self.speech_label.adjustSize() def sound_cb(self, sound_request): qWarning('Received sound.') self.sound_sig.emit(sound_request) def create_button(self, name): btn = QtGui.QPushButton(name, self._widget) btn.setFixedWidth(150) btn.clicked.connect(self.command_cb) return btn def create_pressed_button(self, name): btn = QtGui.QPushButton(name, self._widget) btn.setFixedWidth(150) btn.pressed.connect(self.command_cb) btn.setAutoRepeat(True) # To make sure the movement repeats itself return btn def sound_sig_cb(self, sound_request): qWarning('Received sound signal.') qWarning('Robot said: ' + sound_request.arg) self.speech_label.setText(sound_request.arg) #'Robot said: ' + #a button was clicked def command_cb(self): button_name = self._widget.sender().text() #robot talk button clicked if (button_name == 'Speak'): qWarning('Robot will say: ' + self.speech_label.text()) self._sound_client.say(self.speech_label.text()) self.show_text_in_rviz("Robot is Speaking") #gripper button selected elif ('Gripper' in button_name): self.gripper_click(button_name) # Move forward elif (button_name == '^'): self.base_action(0.25, 0.0, 0.0, 0.0, 0.0, 0.0) # Move left elif (button_name == '<'): self.base_action(0.0, 0.25, 0.0, 0.0, 0.0, 0.0) # Move right elif (button_name == '>'): self.base_action(0.0, -0.25, 0.0, 0.0, 0.0, 0.0) # Move back elif (button_name == 'v'): self.base_action(-0.25, 0.0, 0.0, 0.0, 0.0, 0.0) #Rotate Left elif (button_name == 'Rotate Left'): self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, 0.50) # Rotate Right elif (button_name == 'Rotate Right'): self.base_action(0.0, 0.0, 0.0, 0.0, 0.0, -0.50) # A head button selected elif ('Head' in button_name): self.rotate_head(button_name) #An arm button selected #third param unused in freeze/relax #Second word in button should be side elif ('Arm' in button_name): arm_side = button_name.split()[1] if ('Freeze' in button_name or 'Relax' in button_name): new_arm_state = button_name.split()[0] self.toggle_arm(arm_side[0].lower(), new_arm_state, True) old_arm_state = '' if (new_arm_state == 'Relax'): old_arm_state = 'Freeze' else: old_arm_state = 'Relax' self._widget.sender().setText('%s %s Arm' % (old_arm_state, arm_side)) elif ('Play Animation' == button_name): self.animPlay.left_poses = self.saved_animations[ self.saved_animations_list.currentText()].left self.animPlay.right_poses = self.saved_animations[ self.saved_animations_list.currentText()].right self.animPlay.left_gripper_states = self.saved_animations[ self.saved_animations_list.currentText()].left_gripper self.animPlay.right_gripper_states = self.saved_animations[ self.saved_animations_list.currentText()].right_gripper if self.pose_time.text() == '': self.show_warning('Please enter a duration in seconds.') else: self.animPlay.play(self.pose_time.text()) elif ('Animation' in button_name): if ('Save' in button_name): if self.animation_name.text() == '': self.show_warning('Please enter name for animation') else: self.save_animation(self.animation_name.text()) self.list_widget.clear() self.animation_name.setText('') elif ('Add Current Pose' == button_name): if self.pose_name_temp.text() == '': self.show_warning('Insert name for pose') else: self.animation_map[self.pose_name_temp.text()] = Quad( self.get_joint_state('l'), self.get_joint_state('r'), self.left_gripper_state, self.right_gripper_state) list_item = QListWidgetItem() list_item.setText(self.pose_name_temp.text()) self.list_widget.addItem(list_item) self.pose_name_temp.setText('') elif ('Move to Bin' == button_name): self.move_to_bin_action() elif ('Clean Room' == button_name): rospy.loginfo("STARTING AUTONOMOUS MODE") self.tuck_arms() while (self.index < len(self.locations)): self.roomNav.move_to_trash_location(self.locations[self.index]) # self.index += 1 self.head_action(1.0, 0, -0.50, True) # Returns Nonce if nothing, and the point of the first object it sees otherwise map_point = self.pap.detect_objects() if (map_point == None): self.index += 1 else: self.pick_and_move_trash_action() rospy.loginfo("FINISHED AUTONOMOUS MODE") self.index = 1 elif ('Object Detect' == button_name): self.pick_and_move_trash_action() def pick_and_move_trash_action(self, map_point=None): if map_point == None: self.head_action(1.0, 0, -0.50, True) map_point = self.pap.detect_objects() if map_point == None: return # Convert to base link and move towards the object 0.50m away map_point = Transformer.transform(self._tf_listener, map_point.pose, map_point.header.frame_id, '/base_link') rospy.loginfo("Object is " + str(map_point.pose.position.x) + " away") ''' if(map_point.pose.position.x < 0.8): self.roomNav.move_to_trash_location(self.locations[self.index - 1]) ''' move_back_first = False if (map_point.pose.position.x < 0.7): move_back_first = True map_point.pose.position.x -= 0.450 map_point = Transformer.transform(self._tf_listener, map_point.pose, '/base_link', '/map') if (move_back_first): self.roomNav.move_to_trash_location(self.locations[self.index - 1]) success = self.roomNav.move_to_trash_location(map_point.pose) '''This part of the code strafes the robot left to get closer to the object''' ''' rate = rospy.Rate(10) position = Point() move_cmd = Twist() move_cmd.linear.y = 0.25 odom_frame = '/odom_combined' # Find out if the robot uses /base_link or /base_footprint try: self._tf_listener.waitForTransform(odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_footprint' except (tf.Exception, tf.ConnectivityException, tf.LookupException): try: self._tf_listener.waitForTransform(odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("Cannot find transform between " + odom_frame + " and /base_link or /base_footprint") rospy.signal_shutdown("tf Exception") # Get current position position = self.get_odom() x_start = position.x y_start = position.y # Distance travelled distance = 0 goal_distance = 0.25 rospy.loginfo("Strafing left") # Enter the loop to move along a side while distance < goal_distance: rospy.loginfo("Distance is at " + str(distance)) # Publish the Twist message and sleep 1 cycle self.base_action(0, 0.25, 0, 0, 0, 0) rate.sleep() # Get the current position position = self.get_odom() # Compute the Euclidean distance from the start distance = abs(position.y - y_start) ''' if (success): # Move head to look at the object, this will wait for a result self.head_action(0, 0.4, 0.55, True) # Move arms to ready pickup position, this will wait for a result before trying to detect and pick up object self.animPlay.left_poses = self.saved_animations[ 'ready_pickup'].left self.animPlay.right_poses = self.saved_animations[ 'ready_pickup'].right self.animPlay.left_gripper_states = self.saved_animations[ 'ready_pickup'].left_gripper self.animPlay.right_gripper_states = self.saved_animations[ 'ready_pickup'].right_gripper self.animPlay.play('3.0') for i in range(0, 3): success = self.pap.detect_and_pickup() # Move head back to look forward # Move head to look at the object, this will wait for a result self.head_action(1.0, 0.0, 0.55, True) if success: break # Move to bin self.move_to_bin_action() def get_odom(self): # Get the current transform between the odom and base frames try: trans = self._tf_listener.lookupTransform('/odom_combined', self.base_frame, rospy.Time(0)) except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("TF Exception") return return Point(trans[0][0], trans[0][1], trans[0][2]) # gripper_type is either 'l' for left or 'r' for right # gripper position is the position as a parameter to the gripper goal def gripper_action(self, gripper_type, gripper_position): name_space = '/' + gripper_type + '_gripper_controller/gripper_action' gripper_client = SimpleActionClient(name_space, GripperCommandAction) gripper_client.wait_for_server() gripper_goal = GripperCommandGoal() gripper_goal.command.position = gripper_position gripper_goal.command.max_effort = 30.0 gripper_client.send_goal(gripper_goal) # update the state of the current gripper being modified if (gripper_type == 'l'): if (gripper_position == 0.0): self.left_gripper_state = 'closed' else: self.left_gripper_state = 'open' else: if (gripper_position == 0.0): self.right_gripper_state = 'closed' else: self.right_gripper_state = 'open' def base_action(self, x, y, z, theta_x, theta_y, theta_z): topic_name = '/base_controller/command' base_publisher = rospy.Publisher(topic_name, Twist) twist_msg = Twist() twist_msg.linear = Vector3(x, y, z) twist_msg.angular = Vector3(theta_x, theta_y, theta_z) base_publisher.publish(twist_msg) # Moves the torso of the robot down to its maximum def torso_down(self, wait=False): self.torso_client = SimpleActionClient( '/torso_controller/position_joint_action', SingleJointPositionAction) torso_goal = SingleJointPositionGoal() torso_goal.position = 0.0 torso_goal.min_duration = rospy.Duration(5.0) torso_goal.max_velocity = 1.0 self.torso_client.send_goal(torso_goal) if wait: # wait for the head movement to finish before we try to detect and pickup an object finished_within_time = self.torso_client.wait_for_result( rospy.Duration(15)) # Check for success or failure if not finished_within_time: self.torso_client.cancel_goal() rospy.loginfo("Timed out achieving torso movement goal") else: state = self.torso_client.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Torso movement goal succeeded!") rospy.loginfo("State:" + str(state)) else: rospy.loginfo( "Torso movement goal failed with error code: " + str(state)) def head_action(self, x, y, z, wait=False): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_goal = PointHeadGoal() head_goal.target.header.frame_id = 'base_link' head_goal.min_duration = rospy.Duration(1.0) head_goal.target.point = Point(x, y, z) head_client.send_goal(head_goal) if wait: # wait for the head movement to finish before we try to detect and pickup an object finished_within_time = head_client.wait_for_result( rospy.Duration(5)) # Check for success or failure if not finished_within_time: head_client.cancel_goal() rospy.loginfo("Timed out achieving head movement goal") else: state = head_client.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Head movement goal succeeded!") rospy.loginfo("State:" + str(state)) else: rospy.loginfo( "Head movement goal failed with error code: " + str(self.goal_states[state])) def tuck_arms(self): rospy.loginfo('Left tuck arms') self.animPlay.left_poses = self.saved_animations['left_tuck'].left self.animPlay.right_poses = self.saved_animations['left_tuck'].right self.animPlay.left_gripper_states = self.saved_animations[ 'left_tuck'].left_gripper self.animPlay.right_gripper_states = self.saved_animations[ 'left_tuck'].right_gripper self.animPlay.play('3.0') def move_to_bin_action(self): # First tuck arms self.tuck_arms() # Move to the bin rospy.loginfo('Clicked the move to bin button') self.roomNav.move_to_bin() # Throw the trash away rospy.loginfo('Throwing trash away with left arm') self.animPlay.left_poses = self.saved_animations['l_dispose'].left self.animPlay.right_poses = self.saved_animations['l_dispose'].right self.animPlay.left_gripper_states = self.saved_animations[ 'l_dispose'].left_gripper self.animPlay.right_gripper_states = self.saved_animations[ 'l_dispose'].right_gripper self.animPlay.play('2.0') # Tuck arms again self.tuck_arms() def shutdown_plugin(self): # TODO unregister all publishers here pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass def show_text_in_rviz(self, text): marker = Marker(type=Marker.TEXT_VIEW_FACING, id=0, lifetime=rospy.Duration(1.5), pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)), scale=Vector3(0.06, 0.06, 0.06), header=Header(frame_id='base_link'), color=ColorRGBA(0.0, 1.0, 0.0, 0.8), text=text) self.marker_publisher.publish(marker) def show_arrow_in_rviz(self, arrow): marker = Marker(type=Marker.ARROW, id=0, lifetime=rospy.Duration(1.5), pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)), scale=Vector3(0.06, 0.06, 0.06), header=Header(frame_id='base_link'), color=ColorRGBA(0.0, 1.0, 0.0, 0.8), arrow=arrow) self.marker_publisher.publish(marker) def save_animation(self, animation_name): if animation_name != '': filename = os.path.join(self.dir, 'animations/') anim_file = open(filename + animation_name + '.txt', 'w') l_animation_queue = [] r_animation_queue = [] l_gripper_queue = [] r_gripper_queue = [] for i in range(self.list_widget.count()): item = self.list_widget.item(i) pose_name = item.text() anim_file.write( re.sub( ',', '', str(self.animation_map[pose_name].left).strip('[]') + ' ' + str(self.animation_map[pose_name].right).strip('[]'))) anim_file.write('\n') anim_file.write( str(self.animation_map[pose_name].left_gripper) + ' ' + str(self.animation_map[pose_name].right_gripper)) l_animation_queue.append(self.animation_map[pose_name].left) r_animation_queue.append(self.animation_map[pose_name].right) l_gripper_queue.append( self.animation_map[pose_name].left_gripper) r_gripper_queue.append( self.animation_map[pose_name].right_gripper) anim_file.write('\n') anim_file.close() self.saved_animations[animation_name] = Quad( l_animation_queue, r_animation_queue, l_gripper_queue, r_gripper_queue) self.saved_animations_list.addItem( animation_name) # Bug? Multiple entries # Reset the pending queue self.l_animation_queue = [] self.r_animation_queue = [] else: self.show_warning('Please insert name for animation') def gripper_click(self, button_name): grip_side = '' grip_side_text = '' if ('Left' in button_name): grip_side = 'l' grip_side_text = 'left' else: grip_side = 'r' grip_side_text = 'right' if ('Open' in button_name): grip_action = 20.0 grip_action_text = 'close' qWarning('Robot opened %s gripper' % (grip_side_text)) else: grip_action = 0.0 grip_action_text = 'open' qWarning('Robot closed %s gripper' % (grip_side_text)) self.show_text_in_rviz( "%sing %s Gripper" % (grip_action_text.capitalize(), grip_side_text.capitalize())) self.gripper_action(grip_side, grip_action) self._widget.sender().setText( '%s %s Gripper' % (grip_action_text.capitalize(), grip_side_text.capitalize())) def rotate_head(self, button_name): if ('Left' in button_name): #qWarning('x: %s, y: %s' % (self.head_x, self.head_y)) if (self.head_x < -0.8 and self.head_y > 0.0): self.show_warning('Can\'t rotate anymore') elif (self.head_y < 0.0): self.head_x += 0.1 self.head_y = -((1.0 - self.head_x**2.0)**0.5) self.show_text_in_rviz("Turning Head Left") else: self.head_x -= 0.1 self.head_y = (1.0 - self.head_x**2.0)**0.5 self.show_text_in_rviz("Turning Head Left") qWarning('x: %s, y: %s' % (self.head_x, self.head_y)) self.head_action(self.head_x, self.head_y, self.head_z) elif ('Up' in button_name): if (self.head_z <= 1.6): self.head_z += 0.1 self.show_text_in_rviz("Moving Head Up") self.head_action(self.head_x, self.head_y, self.head_z) else: self.show_warning('Can\'t look up anymore') elif ('Down' in button_name): if (self.head_z >= -2.2): self.head_z -= 0.1 self.show_text_in_rviz("Moving Head Down") self.head_action(self.head_x, self.head_y, self.head_z) else: self.show_warning('Can\'t look down anymore') else: #qWarning('x: %s, y: %s' % (self.head_x, self.head_y)) if (self.head_x < -0.8 and self.head_y < 0.0): self.show_warning('Can\'t rotate anymore') elif (self.head_y > 0.0): self.head_x += 0.1 self.head_y = (1.0 - self.head_x**2.0)**0.5 self.show_text_in_rviz("Turning Head Right") else: self.head_x -= 0.1 self.head_y = -((1.0 - self.head_x**2.0)**0.5) self.show_text_in_rviz("Turning Head Right") #qWarning('x: %s, y: %s' % (self.head_x, self.head_y)) self.head_action(self.head_x, self.head_y, self.head_z) def toggle_arm(self, side, toggle, button): controller_name = side + '_arm_controller' start_controllers = [] stop_controllers = [] if (toggle == 'Relax'): stop_controllers.append(controller_name) else: start_controllers.append(controller_name) self.set_arm_mode(start_controllers, stop_controllers) def set_arm_mode(self, start_controllers, stop_controllers): try: self.switch_service_client(start_controllers, stop_controllers, 1) except rospy.ServiceException: rospy.logerr('Could not change arm mode.') def joint_states_cb(self, msg): '''Callback function that saves the joint positions when a joint_states message is received''' self.lock.acquire() self.all_joint_names = msg.name self.all_joint_poses = msg.position self.joint_sig.emit(msg) self.lock.release() def joint_sig_cb(self, msg): pass def get_joint_state(self, side_prefix): '''Returns position for arm joints on the requested side (r/l)''' if side_prefix == 'r': joint_names = self.r_joint_names else: joint_names = self.l_joint_names if self.all_joint_names == []: rospy.logerr("No robot_state messages received yet!\n") return None positions = [] self.lock.acquire() for joint_name in joint_names: if joint_name in self.all_joint_names: index = self.all_joint_names.index(joint_name) position = self.all_joint_poses[index] positions.append(position) else: rospy.logerr("Joint %s not found!", joint_name) self.lock.release() return None self.lock.release() return positions def show_warning(self, text): qWarning(text) msgBox = QMessageBox() msgBox.setText(text) msgBox.exec_()
class PbDGUI(Plugin): exp_state_sig = Signal(ExperimentState) def __init__(self, context): super(PbDGUI, self).__init__(context) self.setObjectName('PbDGUI') self._widget = QWidget() self.speech_cmd_publisher = rospy.Publisher('recognized_command', Command) self.gui_cmd_publisher = rospy.Publisher('gui_command', GuiCommand) rospy.Subscriber('experiment_state', ExperimentState, self.exp_state_cb) rospy.Subscriber('robotsound', SoundRequest, self.robotSoundReceived) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.exp_state_sig.connect(self.update_state) self.commands = dict() self.commands[Command.TEST_MICROPHONE] = 'Test microphone' self.commands[Command.START_RECORDING] = 'Start recording' self.commands[Command.STOP_RECORDING] = 'Stop recording' self.commands[Command.REPLAY_DEMONSTRATION] = 'Replay demonstration' self.commands[Command.TAKE_TOOL] = 'Take tool' self.commands[Command.RELEASE_TOOL] = 'Release tool' self.currentAction = -1 self.currentStep = -1 allWidgetsBox = QtGui.QVBoxLayout() actionBox = QGroupBox('Demonstrations', self._widget) self.actionGrid = QtGui.QGridLayout() self.actionGrid.setHorizontalSpacing(0) for i in range(6): self.actionGrid.addItem(QtGui.QSpacerItem(90, 90), 0, i, QtCore.Qt.AlignCenter) self.actionGrid.setColumnStretch(i, 0) self.actionIcons = dict() actionBoxLayout = QtGui.QHBoxLayout() actionBoxLayout.addLayout(self.actionGrid) actionBox.setLayout(actionBoxLayout) actionButtonGrid = QtGui.QHBoxLayout() self.stepsBox = QGroupBox('No demonstrations', self._widget) self.stepsGrid = QtGui.QGridLayout() self.l_model = QtGui.QStandardItemModel(self) self.l_view = self._create_table_view(self.l_model, self.l_row_clicked_cb) self.r_model = QtGui.QStandardItemModel(self) self.r_view = self._create_table_view(self.r_model, self.r_row_clicked_cb) self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 0, 2, 3) self.stepsGrid.addItem(QtGui.QSpacerItem(10, 10), 0, 1, 2, 3) self.stepsGrid.addItem(QtGui.QSpacerItem(280, 10), 0, 2, 2, 3) self.stepsGrid.addWidget(QtGui.QLabel('Left Arm'), 0, 0) self.stepsGrid.addWidget(QtGui.QLabel('Right Arm'), 0, 2) self.stepsGrid.addWidget(self.l_view, 1, 0) self.stepsGrid.addWidget(self.r_view, 1, 2) stepsBoxLayout = QtGui.QHBoxLayout() stepsBoxLayout.addLayout(self.stepsGrid) self.stepsBox.setLayout(stepsBoxLayout) stepsButtonGrid = QtGui.QHBoxLayout() stepsButtonGrid.addWidget(self.create_button(Command.REPLAY_DEMONSTRATION)) motionButtonGrid = QtGui.QHBoxLayout() motionButtonGrid.addWidget(self.create_button(Command.START_RECORDING)) motionButtonGrid.addWidget(self.create_button(Command.STOP_RECORDING)) misc_grid = QtGui.QHBoxLayout() misc_grid.addWidget(self.create_button(Command.TEST_MICROPHONE)) misc_grid.addStretch(1) misc_grid2 = QtGui.QHBoxLayout() misc_grid2.addWidget(self.create_button(Command.TAKE_TOOL)) misc_grid2.addWidget(self.create_button(Command.RELEASE_TOOL)) misc_grid2.addStretch(1) speechGroupBox = QGroupBox('Robot Speech', self._widget) speechGroupBox.setObjectName('RobotSpeechGroup') speechBox = QtGui.QHBoxLayout() self.speechLabel = QtGui.QLabel('Robot has not spoken yet') palette = QtGui.QPalette() palette.setColor(QtGui.QPalette.Foreground,QtCore.Qt.blue) self.speechLabel.setPalette(palette) speechBox.addWidget(self.speechLabel) speechGroupBox.setLayout(speechBox) allWidgetsBox.addWidget(actionBox) allWidgetsBox.addLayout(actionButtonGrid) allWidgetsBox.addWidget(self.stepsBox) allWidgetsBox.addLayout(motionButtonGrid) allWidgetsBox.addLayout(stepsButtonGrid) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addLayout(misc_grid) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 20)) allWidgetsBox.addLayout(misc_grid2) allWidgetsBox.addItem(QtGui.QSpacerItem(100, 40)) allWidgetsBox.addWidget(speechGroupBox) allWidgetsBox.addStretch(1) # Fix layout and add main widget to the user interface QtGui.QApplication.setStyle(QtGui.QStyleFactory.create('plastique')) vAllBox = QtGui.QVBoxLayout() vAllBox.addLayout(allWidgetsBox) vAllBox.addStretch(1) hAllBox = QtGui.QHBoxLayout() hAllBox.addLayout(vAllBox) hAllBox.addStretch(1) self._widget.setObjectName('PbDGUI') self._widget.setLayout(hAllBox) context.add_widget(self._widget) rospy.loginfo('Will wait for the experiment state service...') rospy.wait_for_service('get_experiment_state') exp_state_srv = rospy.ServiceProxy('get_experiment_state', GetExperimentState) rospy.loginfo('Got response from the experiment state service...') response = exp_state_srv() self.update_state(response.state) def _create_table_view(self, model, row_click_cb): proxy = QtGui.QSortFilterProxyModel(self) proxy.setSourceModel(model) view = QtGui.QTableView(self._widget) verticalHeader = view.verticalHeader() verticalHeader.sectionClicked.connect(row_click_cb) view.setModel(proxy) view.setMaximumWidth(250) view.setSortingEnabled(False) view.setCornerButtonEnabled(False) return view def get_uid(self, arm_index, index): '''Returns a unique id of the marker''' return (2 * (index + 1) + arm_index) def get_arm_and_index(self, uid): '''Returns a unique id of the marker''' arm_index = uid % 2 index = (uid - arm_index) / 2 return (arm_index, (index - 1)) def r_row_clicked_cb(self, logicalIndex): self.step_pressed(self.get_uid(0, logicalIndex)) def l_row_clicked_cb(self, logicalIndex): self.step_pressed(self.get_uid(1, logicalIndex)) def create_button(self, command): btn = QtGui.QPushButton(self.commands[command], self._widget) btn.clicked.connect(self.command_cb) return btn def update_state(self, state): qWarning('Received new state') n_actions = len(self.actionIcons.keys()) if n_actions < state.n_actions: for i in range(n_actions, state.n_actions): self.new_action() if (self.currentAction != (state.i_current_action - 1)): self.delete_all_steps() self.action_pressed(state.i_current_action - 1, False) n_steps = self.n_steps() if (n_steps < state.n_steps): for i in range(n_steps, state.n_steps): self.save_pose(frameType=ord(state.frame_types[i])) elif (n_steps > state.n_steps): n_to_remove = n_steps - state.n_steps self.r_model.invisibleRootItem().removeRows(state.n_steps, n_to_remove) self.l_model.invisibleRootItem().removeRows(state.n_steps, n_to_remove) ## TODO: DEAL with the following arrays!!! state.r_gripper_states state.l_gripper_states state.r_ref_frames state.l_ref_frames state.objects if (self.currentStep != state.i_current_step): if (self.n_steps() > 0): self.currentStep = state.i_current_step arm_index, index = self.get_arm_and_index(self.currentStep) if (arm_index == 0): self.r_view.selectRow(index) else: self.l_view.selectRow(index) def get_frame_type(self, fr_type): if (fr_type > 1): rospy.logwarn("Invalid frame type @ save_pose -> get_frame_type: " + str(fr_type)) return ["Go to pose", "Maneuver"][fr_type] def save_pose(self, actionIndex=None, frameType=0): nColumns = 9 if actionIndex is None: actionIndex = self.currentAction stepIndex = self.n_steps(actionIndex) r_step = [QtGui.QStandardItem('Step' + str(stepIndex + 1)), QtGui.QStandardItem(self.get_frame_type(frameType)), QtGui.QStandardItem('Absolute')] l_step = [QtGui.QStandardItem('Step' + str(stepIndex + 1)), QtGui.QStandardItem(self.get_frame_type(frameType)), QtGui.QStandardItem('Absolute')] self.r_model.invisibleRootItem().appendRow(r_step) self.l_model.invisibleRootItem().appendRow(l_step) self.update_table_view() self.currentStep = stepIndex def update_table_view(self): self.l_view.setColumnWidth(0, 50) self.l_view.setColumnWidth(1, 100) self.l_view.setColumnWidth(2, 70) self.r_view.setColumnWidth(0, 50) self.r_view.setColumnWidth(1, 100) self.r_view.setColumnWidth(2, 70) def n_steps(self, actionIndex=None): return self.l_model.invisibleRootItem().rowCount() def delete_all_steps(self, actionIndex=None): if actionIndex is None: actionIndex = self.currentAction n_steps = self.n_steps() if (n_steps > 0): self.l_model.invisibleRootItem().removeRows(0, n_steps) self.r_model.invisibleRootItem().removeRows(0, n_steps) def n_actions(self): return len(self.actionIcons.keys()) def new_action(self): nColumns = 6 actionIndex = self.n_actions() for key in self.actionIcons.keys(): self.actionIcons[key].selected = False self.actionIcons[key].updateView() actIcon = ActionIcon(self._widget, actionIndex, self.action_pressed) self.actionGrid.addLayout(actIcon, int(actionIndex/nColumns), actionIndex%nColumns) self.actionIcons[actionIndex] = actIcon def step_pressed(self, step_index): gui_cmd = GuiCommand(GuiCommand.SELECT_ACTION_STEP, step_index) self.gui_cmd_publisher.publish(gui_cmd) def action_pressed(self, actionIndex, isPublish=True): for i in range(len(self.actionIcons.keys())): key = self.actionIcons.keys()[i] if key == actionIndex: self.actionIcons[key].selected = True else: self.actionIcons[key].selected = False self.actionIcons[key].updateView() self.currentAction = actionIndex self.stepsBox.setTitle('Steps for Action ' + str(self.currentAction+1)) if isPublish: gui_cmd = GuiCommand(GuiCommand.SWITCH_TO_ACTION, (actionIndex+1)) self.gui_cmd_publisher.publish(gui_cmd) def command_cb(self): clickedButtonName = self._widget.sender().text() for key in self.commands.keys(): if (self.commands[key] == clickedButtonName): qWarning('Sending speech command: '+ key) command = Command() command.command = key self.speech_cmd_publisher.publish(command) def robotSoundReceived(self, soundReq): if (soundReq.command == SoundRequest.SAY): qWarning('Robot said: ' + soundReq.arg) self.speechLabel.setText('Robot sound: ' + soundReq.arg) def exp_state_cb(self, state): qWarning('Received new experiment state.') self.exp_state_sig.emit(state) def shutdown_plugin(self): # TODO unregister all publishers here self.speech_cmd_publisher.unregister() self.gui_cmd_publisher.unregister() pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class OculusTeleopGUI(Plugin): joint_sig = Signal(JointState) sound_sig = Signal(SoundRequest) # Constants to help us move the robot HEAD_THETA = 0.0 HEAD_Z = 1.0 LEFT_GRIPPER_POSITION = 0.0 RIGHT_GRIPPER_POSITION = 0.0 GRIPPER_INTENSITY = 0.0 MOTION_INTENSITY = 0.0 ARM_INTENSITY = 0.0 r_arm_pose_box = QtGui.QComboBox() l_arm_pose_box = QtGui.QComboBox() def __init__(self, context): super(OculusTeleopGUI, self).__init__(context) self.setObjectName('OculusTeleopGUI') self._widget = QWidget() self.saved_r_arm_pose = dict() self.saved_l_arm_pose = dict() self.switchnode = SwitchNode() self.armnode = ArmNode() self.grippernode = GripperNode() self.speaknode = SpeakNode() # Logging variables self.fileName = "" self.fileNum = 1 self.isLogging = False self.logRate = 10.0 # Hz # Add Clients self.marker_publisher = rospy.Publisher('visualization_marker', Marker) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.sound_sig.connect(self.sound_sig_cb) # Main layout element for GUI large_box = QtGui.QVBoxLayout() palette = QtGui.QPalette() palette.setColor(QtGui.QPalette.Foreground,QtCore.Qt.blue) # Create Button box for GUI button_box = QtGui.QGridLayout() # Add The Buttons to button box here button_box.addWidget(self.create_button('Stack Init'), 1, 1) button_box.addWidget(self.create_button('Drive Init'), 1, 3) button_box.addWidget(self.create_button('Set Rate'), 3, 1) button_box.addWidget(self.create_button('Get Rate'), 3, 3) button_box.addWidget(self.create_button('Start Log'), 5, 1) button_box.addWidget(self.create_button('Stop Log'), 5, 3) # Text box for logging rate self.rate_label = QtGui.QLabel('Enter Logging Rate (Hz): ') self.rate_label.setPalette(palette) button_box.addWidget(self.rate_label, 2, 1) self.rate_text_in = QtGui.QLineEdit('10') button_box.addWidget(self.rate_text_in, 2, 3) # Text box for file names self.file_label = QtGui.QLabel('Enter Log File Name: ') self.file_label.setPalette(palette) button_box.addWidget(self.file_label, 4, 1) self.file_text_in = QtGui.QLineEdit('Default') button_box.addWidget(self.file_text_in, 4, 3) # Add Button Box to main GUI layout element large_box.addLayout(button_box) large_box.addItem(QtGui.QSpacerItem(100,20)) # Speech box for speech display created speech_box = QtGui.QVBoxLayout() self.speech_label = QtGui.QLabel('Robot is ready to run, press button to start') self.speech_label.setPalette(palette) speech_box.addWidget(self.speech_label) # Speech box added to main layout large_box.addLayout(speech_box) # Puts layout in widget and adds widget name for RosGUI to identify self._widget.setObjectName('OculusTeleopGUI') self._widget.setLayout(large_box) context.add_widget(self._widget) rospy.loginfo('GUI initialization complete.') # Will create a button, with all buttons using same event trigger def create_button(self, name): btn = QtGui.QPushButton(name, self._widget) if (name == 'Stack Init'): btn.clicked.connect(self.command_cb) elif (name == 'Drive Init'): btn.clicked.connect(self.command_cb) elif (name == 'Set Rate'): btn.clicked.connect(self.command_cb) elif (name == 'Get Rate'): btn.clicked.connect(self.command_cb) elif (name == 'Start Log'): btn.clicked.connect(self.command_cb) elif (name == 'Stop Log'): btn.clicked.connect(self.command_cb) return btn # Triggered when request to sound_play is made def sound_sig_cb(self, sound_request): qWarning('Received sound signal.') #if (sound_request.command == SoundRequest.SAY): qWarning('Robot said: ' + sound_request.arg) # self.speech_label.setText('Robot said: ' + sound_request.arg) # Triggered When Subscribber to robot sound detects a message def sound_cb(self, sound_request): qWarning('Received sound.') self.sound_sig.emit(sound_request) return # Triggered when button is pressed: Place response actions here def command_cb(self): button_name = self._widget.sender().text() speechLabel = ''; if (button_name == 'Stack Init'): self.armnode.stack_init_motion() speechLabel = 'is initializing arms for stacking'; elif (button_name == 'Drive Init'): self.armnode.fold_motion() speechLabel = 'is initializing arms for driving'; elif (button_name == 'Set Rate'): self.setRate() speechLabel = 'set logging rate to ' + str(self.logRate) + 'Hz.' elif (button_name == 'Get Rate'): qWarning('Logging Rate set to ' + str(self.logRate) + 'Hz') speechLabel = 'is ready to log at ' + str(self.logRate) + 'Hz.' elif (button_name == 'Start Log'): self.start_logger() speechLabel = 'has started logging.' elif (button_name == 'Stop Log'): self.logger.closeAllBags() self.isLogging = False speechLabel = 'has stopped logging.' self.speech_label.setText('Robot ' + speechLabel) def start_logger(self): if (not self.isLogging): name = self.file_text_in.text() if (self.fileName == name): self.fileNum += 1 else: self.fileNum = 1 self.fileName = name self.logger = Logger(self.fileName, str(self.fileNum), 1.0/self.logRate) rospy.on_shutdown(self.logger.closeAllBags) self.isLogging = True def setRate(self): if (not self.isLogging): rate = self.rate_text_in.text() self.logRate = float(rate) def shutdown_plugin(self): # TODO unregister all publishers here # Leave both arm controllers on start_controllers = ['r_arm_controller', 'l_arm_controller'] stop_controllers = [] self.armnode.set_arm_mode(start_controllers, stop_controllers, self.switchnode) def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class SimpleGUI(Plugin): sound_sig = Signal(SoundRequest) def __init__(self, context): super(SimpleGUI, self).__init__(context) self.setObjectName('SimpleGUI') self._widget = QWidget() self._widget.setFixedSize(600, 600) self._sound_client = SoundClient() rospy.Subscriber('robotsound', SoundRequest, self.sound_cb) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.sound_sig.connect(self.sound_sig_cb) large_box = QtGui.QVBoxLayout() # Textbox to enter words for PR2 to say sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") # Default Text sound_textbox.setFixedWidth(450) self.marker_publisher = rospy.Publisher('visualization_marker', Marker) # Set a handler on the textbox to retrieve the text when button clicked self.sound_textbox = sound_textbox button_box = QtGui.QHBoxLayout() button_box.addItem(QtGui.QSpacerItem(15, 20)) button_box.addWidget(self.create_button('Speak', self.command_cb)) button_box.addWidget(sound_textbox) button_box.addStretch(1) large_box.addLayout(button_box) speech_box = QtGui.QHBoxLayout() speech_box.addItem(QtGui.QSpacerItem(15, 20)) self.speech_label = QtGui.QLabel('Robot has not spoken yet') palette = QtGui.QPalette() palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue) self.speech_label.setPalette(palette) speech_box.addWidget(self.speech_label) large_box.addLayout(speech_box) large_box.addStretch(1) #large_box.addItem(QtGui.QSpacerItem(50,20)) # Buttons to move the PR2's head up_head = Head(Head.UP, self) head_box = QtGui.QVBoxLayout() up_head_box = QtGui.QHBoxLayout() up_head_button = self.create_button('^', up_head.create_closure()) down_head = Head(Head.DOWN, self) down_head_box = QtGui.QHBoxLayout() down_head_button = self.create_button('v', down_head.create_closure()) right_head = Head(Head.RIGHT, self) right_head_button = self.create_button('>', right_head.create_closure()) left_head = Head(Head.LEFT, self) left_head_button = self.create_button('<', left_head.create_closure()) left_right_head_box = QtGui.QHBoxLayout() up_head_box.addItem(QtGui.QSpacerItem(235, 20)) up_head_box.addWidget(up_head_button) up_head_box.addItem(QtGui.QSpacerItem(275, 20)) left_right_head_box.addItem(QtGui.QSpacerItem(160, 20)) left_right_head_box.addWidget(left_head_button) left_right_head_box.addItem(QtGui.QSpacerItem(60, 20)) left_right_head_box.addWidget(right_head_button) left_right_head_box.addItem(QtGui.QSpacerItem(225, 20)) down_head_box.addItem(QtGui.QSpacerItem(235, 20)) down_head_box.addWidget(down_head_button) down_head_box.addItem(QtGui.QSpacerItem(275, 20)) head_box.addLayout(up_head_box) head_box.addLayout(left_right_head_box) head_box.addLayout(down_head_box) large_box.addLayout(head_box) # Buttons to move the grippers gripper = Gripper(Gripper.RIGHT, Gripper.OPEN, self) right_gripper = self.create_button('Right gripper', gripper.create_closure()) gripper = Gripper(Gripper.LEFT, Gripper.OPEN, self) left_gripper = self.create_button('Left gripper', gripper.create_closure()) large_box.addItem(QtGui.QSpacerItem(100, 250)) gripper_box = QtGui.QHBoxLayout() gripper_box.addItem(QtGui.QSpacerItem(75, 20)) gripper_box.addWidget(left_gripper) gripper_box.addItem(QtGui.QSpacerItem(450, 20)) gripper_box.addWidget(right_gripper) gripper_box.addItem(QtGui.QSpacerItem(75, 20)) large_box.addLayout(gripper_box) # Buttons to move the base base_box = QtGui.QVBoxLayout() large_box.addItem(QtGui.QSpacerItem(100, 100)) forward_base_box = QtGui.QHBoxLayout() forward_base = Base(Base.FORWARD, self) forward_base_button = self.create_button('move forward', forward_base.create_closure()) forward_base_box.addItem(QtGui.QSpacerItem(400, 20)) forward_base_box.addWidget(forward_base_button) forward_base_box.addItem(QtGui.QSpacerItem(400, 20)) base_box.addLayout(forward_base_box) left_right_base_box = QtGui.QHBoxLayout() left_base = Base(Base.LEFT, self) left_base_button = self.create_button('move left', left_base.create_closure()) right_base = Base(Base.RIGHT, self) right_base_button = self.create_button('move right', right_base.create_closure()) left_right_base_box.addItem(QtGui.QSpacerItem(300, 20)) left_right_base_box.addWidget(left_base_button) left_right_base_box.addItem(QtGui.QSpacerItem(50, 20)) left_right_base_box.addWidget(right_base_button) left_right_base_box.addItem(QtGui.QSpacerItem(300, 20)) base_box.addLayout(left_right_base_box) backward_base_box = QtGui.QHBoxLayout() backward_base = Base(Base.BACKWARD, self) backward_base_button = self.create_button( 'move backward', backward_base.create_closure()) backward_base_box.addItem(QtGui.QSpacerItem(400, 20)) backward_base_box.addWidget(backward_base_button) backward_base_box.addItem(QtGui.QSpacerItem(400, 20)) base_box.addLayout(backward_base_box) large_box.addLayout(base_box) turn_base_box = QtGui.QHBoxLayout() counter_base = Base(Base.COUNTER, self) counter_base_button = self.create_button('\\\n -->', counter_base.create_closure()) clockwise_base = Base(Base.CLOCKWISE, self) clockwise_base_button = self.create_button( ' /\n<--', clockwise_base.create_closure()) turn_base_box.addItem(QtGui.QSpacerItem(75, 20)) turn_base_box.addWidget(counter_base_button) turn_base_box.addItem(QtGui.QSpacerItem(225, 20)) turn_base_box.addWidget(clockwise_base_button) turn_base_box.addItem(QtGui.QSpacerItem(100, 20)) large_box.addLayout(turn_base_box) self._widget.setObjectName('SimpleGUI') self._widget.setLayout(large_box) context.add_widget(self._widget) self._widget.setStyleSheet( "QWidget { image: url(%s) }" % (str(os.path.dirname(os.path.realpath(__file__))) + "/../../rosie_background.jpg")) def show_text_in_rviz(self, text): marker = Marker(type=Marker.TEXT_VIEW_FACING, id=0, lifetime=rospy.Duration(1.5), pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)), scale=Vector3(0.06, 0.06, 0.06), header=Header(frame_id='base_link'), color=ColorRGBA(0.0, 1.0, 0.0, 0.8), text=text) self.marker_publisher.publish(marker) def sound_cb(self, sound_request): qWarning('Received sound.') self.sound_sig.emit(sound_request) def create_button(self, name, method): btn = QtGui.QPushButton(name, self._widget) btn.clicked.connect(method) btn.setAutoRepeat(True) return btn def sound_sig_cb(self, sound_request): qWarning('Received sound signal.') #if (sound_request.command == SoundRequest.SAY): qWarning('Robot said: ' + sound_request.arg) self.speech_label.setText('Robot said: ' + sound_request.arg) def command_cb(self): button_name = self._widget.sender().text() if (button_name == 'Speak'): qWarning('Robot will say: ' + self.sound_textbox.text()) self._sound_client.say(self.sound_textbox.text()) def shutdown_plugin(self): # TODO unregister all publishers here pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class SimpleGUI(Plugin): sound_sig = Signal(SoundRequest) def __init__(self, context): super(SimpleGUI, self).__init__(context) self.setObjectName('SimpleTeleopGUI') self._widget = QWidget() # INIT CLIENTS self._gripper_client = GripperClient() self._base_publisher = BasePublisher() self._sound_client = SoundClient() self._head_client = HeadClient() rospy.Subscriber('robotsound', SoundRequest, self.sound_cb) self.sound_sig.connect(self.sound_sig_cb) self._base_speed = 0.5 # OVERALL LAYOUT large_box = QtGui.QVBoxLayout() upper_box = QtGui.QVBoxLayout() large_box.addLayout(upper_box) lower_box = QtGui.QHBoxLayout() large_box.addLayout(lower_box) gripper_box = QtGui.QHBoxLayout() lower_box.addLayout(gripper_box) # GRIPPER CONTROLS gripper_box_label = QtGui.QLabel('Grippers: ') gripper_box.addWidget(gripper_box_label) gripper_left_btns = QtGui.QVBoxLayout() g_left_open_btn = QtGui.QPushButton('Open L', self._widget) g_left_open_btn.clicked.connect(self.gripper_cb) gripper_left_btns.addWidget(g_left_open_btn) g_left_close_btn = QtGui.QPushButton('Close L', self._widget) g_left_close_btn.clicked.connect(self.gripper_cb) gripper_left_btns.addWidget(g_left_close_btn) gripper_box.addLayout(gripper_left_btns) gripper_right_btns = QtGui.QVBoxLayout() g_right_open_btn = QtGui.QPushButton('Open R', self._widget) g_right_open_btn.clicked.connect(self.gripper_cb) gripper_right_btns.addWidget(g_right_open_btn) g_right_close_btn = QtGui.QPushButton('Close R', self._widget) g_right_close_btn.clicked.connect(self.gripper_cb) gripper_right_btns.addWidget(g_right_close_btn) gripper_box.addLayout(gripper_right_btns) lower_box.addItem(QtGui.QSpacerItem(100,20)) # SPEECH CONTROLS speech_box = QtGui.QHBoxLayout() speech_label = QtGui.QLabel('Speech: ') speech_box.addWidget(speech_label) self.speech_text = QtGui.QLineEdit(self._widget); speech_box.addWidget(self.speech_text); speech_box.addWidget(self.create_btn('Speak')) speech_box.addStretch(1) lower_box.addLayout(speech_box) large_box.addItem(QtGui.QSpacerItem(100,20)) # speech_box = QtGui.QHBoxLayout() # self.speech_label = QtGui.QLabel('Robot has not spoken yet') # palette = QtGui.QPalette() # palette.setColor(QtGui.QPalette.Foreground,QtCore.Qt.blue) # self.speech_label.setPalette(palette) # speech_box.addWidget(self.speech_label) # large_box.addLayout(speech_box) # BASE MOVEMENT CONTROLS base_controls = QtGui.QHBoxLayout() base_label = QtGui.QLabel('Base Movement: ') base_label.setGeometry(100,100,100,100) move_speed_controls = QtGui.QVBoxLayout() move_speed_controls.addWidget(base_label) move_speed_label = QtGui.QLabel('Move Speed') move_speed_controls.addWidget(move_speed_label) move_speed_sldr = self.create_vert_sldr() move_speed_sldr.valueChanged[int].connect(self._change_move_speed) move_speed_controls.addWidget(move_speed_sldr) move_base_controls = QtGui.QVBoxLayout() move_fwd_controls = QtGui.QHBoxLayout() move_fwd_right_btn = self.create_toggle_btn("Forward Left") move_fwd_right_btn.clicked.connect(self.make_move_base_cb(0.3)) move_fwd_controls.addWidget(move_fwd_right_btn) move_fwd_btn = self.create_toggle_btn("Forward") move_fwd_btn.clicked.connect(self.make_move_base_cb(0.3)) move_fwd_controls.addWidget(move_fwd_btn) move_fwd_left_btn = self.create_toggle_btn("Forward Right") move_fwd_left_btn.clicked.connect(self.make_move_base_cb(0.3)) move_fwd_controls.addWidget(move_fwd_left_btn) turn_controls = QtGui.QHBoxLayout() turn_left_btn = self.create_toggle_btn("Turn Left") turn_left_btn.clicked.connect(self.make_move_base_cb(0.3)) turn_controls.addWidget(turn_left_btn) stop_btn = self.create_toggle_btn("STOP") turn_controls.addWidget(stop_btn) turn_right_btn = self.create_toggle_btn("Turn Right") turn_right_btn.clicked.connect(self.make_move_base_cb(0.3)) turn_controls.addWidget(turn_right_btn) move_bkwd_controls = QtGui.QHBoxLayout() move_bkwd_right_btn = self.create_toggle_btn("Backward Left") move_bkwd_right_btn.clicked.connect(self.make_move_base_cb(0.3)) move_bkwd_controls.addWidget(move_bkwd_right_btn) move_bkwd_btn = self.create_toggle_btn("Backward") move_bkwd_btn.clicked.connect(self.make_move_base_cb(0.3)) move_bkwd_controls.addWidget(move_bkwd_btn) move_bkwd_left_btn = self.create_toggle_btn("Backward Right") move_bkwd_left_btn.clicked.connect(self.make_move_base_cb(0.3)) move_bkwd_controls.addWidget(move_bkwd_left_btn) move_base_controls.addLayout(move_fwd_controls) move_base_controls.addLayout(turn_controls) move_base_controls.addLayout(move_bkwd_controls) base_controls.addLayout(move_speed_controls) base_controls.addLayout(move_base_controls) upper_box.addLayout(base_controls) # HEAD CONTROLS move_head_controls = QtGui.QHBoxLayout() head_label = QtGui.QLabel('Head Position: ') move_head_controls.addWidget(head_label) move_vert_controls = QtGui.QHBoxLayout() move_vert_sldr = self.create_vert_sldr() move_vert_sldr.valueChanged[int].connect(self.move_head_vert) move_hor_controls = QtGui.QHBoxLayout() move_hor_sldr = self.create_hor_sldr() move_hor_sldr.valueChanged[int].connect(self.move_head_hor) move_vert_controls.addWidget(move_vert_sldr) move_hor_controls.addWidget(move_hor_sldr) move_head_controls.addLayout(move_vert_controls) move_head_controls.addLayout(move_hor_controls) upper_box.addLayout(move_head_controls) # ARM CONTROLS arm_controls = QtGui.QVBoxLayout() arm_r_controls = QtGui.QVBoxLayout() arm_l_controls = QtGui.QVBoxLayout() arm_r_controls.addWidget(QtGui.QLabel('Right Arm: ')) arm_l_controls.addWidget(QtGui.QLabel('Left Arm: ')) arm_l_control_grid = QtGui.QGridLayout() arm_r_control_grid = QtGui.QGridLayout() #for i in range(1, 4): # slider = QtGui.QSlider(QtCore.Qt.Vertical, self._widget) # slider.setFocusPolicy(QtCore.Qt.NoFocus) # slider.setGeometry(30, 40, 30, 100) #arm_l_control_grid.addWidget(slider, 0, i-1) #arm_l_control_grid.addWidget(QtGui.QLabel(str(i)), 1, i-1) #for i in range(1, 4): # slider = QtGui.QSlider(QtCore.Qt.Vertical, self._widget) # slider.setFocusPolicy(QtCore.Qt.NoFocus) # slider.setGeometry(30, 40, 30, 100) #arm_r_control_grid.addWidget(slider, 0, i-1) #arm_r_control_grid.addWidget(QtGui.QLabel(str(i)), 1, i-1) arm_l_controls.addLayout(arm_l_control_grid) arm_r_controls.addLayout(arm_r_control_grid) arm_controls.addLayout(arm_l_controls) arm_controls.addLayout(arm_r_controls) # large_box.addLayout(arm_controls) # SET EVERYTHING QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self._widget.setObjectName('SimpleGUI') self._widget.setLayout(large_box) context.add_widget(self._widget) def sound_cb(self, sound_request): qWarning('Received sound.') self.sound_sig.emit(sound_request) def create_btn(self, name): btn = QtGui.QPushButton(name, self._widget) btn.clicked.connect(self.command_cb) return btn def create_toggle_btn(self, name): btn = QtGui.QPushButton(name, self._widget) btn.setAutoRepeat(True) btn.setAutoRepeatInterval(10) return btn def create_vert_sldr(self): sldr = QtGui.QSlider() sldr.setSliderPosition(50) return sldr def create_hor_sldr(self): sldr = QtGui.QSlider(QtCore.Qt.Horizontal) sldr.setSliderPosition(50) return sldr def sound_sig_cb(self, sound_request): qWarning('Received sound signal.') #if (sound_request.command == SoundRequest.SAY): qWarning('Robot said: ' + sound_request.arg) #self.speech_label.setText('Robot said: ' + sound_request.arg) def command_cb(self): btn_name = self._widget.sender().text() if (btn_name == 'Speak'): text = self.speech_text.text() qWarning('Robot will say: ' + text) self._sound_client.say(text) def gripper_cb(self): btn_name = self._widget.sender().text() if btn_name == 'Open L': self._gripper_client.command(True, False) elif btn_name == 'Open R': self._gripper_client.command(False, False) elif btn_name == 'Close L': self._gripper_client.command(True, True) elif btn_name == 'Close R': self._gripper_client.command(False, True) def make_move_base_cb(self, speed): def callback(): self.move_base_cb(self._base_speed) return callback def move_base_cb(self, speed): qWarning('Received movement command.') btn_name = self._widget.sender().text() if btn_name == 'Forward': self._base_publisher.move_fwd_straight(speed) elif btn_name == 'Forward Right': self._base_publisher.move_fwd_right(speed) elif btn_name == 'Forward Left': self._base_publisher.move_fwd_left(speed) elif btn_name == 'Turn Left': self._base_publisher.turn_left(speed) elif btn_name == 'Turn Right': self._base_publisher.turn_right(speed) elif btn_name == 'Backward': self._base_publisher.move_bkwd_straight(speed) elif btn_name == 'Backward Right': self._base_publisher.move_bkwd_right(speed) elif btn_name == 'Backward Left': self._base_publisher.move_bkwd_left(speed) def _change_move_speed(self, speed_percent): self._base_speed = speed_percent / 100.0 def move_head_vert(self, pos): self._head_client.move_head_vert(pos) def move_head_hor(self, pos): self._head_client.move_head_hor(pos) def shutdown_plugin(self): # nothing to cleanup pass def save_settings(self, plugin_settings, instance_settings): # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # v = instance_settings.value(k) pass
class WaterPulse(Plugin): sound_sig = Signal(SoundRequest) positions = [[ -0.013001995432544766, -0.24895825213211362, -0.0055992576345036404, -1.7647281786034612, -496.3549908032631, -0.09960750521541717, 25.125128627428623 ], [ 0.0029160750999965845, -0.29700816845544387, 0.010917289481594317, -0.8034506550168371, -496.35799885178454, -0.09965101413551591, 25.12517213634872 ]] def __init__(self, context): super(WaterPulse, self).__init__(context) self.setObjectName('WaterPulse') self._widget = QWidget() self._widget.setFixedSize(600, 600) self._sound_client = SoundClient() rospy.Subscriber('robotsound', SoundRequest, self.sound_cb) self.l_joint_names = [ 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint' ] l_traj_contoller_name = None l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for LEFT arm...') self.l_traj_action_client.wait_for_server() QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.sound_sig.connect(self.sound_sig_cb) large_box = QtGui.QVBoxLayout() # Textbox to enter words for PR2 to say sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") # Default Text sound_textbox.setFixedWidth(450) self.marker_publisher = rospy.Publisher('visualization_marker', Marker) # Set a handler on the textbox to retrieve the text when button clicked self.sound_textbox = sound_textbox button_box = QtGui.QHBoxLayout() button_box.addItem(QtGui.QSpacerItem(15, 20)) button_box.addWidget(self.create_button('Speak', self.command_cb)) button_box.addWidget(sound_textbox) button_box.addStretch(1) large_box.addLayout(button_box) # 'Trick or Treat' & 'Thank You' Buttons halloween_box = QtGui.QHBoxLayout() halloween_box.addItem(QtGui.QSpacerItem(15, 20)) halloween_box.addWidget( self.create_button('Trick or Treat', self.command_cb)) halloween_box.addWidget( self.create_button('Thank You', self.command_cb)) halloween_box.addStretch(1) large_box.addLayout(halloween_box) # Buttons to move the PR2's head up_head = Head(Head.UP, self) head_box = QtGui.QVBoxLayout() up_head_box = QtGui.QHBoxLayout() up_head_button = self.create_button('^', up_head.create_closure()) down_head = Head(Head.DOWN, self) down_head_box = QtGui.QHBoxLayout() down_head_button = self.create_button('v', down_head.create_closure()) right_head = Head(Head.RIGHT, self) right_head_button = self.create_button('>', right_head.create_closure()) left_head = Head(Head.LEFT, self) left_head_button = self.create_button('<', left_head.create_closure()) left_right_head_box = QtGui.QHBoxLayout() head_speed_sld = QtGui.QSlider(QtCore.Qt.Horizontal) head_speed_sld.setFocusPolicy(QtCore.Qt.NoFocus) head_speed_sld.setMinimum(1) head_speed_sld.setMaximum(5) head_speed_sld.valueChanged[int].connect(Head.set_speed) up_head_box.addItem(QtGui.QSpacerItem(235, 20)) up_head_box.addWidget(up_head_button) up_head_box.addItem(QtGui.QSpacerItem(275, 20)) left_right_head_box.addItem(QtGui.QSpacerItem(160, 20)) left_right_head_box.addWidget(left_head_button) left_right_head_box.addItem(QtGui.QSpacerItem(60, 20)) left_right_head_box.addWidget(right_head_button) left_right_head_box.addItem(QtGui.QSpacerItem(225, 20)) down_head_box.addItem(QtGui.QSpacerItem(235, 20)) down_head_box.addWidget(down_head_button) down_head_box.addItem(QtGui.QSpacerItem(275, 20)) head_sld_box = QtGui.QHBoxLayout() head_sld_box.addItem(QtGui.QSpacerItem(225, 20)) head_sld_box.addWidget(head_speed_sld) head_sld_box.addItem(QtGui.QSpacerItem(225, 20)) head_box.addLayout(up_head_box) head_box.addLayout(left_right_head_box) head_box.addLayout(down_head_box) head_box.addLayout(head_sld_box) large_box.addLayout(head_box) # Buttons to move the grippers gripper = Gripper(Gripper.RIGHT, Gripper.OPEN, self) right_gripper = self.create_button('Right gripper', gripper.create_closure()) gripper = Gripper(Gripper.LEFT, Gripper.OPEN, self) left_gripper = self.create_button('Left gripper', gripper.create_closure()) knock_button = self.create_button('Knock', self.knock) large_box.addItem(QtGui.QSpacerItem(100, 250)) gripper_box = QtGui.QHBoxLayout() gripper_box.addItem(QtGui.QSpacerItem(75, 20)) gripper_box.addWidget(left_gripper) gripper_box.addWidget(knock_button) gripper_box.addItem(QtGui.QSpacerItem(450, 20)) gripper_box.addWidget(right_gripper) gripper_box.addItem(QtGui.QSpacerItem(75, 20)) large_box.addLayout(gripper_box) # Buttons to move the base base_box = QtGui.QVBoxLayout() large_box.addItem(QtGui.QSpacerItem(100, 100)) forward_base_box = QtGui.QHBoxLayout() forward_base = Base(Base.FORWARD, self) forward_base_button = self.create_button('move forward', forward_base.create_closure()) forward_base_box.addItem(QtGui.QSpacerItem(400, 20)) forward_base_box.addWidget(forward_base_button) forward_base_box.addItem(QtGui.QSpacerItem(400, 20)) base_box.addLayout(forward_base_box) left_right_base_box = QtGui.QHBoxLayout() left_base = Base(Base.LEFT, self) left_base_button = self.create_button('move left', left_base.create_closure()) right_base = Base(Base.RIGHT, self) right_base_button = self.create_button('move right', right_base.create_closure()) left_right_base_box.addItem(QtGui.QSpacerItem(300, 20)) left_right_base_box.addWidget(left_base_button) left_right_base_box.addItem(QtGui.QSpacerItem(50, 20)) left_right_base_box.addWidget(right_base_button) left_right_base_box.addItem(QtGui.QSpacerItem(300, 20)) base_box.addLayout(left_right_base_box) backward_base_box = QtGui.QHBoxLayout() backward_base = Base(Base.BACKWARD, self) backward_base_button = self.create_button( 'move backward', backward_base.create_closure()) backward_base_box.addItem(QtGui.QSpacerItem(400, 20)) backward_base_box.addWidget(backward_base_button) backward_base_box.addItem(QtGui.QSpacerItem(400, 20)) base_box.addLayout(backward_base_box) large_box.addLayout(base_box) turn_base_box = QtGui.QHBoxLayout() counter_base = Base(Base.COUNTER, self) counter_base_button = self.create_button('\\\n -->', counter_base.create_closure()) clockwise_base = Base(Base.CLOCKWISE, self) clockwise_base_button = self.create_button( ' /\n<--', clockwise_base.create_closure()) turn_base_box.addItem(QtGui.QSpacerItem(75, 20)) turn_base_box.addWidget(counter_base_button) turn_base_box.addItem(QtGui.QSpacerItem(225, 20)) turn_base_box.addWidget(clockwise_base_button) turn_base_box.addItem(QtGui.QSpacerItem(100, 20)) large_box.addLayout(turn_base_box) self._widget.setObjectName('WaterPulse') self._widget.setLayout(large_box) context.add_widget(self._widget) self._widget.setStyleSheet( "QWidget { image: url(%s) }" % (str(os.path.dirname(os.path.realpath(__file__))) + "/../../rosie_background.jpg")) def show_text_in_rviz(self, text): marker = Marker(type=Marker.TEXT_VIEW_FACING, id=0, lifetime=rospy.Duration(1.5), pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)), scale=Vector3(0.06, 0.06, 0.06), header=Header(frame_id='base_link'), color=ColorRGBA(0.0, 1.0, 0.0, 0.8), text=text) self.marker_publisher.publish(marker) def sound_cb(self, sound_request): qWarning('Received sound.') self.sound_sig.emit(sound_request) def create_button(self, name, method): btn = QtGui.QPushButton(name, self._widget) btn.clicked.connect(method) btn.setAutoRepeat(True) return btn def sound_sig_cb(self, sound_request): qWarning('Received sound signal.') #if (sound_request.command == SoundRequest.SAY): qWarning('Robot said: ' + sound_request.arg) def command_cb(self): button_name = self._widget.sender().text() if (button_name == 'Speak'): qWarning('Robot will say: ' + self.sound_textbox.text()) self._sound_client.say(self.sound_textbox.text()) elif (button_name == 'Trick or Treat'): qWarning('Robot will say: Trick or Treat') self._sound_client.say('Trick or Treat') elif (button_name == 'Thank You'): qWarning('Robot will say: Thank You') self._sound_client.say('Thank You') def knock(self): for position in WaterPulse.positions: velocities = [0] * len(position) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) time_to_joint = 2.0 traj_goal.trajectory.points.append( JointTrajectoryPoint( positions=position, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) traj_goal.trajectory.joint_names = self.l_joint_names self.l_traj_action_client.send_goal(traj_goal) result = 0 while (result < 2): # ACTIVE or PENDING self.l_traj_action_client.wait_for_result() result = self.l_traj_action_client.get_result() #self.l_traj_action_client.wait_for_result() #time.sleep(1) def shutdown_plugin(self): # TODO unregister all publishers here pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class WaterPulse(Plugin): sound_sig = Signal(SoundRequest) positions = [[-0.013001995432544766, -0.24895825213211362, -0.0055992576345036404, -1.7647281786034612, -496.3549908032631, -0.09960750521541717, 25.125128627428623], [0.0029160750999965845, -0.29700816845544387, 0.010917289481594317, -0.8034506550168371, -496.35799885178454, -0.09965101413551591, 25.12517213634872]] def __init__(self, context): super(WaterPulse, self).__init__(context) self.setObjectName('WaterPulse') self._widget = QWidget() self._widget.setFixedSize(600, 600) self._sound_client = SoundClient() rospy.Subscriber('robotsound', SoundRequest, self.sound_cb) self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] l_traj_contoller_name = None l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for LEFT arm...') self.l_traj_action_client.wait_for_server() QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) self.sound_sig.connect(self.sound_sig_cb) large_box = QtGui.QVBoxLayout() # Textbox to enter words for PR2 to say sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") # Default Text sound_textbox.setFixedWidth(450) self.marker_publisher = rospy.Publisher('visualization_marker', Marker) # Set a handler on the textbox to retrieve the text when button clicked self.sound_textbox = sound_textbox button_box = QtGui.QHBoxLayout() button_box.addItem(QtGui.QSpacerItem(15,20)) button_box.addWidget(self.create_button('Speak', self.command_cb)) button_box.addWidget(sound_textbox) button_box.addStretch(1) large_box.addLayout(button_box) # 'Trick or Treat' & 'Thank You' Buttons halloween_box = QtGui.QHBoxLayout() halloween_box.addItem(QtGui.QSpacerItem(15,20)) halloween_box.addWidget(self.create_button('Trick or Treat', self.command_cb)) halloween_box.addWidget(self.create_button('Thank You', self.command_cb)) halloween_box.addStretch(1) large_box.addLayout(halloween_box) # Buttons to move the PR2's head up_head = Head(Head.UP, self) head_box = QtGui.QVBoxLayout() up_head_box = QtGui.QHBoxLayout() up_head_button = self.create_button('^', up_head.create_closure()) down_head = Head(Head.DOWN, self) down_head_box = QtGui.QHBoxLayout() down_head_button = self.create_button('v', down_head.create_closure()) right_head = Head(Head.RIGHT, self) right_head_button = self.create_button('>', right_head.create_closure()) left_head = Head(Head.LEFT, self) left_head_button = self.create_button('<', left_head.create_closure()) left_right_head_box = QtGui.QHBoxLayout() head_speed_sld = QtGui.QSlider(QtCore.Qt.Horizontal) head_speed_sld.setFocusPolicy(QtCore.Qt.NoFocus) head_speed_sld.setMinimum(1) head_speed_sld.setMaximum(5) head_speed_sld.valueChanged[int].connect(Head.set_speed) up_head_box.addItem(QtGui.QSpacerItem(235,20)) up_head_box.addWidget(up_head_button) up_head_box.addItem(QtGui.QSpacerItem(275,20)) left_right_head_box.addItem(QtGui.QSpacerItem(160,20)) left_right_head_box.addWidget(left_head_button) left_right_head_box.addItem(QtGui.QSpacerItem(60,20)) left_right_head_box.addWidget(right_head_button) left_right_head_box.addItem(QtGui.QSpacerItem(225,20)) down_head_box.addItem(QtGui.QSpacerItem(235,20)) down_head_box.addWidget(down_head_button) down_head_box.addItem(QtGui.QSpacerItem(275,20)) head_sld_box = QtGui.QHBoxLayout() head_sld_box.addItem(QtGui.QSpacerItem(225,20)) head_sld_box.addWidget(head_speed_sld) head_sld_box.addItem(QtGui.QSpacerItem(225,20)) head_box.addLayout(up_head_box) head_box.addLayout(left_right_head_box) head_box.addLayout(down_head_box) head_box.addLayout(head_sld_box) large_box.addLayout(head_box) # Buttons to move the grippers gripper = Gripper(Gripper.RIGHT, Gripper.OPEN, self) right_gripper = self.create_button('Right gripper', gripper.create_closure()) gripper = Gripper(Gripper.LEFT, Gripper.OPEN, self) left_gripper = self.create_button('Left gripper', gripper.create_closure()) knock_button = self.create_button('Knock', self.knock) large_box.addItem(QtGui.QSpacerItem(100,250)) gripper_box = QtGui.QHBoxLayout() gripper_box.addItem(QtGui.QSpacerItem(75,20)) gripper_box.addWidget(left_gripper) gripper_box.addWidget(knock_button) gripper_box.addItem(QtGui.QSpacerItem(450,20)) gripper_box.addWidget(right_gripper) gripper_box.addItem(QtGui.QSpacerItem(75,20)) large_box.addLayout(gripper_box) # Buttons to move the base base_box = QtGui.QVBoxLayout() large_box.addItem(QtGui.QSpacerItem(100,100)) forward_base_box = QtGui.QHBoxLayout() forward_base = Base(Base.FORWARD, self) forward_base_button = self.create_button('move forward', forward_base.create_closure()) forward_base_box.addItem(QtGui.QSpacerItem(400,20)) forward_base_box.addWidget(forward_base_button) forward_base_box.addItem(QtGui.QSpacerItem(400,20)) base_box.addLayout(forward_base_box) left_right_base_box = QtGui.QHBoxLayout() left_base= Base(Base.LEFT, self) left_base_button = self.create_button('move left', left_base.create_closure()) right_base= Base(Base.RIGHT, self) right_base_button= self.create_button('move right', right_base.create_closure()) left_right_base_box.addItem(QtGui.QSpacerItem(300,20)) left_right_base_box.addWidget(left_base_button) left_right_base_box.addItem(QtGui.QSpacerItem(50,20)) left_right_base_box.addWidget(right_base_button) left_right_base_box.addItem(QtGui.QSpacerItem(300,20)) base_box.addLayout(left_right_base_box) backward_base_box = QtGui.QHBoxLayout() backward_base= Base(Base.BACKWARD, self) backward_base_button = self.create_button('move backward', backward_base.create_closure()) backward_base_box.addItem(QtGui.QSpacerItem(400,20)) backward_base_box.addWidget(backward_base_button) backward_base_box.addItem(QtGui.QSpacerItem(400,20)) base_box.addLayout(backward_base_box) large_box.addLayout(base_box) turn_base_box = QtGui.QHBoxLayout() counter_base= Base(Base.COUNTER, self) counter_base_button = self.create_button('\\\n -->', counter_base.create_closure()) clockwise_base= Base(Base.CLOCKWISE, self) clockwise_base_button = self.create_button(' /\n<--', clockwise_base.create_closure()) turn_base_box.addItem(QtGui.QSpacerItem(75,20)) turn_base_box.addWidget(counter_base_button) turn_base_box.addItem(QtGui.QSpacerItem(225,20)) turn_base_box.addWidget(clockwise_base_button) turn_base_box.addItem(QtGui.QSpacerItem(100,20)) large_box.addLayout(turn_base_box) self._widget.setObjectName('WaterPulse') self._widget.setLayout(large_box) context.add_widget(self._widget) self._widget.setStyleSheet("QWidget { image: url(%s) }" % (str(os.path.dirname(os.path.realpath(__file__))) + "/../../rosie_background.jpg")) def show_text_in_rviz(self, text): marker = Marker(type=Marker.TEXT_VIEW_FACING, id=0, lifetime=rospy.Duration(1.5), pose=Pose(Point(0.5, 0.5, 1.45), Quaternion(0, 0, 0, 1)), scale=Vector3(0.06, 0.06, 0.06), header=Header(frame_id='base_link'), color=ColorRGBA(0.0, 1.0, 0.0, 0.8), text=text) self.marker_publisher.publish(marker) def sound_cb(self, sound_request): qWarning('Received sound.') self.sound_sig.emit(sound_request) def create_button(self, name, method): btn = QtGui.QPushButton(name, self._widget) btn.clicked.connect(method) btn.setAutoRepeat(True) return btn def sound_sig_cb(self, sound_request): qWarning('Received sound signal.') #if (sound_request.command == SoundRequest.SAY): qWarning('Robot said: ' + sound_request.arg) def command_cb(self): button_name = self._widget.sender().text() if (button_name == 'Speak'): qWarning('Robot will say: ' + self.sound_textbox.text()) self._sound_client.say(self.sound_textbox.text()) elif (button_name == 'Trick or Treat'): qWarning('Robot will say: Trick or Treat') self._sound_client.say('Trick or Treat') elif (button_name == 'Thank You'): qWarning('Robot will say: Thank You') self._sound_client.say('Thank You') def knock(self): for position in WaterPulse.positions: velocities = [0] * len(position) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) time_to_joint = 2.0 traj_goal.trajectory.points.append(JointTrajectoryPoint( positions=position, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) traj_goal.trajectory.joint_names = self.l_joint_names self.l_traj_action_client.send_goal(traj_goal) result = 0 while(result < 2): # ACTIVE or PENDING self.l_traj_action_client.wait_for_result() result = self.l_traj_action_client.get_result() #self.l_traj_action_client.wait_for_result() #time.sleep(1) def shutdown_plugin(self): # TODO unregister all publishers here pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass