class WoZPlugin(Plugin): def __init__(self, context): super(WoZPlugin, self).__init__(context) self._widget = QWidget() self._widget.setFixedSize(600, 600) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) large_box = QtGui.QVBoxLayout() #Button for outreaching to recieve book from Human button_box = QtGui.QVBoxLayout() box_1 = QtGui.QHBoxLayout() box_2 = QtGui.QHBoxLayout() box_3 = QtGui.QHBoxLayout() box_4 = QtGui.QHBoxLayout() box_5 = QtGui.QHBoxLayout() box_6 = QtGui.QHBoxLayout() box_7 = QtGui.QHBoxLayout() box_8 = QtGui.QHBoxLayout() box_9 = QtGui.QHBoxLayout() box_10 = QtGui.QHBoxLayout() box_1.addWidget(self.create_button('print nice', self.printCallback)) self._widget.setObjectName('TrashbotGUI') self._widget.setLayout(large_box) context.add_widget(self._widget) def create_button(self, name, callback): btn = QtGui.QPushButton(name, self._widget) btn.clicked.connect(callback) btn.setAutoRepeat(True) return btn def printCallback(self): print 'nice'
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 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 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): 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): 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 Milestone1GUI(Plugin): RECEIVE_FROM_HUMAN_R_POS = [ 0.00952670015493673, 0.3270780665526253, 0.03185028324084582, -1.3968658009779173, -3.058799671876592, -1.1083678332942686, -1.6314425515258866 ] READ_FIDUCIAL_R_POS = [ 0.41004856860653505, 0.29772362823537935, -0.019944325676627628, -1.8394298656773618, -0.44139252862458106, -0.09922194050427624, -4.761735317011306 ] NAVIGATE_R_POS = [ -0.3594077470836499, 0.971353000916152, -1.9647276598906076, -1.431900313132731, -1.1839177367219755, -0.09817772642188527, -1.622044624784374 ] PLACE_ON_SHELF_R_POS = [ -0.15015144487461773, 0.4539704512093072, -0.10846018983280459, -0.9819529421527269, -3.0207362886631235, -0.4990689162195487, -1.6026396464199553 ] LOWER_ON_SHELF_R_POS = [ -0.2159792990560645, 0.6221451583409631, -0.1886376030177479, -0.959223940465513, 9.690004126983537, -0.2866303982732683, 111.39703078836274 ] RELEASE_BOOK_R_POS = [ -0.15545746838546493, 0.44145040258984786, -0.13267376861465774, -0.972108533778647, -3.028545645401449, -0.38572817936010495, 0.008017066746929924 ] TUCKED_UNDER_L_POS = [ 0.05688828299939419, 1.2955758539090194, 1.7180547710154033, -1.4511548177467404, -0.28718096455759035, -0.0938208188421713, -10.980395466225648 ] sound_sig = Signal(SoundRequest) def __init__(self, context): super(Milestone1GUI, self).__init__(context) self._sound_client = SoundClient() rospy.Subscriber('robotsound', SoundRequest, self.sound_cb) 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' ] # 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() #init torso self.torso = Torso() #init gripper self.l_gripper = Gripper('l') self.r_gripper = Gripper('r') #init navigation self.navigation = Navigation() self._widget = QWidget() self._widget.setFixedSize(600, 600) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) large_box = QtGui.QVBoxLayout() #Button for outreaching to recieve book from Human button_box = QtGui.QVBoxLayout() box_1 = QtGui.QHBoxLayout() box_2 = QtGui.QHBoxLayout() box_3 = QtGui.QHBoxLayout() box_4 = QtGui.QHBoxLayout() box_5 = QtGui.QHBoxLayout() box_6 = QtGui.QHBoxLayout() box_7 = QtGui.QHBoxLayout() box_8 = QtGui.QHBoxLayout() box_9 = QtGui.QHBoxLayout() box_10 = QtGui.QHBoxLayout() box_1.addItem(QtGui.QSpacerItem(15, 2)) box_1.addWidget( self.create_button('Prepare To Take', self.prepare_to_take)) box_1.addItem(QtGui.QSpacerItem(445, 2)) box_2.addItem(QtGui.QSpacerItem(15, 2)) box_2.addWidget( self.create_button('Take From Human', self.take_from_human)) box_2.addItem(QtGui.QSpacerItem(445, 2)) box_3.addItem(QtGui.QSpacerItem(15, 2)) box_3.addWidget( self.create_button('Prepare To Navigate', self.prepare_to_navigate)) box_3.addItem(QtGui.QSpacerItem(445, 2)) # Button to move to shelf box_5.addItem(QtGui.QSpacerItem(15, 2)) box_5.addWidget( self.create_button('Navigate To Shelf', self.navigate_to_shelf)) box_5.addItem(QtGui.QSpacerItem(445, 2)) box_4.addItem(QtGui.QSpacerItem(15, 2)) box_4.addWidget( self.create_button('Place On Shelf', self.place_on_shelf)) box_4.addItem(QtGui.QSpacerItem(445, 2)) box_6.addItem(QtGui.QSpacerItem(15, 2)) box_6.addWidget( self.create_button('Give Information', self.give_information)) box_6.addItem(QtGui.QSpacerItem(445, 2)) box_7.addItem(QtGui.QSpacerItem(15, 2)) box_7.addWidget( self.create_button('Navigate To Person', self.navigate_to_person)) box_7.addItem(QtGui.QSpacerItem(445, 2)) self.book_textbox = QtGui.QLineEdit() self.book_textbox.setFixedWidth(100) box_8.addItem(QtGui.QSpacerItem(15, 2)) box_8.addWidget(self.book_textbox) box_8.addWidget( self.create_button('Pick Up Book', self.pick_up_from_shelf_button)) box_8.addItem(QtGui.QSpacerItem(445, 2)) box_9.addItem(QtGui.QSpacerItem(15, 2)) box_9.addWidget(self.create_button('Localize', self.spin_base)) box_9.addItem(QtGui.QSpacerItem(445, 2)) box_10.addItem(QtGui.QSpacerItem(15, 2)) box_10.addWidget( self.create_button('Non-nav Pick Up', self.pick_up_from_shelf)) box_10.addItem(QtGui.QSpacerItem(445, 2)) button_box.addItem(QtGui.QSpacerItem(20, 120)) button_box.addLayout(box_1) button_box.addLayout(box_2) button_box.addLayout(box_3) button_box.addLayout(box_5) button_box.addLayout(box_4) button_box.addLayout(box_6) button_box.addLayout(box_7) button_box.addLayout(box_8) button_box.addLayout(box_9) button_box.addLayout(box_10) button_box.addItem(QtGui.QSpacerItem(20, 240)) large_box.addLayout(button_box) self.marker_perception = ReadMarkers() self.speech_recognition = SpeechRecognition(self) self.bookDB = BookDB() self.book_map = self.bookDB.getAllBookCodes() self._widget.setObjectName('Milestone1GUI') 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__))) + "/../../librarian_gui_background.jpg")) def get_function(self, text): functions = { "bring-me-a-book": self.pick_up_from_shelf_routine, "put-this-book-back": self.prepare_to_take, "give-me-information": self.give_information_routine, "here-you-go": self.put_this_book_back_routine, } if text not in functions: print("Could not find key %s" % text) return None return functions[text] 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 prepare_to_take(self): # Open gripper and move arms to take book self.torso.move(.15) # move torso .1 meter from base (MAX is .2) self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS self.saved_r_arm_pose = Milestone1GUI.RECEIVE_FROM_HUMAN_R_POS self.move_arm('l', 5.0) self.move_arm('r', 5.0) # Increase these numbers for slower movement self.l_gripper.close_gripper() self.r_gripper.open_gripper(True) self._sound_client.say("Please give me a book") def take_from_human(self): self.marker_perception.is_listening = True # Close gripper and move arms to see book self.r_gripper.close_gripper(True) self._sound_client.say("Thank you") time.sleep(1) self.saved_r_arm_pose = Milestone1GUI.READ_FIDUCIAL_R_POS self.move_arm('r', 5.0, True) # Increase these numbers for slower movement self.look_at_r_gripper() rospy.loginfo("marker id returned by get_marker_id is: " + str(self.marker_perception.get_marker_id())) def look_at_r_gripper(self): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_client.wait_for_server() head_goal = PointHeadGoal() head_goal.target.header.frame_id = "r_gripper_tool_frame" head_goal.min_duration = rospy.Duration(0.3) head_goal.target.point = Point(0, 0, 0) head_goal.max_velocity = 1.0 head_client.send_goal(head_goal) head_client.wait_for_result(rospy.Duration(5.0)) if (head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.') def prepare_to_navigate(self): self.marker_perception.is_listening = False # Tuck arms self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS self.move_arm('r', 5.0) # Increase these numbers for slower movement #spin 360 * rotate_count degress clock wise def spin_base(self, rotate_count): # Adjust height and tuck arms before localization self._sound_client.say("Please wait while I orient myself.") self.torso.move(.15) self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS self.move_arm('l', 5.0, True) self.move_arm('r', 5.0, True) self.l_gripper.close_gripper() self.r_gripper.close_gripper() if not rotate_count: rotate_count = 2 topic_name = '/base_controller/command' base_publisher = rospy.Publisher(topic_name, Twist) twist_msg = Twist() twist_msg.linear = Vector3(0.0, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 0.5) start_time = rospy.get_rostime() while rospy.get_rostime() < start_time + rospy.Duration( 15.0 * rotate_count): base_publisher.publish(twist_msg) def navigate_to_shelf(self, marker_id=None): # Move forward, place book on the shelf, and move back if marker_id is None or marker_id is False: marker_id = self.marker_perception.get_marker_id() rospy.loginfo("marker id returned by get_marker_id is: " + str(marker_id)) if marker_id is False: rospy.loginfo("wuuuuuuuut") if marker_id is None: self._sound_client.say("I don't think I am holding a book " "right now") rospy.logwarn("Navigate to shelf called when marker id is None") return book = self.book_map.get(unicode(marker_id)) if book is None: self._sound_client.say("The book that I am holding is unknown " "to me") rospy.logwarn( "Navigate to shelf called when marker id is not in database") return x = book.getXCoordinate() y = book.getYCoordinate() self.navigation.move_to_shelf(x, y) # Ye Olde Dropping way of putting a book on the shelf. Deprecating. def drop_on_shelf(self): self.saved_r_arm_pose = Milestone1GUI.PLACE_ON_SHELF_R_POS self.move_arm('r', 4.0, True) # Increase these numbers for slower movement self.move_base(True) self.r_gripper.open_gripper(True) self.saved_r_arm_pose = Milestone1GUI.RELEASE_BOOK_R_POS self.move_arm('r', 2.0, True) # Increase these numbers for slower movement self.move_base(False) self.marker_perception.reset_marker_id() def place_on_shelf(self): self.saved_r_arm_pose = Milestone1GUI.PLACE_ON_SHELF_R_POS self.move_arm('r', 4.0, True) # Increase these numbers for slower movement self.move_base(True) self.saved_r_arm_pose = Milestone1GUI.LOWER_ON_SHELF_R_POS self.move_arm('r', 2.0, True) # Increase these numbers for slower movement self.r_gripper.open_gripper(True) self.move_base(False) self.marker_perception.reset_marker_id() def give_information(self): marker_id = self.marker_perception.get_marker_id() rospy.loginfo("marker id returned by get_marker_id is: " + str(marker_id)) if marker_id is not None: book = self.book_map.get(unicode(marker_id)) if book is None: rospy.logwarn( "Give information called when marker id is not in database" ) self._sound_client.say( "The book that I am holding is unknown to me") else: self._sound_client.say(book.getInformation()) else: rospy.logwarn("Give information called when marker id is None") self._sound_client.say( "I don't think I am holding a book right now") def pick_up_from_shelf_button(self): self.pick_up_from_shelf_routine(self.book_textbox.text()) def pick_up_from_shelf_routine(self, book_title): book_id = self.bookDB.getBookIdByTitle(book_title) if book_id is None: rospy.logwarn("Book asked for was not present in database") self._sound_client.say( "The book you requested is not present in the database.") else: self.torso.move(.15) # move torso .1 meter from base (MAX is .2) self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS self.move_arm('l', 5.0) self.l_gripper.close_gripper() # self.marker_perception.set_marker_id(book_id) self.prepare_to_navigate() time.sleep(5) self.navigate_to_shelf(book_id) # Navigate to book location self.pick_up_from_shelf() # Pick up from the shelf self.prepare_to_navigate() time.sleep(5) self.navigate_to_person( ) # Navigate to designated help desk location self.give_book() # Give Book to Human def put_this_book_back_routine(self): self.take_from_human() time.sleep(5) self.prepare_to_navigate() time.sleep(5) self.navigate_to_shelf() self.place_on_shelf() self.prepare_to_navigate() # TODO: return to human? def give_information_routine(self, book_title): book_id = self.bookDB.getBookIdByTitle(book_title) if book_id is None: rospy.logwarn("Book asked for was not present in database") self._sound_client.say( "The book you requested is not present in the database.") return book = self.book_map.get(unicode(book_id)) self._sound_client.say(book.getInformation()) def pick_up_from_shelf(self): self.saved_r_arm_pose = Milestone1GUI.LOWER_ON_SHELF_R_POS self.move_arm('r', 4.0, True) # Increase these numbers for slower movement self.r_gripper.open_gripper(True) self.move_base(True) self.r_gripper.close_gripper(True) self.move_base(False) def navigate_to_person(self): x = 2.82690143585 y = -0.416650295258 z = 0.252587109056 w = 0.967574158573 self.navigation.move_to_shelf(x, y, z, w) def give_book(self): self.saved_r_arm_pose = Milestone1GUI.RECEIVE_FROM_HUMAN_R_POS self.move_arm('r', 4.0, True) self.r_gripper.open_gripper(True) # Moves forward to the bookshelf (or backward if isForward is false) def move_base(self, isForward): topic_name = '/base_controller/command' base_publisher = rospy.Publisher(topic_name, Twist) distance = 0.25 if not isForward: distance *= -1 twist_msg = Twist() twist_msg.linear = Vector3(distance, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 0.0) for x in range(0, 15): rospy.loginfo("Moving the base") base_publisher.publish(twist_msg) time.sleep(0.15) time.sleep(1.5) # Moves arms using kinematics def move_arm(self, side_prefix, time_to_joints=2.0, wait=False): # 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) self.move_to_joints(side_prefix, self.saved_r_arm_pose, time_to_joints, wait) 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) self.move_to_joints(side_prefix, self.saved_l_arm_pose, time_to_joints, wait) pass 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 move_to_joints(self, side_prefix, positions, time_to_joint, wait=False): '''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) if side_prefix == 'r': traj_goal.trajectory.joint_names = self.r_joint_names action_client = self.r_traj_action_client else: traj_goal.trajectory.joint_names = self.l_joint_names action_client = self.l_traj_action_client action_client.send_goal(traj_goal) if wait: time.sleep(time_to_joint)
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
class Milestone1GUI(Plugin): RECEIVE_FROM_HUMAN_R_POS = [0.00952670015493673, 0.3270780665526253, 0.03185028324084582, -1.3968658009779173, -3.058799671876592, -1.1083678332942686, -1.6314425515258866] READ_FIDUCIAL_R_POS = [0.41004856860653505, 0.29772362823537935, -0.019944325676627628, -1.8394298656773618, -0.44139252862458106, -0.09922194050427624, -4.761735317011306] NAVIGATE_R_POS = [-0.3594077470836499, 0.971353000916152, -1.9647276598906076, -1.431900313132731, -1.1839177367219755, -0.09817772642188527, -1.622044624784374] PLACE_ON_SHELF_R_POS = [-0.15015144487461773, 0.4539704512093072, -0.10846018983280459, -0.9819529421527269, -3.0207362886631235, -0.4990689162195487, -1.6026396464199553] LOWER_ON_SHELF_R_POS = [-0.2159792990560645, 0.6221451583409631, -0.1886376030177479, -0.959223940465513, 9.690004126983537, -0.2866303982732683, 111.39703078836274] RELEASE_BOOK_R_POS = [-0.15545746838546493, 0.44145040258984786, -0.13267376861465774, -0.972108533778647, -3.028545645401449, -0.38572817936010495, 0.008017066746929924] TUCKED_UNDER_L_POS = [0.05688828299939419, 1.2955758539090194, 1.7180547710154033, -1.4511548177467404, -0.28718096455759035, -0.0938208188421713, -10.980395466225648] sound_sig = Signal(SoundRequest) def __init__(self, context): super(Milestone1GUI, self).__init__(context) self._sound_client = SoundClient() rospy.Subscriber('robotsound', SoundRequest, self.sound_cb) 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'] # 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() #init torso self.torso = Torso() #init gripper self.l_gripper = Gripper('l') self.r_gripper = Gripper('r') #init navigation self.navigation = Navigation() self._widget = QWidget() self._widget.setFixedSize(600, 600) QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10)) large_box = QtGui.QVBoxLayout() #Button for outreaching to recieve book from Human button_box = QtGui.QVBoxLayout() box_1 = QtGui.QHBoxLayout() box_2 = QtGui.QHBoxLayout() box_3 = QtGui.QHBoxLayout() box_4 = QtGui.QHBoxLayout() box_5 = QtGui.QHBoxLayout() box_6 = QtGui.QHBoxLayout() box_7 = QtGui.QHBoxLayout() box_8 = QtGui.QHBoxLayout() box_9 = QtGui.QHBoxLayout() box_10 = QtGui.QHBoxLayout() box_1.addItem(QtGui.QSpacerItem(15,2)) box_1.addWidget(self.create_button('Prepare To Take', self.prepare_to_take)) box_1.addItem(QtGui.QSpacerItem(445,2)) box_2.addItem(QtGui.QSpacerItem(15,2)) box_2.addWidget(self.create_button('Take From Human', self.take_from_human)) box_2.addItem(QtGui.QSpacerItem(445,2)) box_3.addItem(QtGui.QSpacerItem(15,2)) box_3.addWidget(self.create_button('Prepare To Navigate', self.prepare_to_navigate)) box_3.addItem(QtGui.QSpacerItem(445,2)) # Button to move to shelf box_5.addItem(QtGui.QSpacerItem(15,2)) box_5.addWidget(self.create_button('Navigate To Shelf', self.navigate_to_shelf)) box_5.addItem(QtGui.QSpacerItem(445,2)) box_4.addItem(QtGui.QSpacerItem(15,2)) box_4.addWidget(self.create_button('Place On Shelf', self.place_on_shelf)) box_4.addItem(QtGui.QSpacerItem(445,2)) box_6.addItem(QtGui.QSpacerItem(15,2)) box_6.addWidget(self.create_button('Give Information', self.give_information)) box_6.addItem(QtGui.QSpacerItem(445,2)) box_7.addItem(QtGui.QSpacerItem(15,2)) box_7.addWidget(self.create_button('Navigate To Person', self.navigate_to_person)) box_7.addItem(QtGui.QSpacerItem(445,2)) self.book_textbox = QtGui.QLineEdit() self.book_textbox.setFixedWidth(100) box_8.addItem(QtGui.QSpacerItem(15,2)) box_8.addWidget(self.book_textbox) box_8.addWidget(self.create_button('Pick Up Book', self.pick_up_from_shelf_button)) box_8.addItem(QtGui.QSpacerItem(445,2)) box_9.addItem(QtGui.QSpacerItem(15,2)) box_9.addWidget(self.create_button('Localize', self.spin_base)) box_9.addItem(QtGui.QSpacerItem(445,2)) box_10.addItem(QtGui.QSpacerItem(15,2)) box_10.addWidget(self.create_button('Non-nav Pick Up', self.pick_up_from_shelf)) box_10.addItem(QtGui.QSpacerItem(445,2)) button_box.addItem(QtGui.QSpacerItem(20,120)) button_box.addLayout(box_1) button_box.addLayout(box_2) button_box.addLayout(box_3) button_box.addLayout(box_5) button_box.addLayout(box_4) button_box.addLayout(box_6) button_box.addLayout(box_7) button_box.addLayout(box_8) button_box.addLayout(box_9) button_box.addLayout(box_10) button_box.addItem(QtGui.QSpacerItem(20,240)) large_box.addLayout(button_box) self.marker_perception = ReadMarkers() self.speech_recognition = SpeechRecognition(self) self.bookDB = BookDB() self.book_map = self.bookDB.getAllBookCodes() self._widget.setObjectName('Milestone1GUI') 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__))) + "/../../librarian_gui_background.jpg")) def get_function(self, text): functions = { "bring-me-a-book": self.pick_up_from_shelf_routine, "put-this-book-back": self.prepare_to_take, "give-me-information": self.give_information_routine, "here-you-go": self.put_this_book_back_routine, } if text not in functions: print ("Could not find key %s" % text) return None return functions[text] 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 prepare_to_take(self): # Open gripper and move arms to take book self.torso.move(.15) # move torso .1 meter from base (MAX is .2) self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS self.saved_r_arm_pose = Milestone1GUI.RECEIVE_FROM_HUMAN_R_POS self.move_arm('l', 5.0) self.move_arm('r', 5.0) # Increase these numbers for slower movement self.l_gripper.close_gripper() self.r_gripper.open_gripper(True) self._sound_client.say("Please give me a book") def take_from_human(self): self.marker_perception.is_listening = True # Close gripper and move arms to see book self.r_gripper.close_gripper(True) self._sound_client.say("Thank you") time.sleep(1) self.saved_r_arm_pose = Milestone1GUI.READ_FIDUCIAL_R_POS self.move_arm('r', 5.0, True) # Increase these numbers for slower movement self.look_at_r_gripper() rospy.loginfo("marker id returned by get_marker_id is: " + str(self.marker_perception.get_marker_id())) def look_at_r_gripper(self): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_client.wait_for_server() head_goal = PointHeadGoal() head_goal.target.header.frame_id = "r_gripper_tool_frame" head_goal.min_duration = rospy.Duration(0.3) head_goal.target.point = Point(0, 0, 0) head_goal.max_velocity = 1.0 head_client.send_goal(head_goal) head_client.wait_for_result(rospy.Duration(5.0)) if (head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.') def prepare_to_navigate(self): self.marker_perception.is_listening = False # Tuck arms self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS self.move_arm('r', 5.0) # Increase these numbers for slower movement #spin 360 * rotate_count degress clock wise def spin_base(self, rotate_count): # Adjust height and tuck arms before localization self._sound_client.say("Please wait while I orient myself.") self.torso.move(.15) self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS self.move_arm('l', 5.0, True) self.move_arm('r', 5.0, True) self.l_gripper.close_gripper() self.r_gripper.close_gripper() if not rotate_count: rotate_count = 2 topic_name = '/base_controller/command' base_publisher = rospy.Publisher(topic_name, Twist) twist_msg = Twist() twist_msg.linear = Vector3(0.0, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 0.5) start_time = rospy.get_rostime() while rospy.get_rostime() < start_time + rospy.Duration(15.0 * rotate_count): base_publisher.publish(twist_msg) def navigate_to_shelf(self, marker_id = None): # Move forward, place book on the shelf, and move back if marker_id is None or marker_id is False: marker_id = self.marker_perception.get_marker_id() rospy.loginfo("marker id returned by get_marker_id is: " + str(marker_id)) if marker_id is False: rospy.loginfo("wuuuuuuuut") if marker_id is None: self._sound_client.say("I don't think I am holding a book " "right now") rospy.logwarn("Navigate to shelf called when marker id is None") return book = self.book_map.get(unicode(marker_id)) if book is None: self._sound_client.say("The book that I am holding is unknown " "to me") rospy.logwarn("Navigate to shelf called when marker id is not in database") return x = book.getXCoordinate() y = book.getYCoordinate() self.navigation.move_to_shelf(x, y) # Ye Olde Dropping way of putting a book on the shelf. Deprecating. def drop_on_shelf(self): self.saved_r_arm_pose = Milestone1GUI.PLACE_ON_SHELF_R_POS self.move_arm('r', 4.0, True) # Increase these numbers for slower movement self.move_base(True) self.r_gripper.open_gripper(True) self.saved_r_arm_pose = Milestone1GUI.RELEASE_BOOK_R_POS self.move_arm('r', 2.0, True) # Increase these numbers for slower movement self.move_base(False) self.marker_perception.reset_marker_id() def place_on_shelf(self): self.saved_r_arm_pose = Milestone1GUI.PLACE_ON_SHELF_R_POS self.move_arm('r', 4.0, True) # Increase these numbers for slower movement self.move_base(True) self.saved_r_arm_pose = Milestone1GUI.LOWER_ON_SHELF_R_POS self.move_arm('r', 2.0, True) # Increase these numbers for slower movement self.r_gripper.open_gripper(True) self.move_base(False) self.marker_perception.reset_marker_id() def give_information(self): marker_id = self.marker_perception.get_marker_id() rospy.loginfo("marker id returned by get_marker_id is: " + str(marker_id)) if marker_id is not None: book = self.book_map.get(unicode(marker_id)) if book is None: rospy.logwarn("Give information called when marker id is not in database") self._sound_client.say("The book that I am holding is unknown to me") else: self._sound_client.say(book.getInformation()) else: rospy.logwarn("Give information called when marker id is None") self._sound_client.say("I don't think I am holding a book right now") def pick_up_from_shelf_button(self): self.pick_up_from_shelf_routine(self.book_textbox.text()) def pick_up_from_shelf_routine(self, book_title): book_id = self.bookDB.getBookIdByTitle(book_title) if book_id is None: rospy.logwarn("Book asked for was not present in database") self._sound_client.say("The book you requested is not present in the database.") else: self.torso.move(.15) # move torso .1 meter from base (MAX is .2) self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS self.move_arm('l', 5.0) self.l_gripper.close_gripper() # self.marker_perception.set_marker_id(book_id) self.prepare_to_navigate() time.sleep(5) self.navigate_to_shelf(book_id) # Navigate to book location self.pick_up_from_shelf() # Pick up from the shelf self.prepare_to_navigate() time.sleep(5) self.navigate_to_person() # Navigate to designated help desk location self.give_book() # Give Book to Human def put_this_book_back_routine(self): self.take_from_human() time.sleep(5) self.prepare_to_navigate() time.sleep(5) self.navigate_to_shelf() self.place_on_shelf() self.prepare_to_navigate() # TODO: return to human? def give_information_routine(self, book_title): book_id = self.bookDB.getBookIdByTitle(book_title) if book_id is None: rospy.logwarn("Book asked for was not present in database") self._sound_client.say("The book you requested is not present in the database.") return book = self.book_map.get(unicode(book_id)) self._sound_client.say(book.getInformation()) def pick_up_from_shelf(self): self.saved_r_arm_pose = Milestone1GUI.LOWER_ON_SHELF_R_POS self.move_arm('r', 4.0, True) # Increase these numbers for slower movement self.r_gripper.open_gripper(True) self.move_base(True) self.r_gripper.close_gripper(True) self.move_base(False) def navigate_to_person(self): x = 2.82690143585 y = -0.416650295258 z = 0.252587109056 w = 0.967574158573 self.navigation.move_to_shelf(x, y, z, w) def give_book(self): self.saved_r_arm_pose = Milestone1GUI.RECEIVE_FROM_HUMAN_R_POS self.move_arm('r', 4.0, True) self.r_gripper.open_gripper(True) # Moves forward to the bookshelf (or backward if isForward is false) def move_base(self, isForward): topic_name = '/base_controller/command' base_publisher = rospy.Publisher(topic_name, Twist) distance = 0.25 if not isForward: distance *= -1 twist_msg = Twist() twist_msg.linear = Vector3(distance, 0.0, 0.0) twist_msg.angular = Vector3(0.0, 0.0, 0.0) for x in range(0, 15): rospy.loginfo("Moving the base") base_publisher.publish(twist_msg) time.sleep(0.15) time.sleep(1.5) # Moves arms using kinematics def move_arm(self, side_prefix, time_to_joints = 2.0, wait = False): # 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) self.move_to_joints(side_prefix, self.saved_r_arm_pose, time_to_joints, wait) 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) self.move_to_joints(side_prefix, self.saved_l_arm_pose, time_to_joints, wait) pass 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 move_to_joints(self, side_prefix, positions, time_to_joint, wait = False): '''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) if side_prefix == 'r': traj_goal.trajectory.joint_names = self.r_joint_names action_client = self.r_traj_action_client else: traj_goal.trajectory.joint_names = self.l_joint_names action_client = self.l_traj_action_client action_client.send_goal(traj_goal) if wait: time.sleep(time_to_joint)