예제 #1
0
    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"))
예제 #2
0
    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"))
예제 #3
0
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)
예제 #4
0
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)