class APCPlugin(Plugin):

#Components   @TODO: consider list and dynamic widget
    folder_name_launchfile = 'launch'
    launchfile_name_general = rospy.get_param('BUTTON_GEN_LAUNCH','openni2.launch')
    pkg_name_general = rospy.get_param('BUTTON_GEN_PKG', 'openni2_launch' )
    launchfile_name_bt = rospy.get_param('BUTTON_BT_LAUNCH','xs_bayer_rggb.launch')
    pkg_name_bt = rospy.get_param('BUTTON_BT_PKG', 'ueye_cam' )
    launchfile_name_perception= rospy.get_param('BUTTON_PERCEPTION_LAUNCH','openni2.launch')
    pkg_name_perception = rospy.get_param('BUTTON_PERCEPTION_PKG', 'openni2_launch' )
    launchfile_name_grasping = rospy.get_param('BUTTON_GRASPING_LAUNCH','openni2.launch')
    pkg_name_grasping = rospy.get_param('BUTTON_GRASPING_PKG', 'openni2_launch' )
    launchfile_name_calibration_1 = rospy.get_param('BUTTON_CALIBRATION_1_LAUNCH','openni2.launch')
    pkg_name_calibration_1 = rospy.get_param('BUTTON_CALIBRATION_1_PKG', 'openni2_launch' )
    launchfile_name_calibration_2 = rospy.get_param('BUTTON_CALIBRATION_2_LAUNCH','openni2.launch')
    pkg_name_calibration_2 = rospy.get_param('BUTTON_CALIBRATION_2_PKG', 'openni2_launch' )
    launchfile_name_calibration_3 = rospy.get_param('BUTTON_CALIBRATION_3_LAUNCH','openni2.launch')
    pkg_name_calibration_3 = rospy.get_param('BUTTON_CALIBRATION_3_PKG', 'openni2_launch' )


    args_general =  roslaunch.rlutil.resolve_launch_arguments([pkg_name_general, launchfile_name_general])
    args_bt =  roslaunch.rlutil.resolve_launch_arguments([pkg_name_bt, launchfile_name_bt])
    args_perception =  roslaunch.rlutil.resolve_launch_arguments([pkg_name_perception, launchfile_name_perception])
    args_grasping =  roslaunch.rlutil.resolve_launch_arguments([pkg_name_grasping, launchfile_name_grasping])
    args_calibration_1 =  roslaunch.rlutil.resolve_launch_arguments([pkg_name_calibration_1, launchfile_name_calibration_1])
    args_calibration_2 =  roslaunch.rlutil.resolve_launch_arguments([pkg_name_calibration_2, launchfile_name_calibration_2])
    args_calibtration_3 =  roslaunch.rlutil.resolve_launch_arguments([pkg_name_calibration_3, launchfile_name_calibration_3])

    runner_general = []
    runner_bt = []
    runner_perception = []
    runner_grasping = []
    runner_calibration_1 = []
    runner_calibration_2 = []
    runner_calibration_3 = []

    #icons
    _icon_node_start = QIcon.fromTheme('media-playback-start')
    _icon_node_stop = QIcon.fromTheme('media-playback-stop')
    _icon_respawn_toggle = QIcon.fromTheme('view-refresh')


    def __init__(self, context):
        super(APCPlugin, self).__init__(context)
        # Give QObjects reasonable names
        self.setObjectName('APCPlugin')

        # Process standalone plugin command-line arguments
        from argparse import ArgumentParser
        parser = ArgumentParser()
        # Add argument(s) to the parser.
        parser.add_argument("-q", "--quiet", action="store_true",
                      dest="quiet",
                      help="Put plugin in silent mode")
        args, unknowns = parser.parse_known_args(context.argv())
        if not args.quiet:
            print 'arguments: ', args
            print 'unknowns: ', unknowns

        # Create QWidget
        self._widget = QMainWindow()
        # Get path to UI file which should be in the "resource" folder of this package
        ui_file = os.path.join(rospkg.RosPack().get_path('rqt_apc'), 'resource', 'apc.ui')
        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self._widget)
        # Give QObjects reasonable names
        self._widget.setObjectName('APCPlugin')
        # Show _widget.windowTitle on left-top of each plugin (when
        # it's set in _widget). This is useful when you open multiple
        # plugins at once. Also if you open multiple instances of your
        # plugin at once, these lines add number to make it easy to
        # tell from pane to pane.
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
        # Add widget to the user interface
        context.add_widget(self._widget)


        # Add function calls for each of the buttons.
        self._widget.button_component_general.clicked.connect(self.button_component_general_clicked)
        self._widget.button_component_0.clicked.connect(self.button_component_0_clicked)
        self._widget.button_component_1.clicked.connect(self.button_component_1_clicked)
        self._widget.button_component_2.clicked.connect(self.button_component_2_clicked)
        self._widget.button_calibration_0.clicked.connect(self.button_calibration_0_clicked)
        self._widget.button_calibration_1.clicked.connect(self.button_calibration_1_clicked)
        self._widget.button_calibration_2.clicked.connect(self.button_calibration_2_clicked)

        self._widget.toggle_arm_button.clicked[bool].connect(self.toggle_arm_button_clicked)

        self._widget.home_button.clicked[bool].connect(self.home_button_clicked)
        self._widget.bin_a_button.clicked[bool].connect(self.bin_a_button_clicked)
        self._widget.bin_b_button.clicked[bool].connect(self.bin_b_button_clicked)
        self._widget.bin_c_button.clicked[bool].connect(self.bin_c_button_clicked)
        self._widget.bin_d_button.clicked[bool].connect(self.bin_d_button_clicked)
        self._widget.bin_e_button.clicked[bool].connect(self.bin_e_button_clicked)
        self._widget.bin_f_button.clicked[bool].connect(self.bin_f_button_clicked)
        self._widget.bin_g_button.clicked[bool].connect(self.bin_g_button_clicked)
        self._widget.bin_h_button.clicked[bool].connect(self.bin_h_button_clicked)
        self._widget.bin_i_button.clicked[bool].connect(self.bin_i_button_clicked)
        self._widget.bin_j_button.clicked[bool].connect(self.bin_j_button_clicked)
        self._widget.bin_k_button.clicked[bool].connect(self.bin_k_button_clicked)
        self._widget.bin_l_button.clicked[bool].connect(self.bin_l_button_clicked)

        self._widget.move_to_bin_button.clicked[bool].connect(self.move_to_bin_button_clicked)

        self._widget.left_gripper_button.clicked[bool].connect(self.left_gripper_button_clicked)
        self._widget.right_gripper_button.clicked[bool].connect(self.right_gripper_button_clicked)
        self._widget.left_suction_off_button.clicked[bool].connect(self.left_suction_off_button_clicked)
        self._widget.right_suction_off_button.clicked[bool].connect(self.right_suction_off_button_clicked)

        self._widget.start_bt_button.clicked[bool].connect(self.start_bt_button_clicked)
        self._widget.pause_bt_button.clicked[bool].connect(self.pause_bt_button_clicked)

        self._widget.click_to_pick_button.clicked[bool].connect(self.click_to_pick_button_clicked)
        self._widget.place_button.clicked[bool].connect(self.place_button_clicked)

        #Set icons to buttons
        self._widget.button_component_general.setIcon(self._icon_node_start)
        self._widget.button_component_diagn.setIcon(self._icon_node_start)
        self._widget.button_component_0.setIcon(self._icon_node_start)
        self._widget.button_component_1.setIcon(self._icon_node_start)
        self._widget.button_component_2.setIcon(self._icon_node_start)
        self._widget.button_calibration_0.setIcon(self._icon_node_start)
        self._widget.button_calibration_1.setIcon(self._icon_node_start)
        self._widget.button_calibration_2.setIcon(self._icon_node_start)


        self._json_data = None


        # for storing the joint states
        self._left_joint_state = [0.0 for x in xrange(7)]
        self._right_joint_state = [0.0 for x in xrange(7)]

        # for selecting the arm
        self._arm = 'left_arm'

        # subscribers
        self._joint_state_sub = rospy.Subscriber('/robot/joint_states', JointState, self.joint_state_cb)
        self._tf_listener = tf.TransformListener()
        self._move_arm_action_server_name = '/apc/manipulation/move_arm'
        self._move_arm_client = actionlib.SimpleActionClient(self._move_arm_action_server_name, MoveArmAction)

        self._pick_object_action_server_name = '/apc/manipulation/pick_object'
        self._pick_object_client = actionlib.SimpleActionClient(self._pick_object_action_server_name, PickObjectAction)

        self._place_object_action_server_name = '/apc/manipulation/place_object'
        self._place_object_client = actionlib.SimpleActionClient(self._place_object_action_server_name, PlaceObjectAction)


        # baxter grippers
        self._grippers_initialized = False


        self._timer = rospy.Timer(rospy.Duration(1.0), self.timer_cb)

    @staticmethod
    def add_arguments(parser):
        group = parser.add_argument_group('Options for rqt_apc plugin')
        # group.add_argument('bagfiles', type=argparse.FileType('r'), nargs='*', default=[], help='Bagfiles to load')

    def get_desperation(self):
        desperation = rospy.get_param('/apc/task_manager/desperation', 1)

        if desperation == 1:
            return "All is good, I'm so confident"
        elif desperation == 2:
            return "Ooops, using plan B"
        elif desperation == 3:
            return "Double oops, using plan C"
        elif desperation == 4:
            return "Triple oops, using plan D"

    def timer_cb(self, event):

        # get the demo parameter
        while not rospy.is_shutdown():
            try:
                demo = rospy.get_param('/apc/demo', 'NO DEMO PARAM')
                object = rospy.get_param("/apc/task_manager/target_object", 'NO OBJECT')
                bin = rospy.get_param("/apc/task_manager/target_bin", 'NO BIN')
                break
            except:
                rospy.sleep(random.uniform(0, 1))
                continue

        if rospy.is_shutdown():
            return

        if self._json_data == None:
            self.init_json()

        self._widget.demo_label.setText(demo)
        self._widget.action_label.setText(rospy.get_param('/apc/bt/action', 'no action'))
        self._widget.action_label_2.setText(self.get_desperation())
        self._widget.json_filename_label.setText('apc_bt_launcher/data/' + self._json_filename)

        self._widget.current_arm_label.setText(self._arm)

        self._widget.item_label.setText(object)
        self._widget.bin_label.setText(bin)

        # check if gripper state has been published and initialize the grippers
        if not self._grippers_initialized:
            try:
                rospy.wait_for_message('/robot/end_effector/left_gripper/state', EndEffectorState, 1.0)
                rospy.wait_for_message('/robot/end_effector/right_gripper/state', EndEffectorState, 1.0)
                self._left_gripper = baxter_interface.Gripper('left')
                self._right_gripper = baxter_interface.Gripper('right')
                self._grippers_initialized = True
            except rospy.ROSException:
                self._grippers_initialized = False
                rospy.logwarn('[' + rospy.get_name() + ']: waiting for gripper state on topic /robot/end_effector/*_gripper/state')


        # set the joint states

        left_joint_state_str = str(['%.2f' % q for q in self._left_joint_state])
        right_joint_state_str = str(['%.2f' % q for q in self._right_joint_state])

        if self._arm == "left_arm":
            self._widget.joint_pos_label.setText(str(left_joint_state_str))
            gripper_frame = 'left_gripper'
        else:
            self._widget.joint_pos_label.setText(str(right_joint_state_str))
            gripper_frame = 'right_gripper'

        # get the transform for the gripper tip

        try:
            gripper_pose = posemath.fromTf(self._tf_listener.lookupTransform('base', gripper_frame, rospy.Time(0)))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            return

        rpy = gripper_pose.M.GetRPY()
        pose = ['' for x in xrange(6)]

        for index in xrange(3):
            pose[index] = '%.2f' % gripper_pose.p[index]
            pose[index+3] = '%.2f' % rpy[index]

        self._widget.pose_label.setText(str(pose))

    def init_json(self):

        # get the demo parameter
        while not rospy.is_shutdown():
            try:
                self._json_filename = rospy.get_param('/apc/task_manager/json_filename', 'NO JSON FILENAME')
                break
            except:
                rospy.sleep(random.uniform(0, 1))
                continue

        # open the json file and get the object to pick according to the bin
        rospack = rospkg.RosPack()
        json_path = rospack.get_path('apc_bt_launcher') + '/data/'
        json_file = open(json_path + self._json_filename + '.json')

        self._json_data = json.load(json_file)

        json_file.close()



    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


    # Callbacks ------------------
    def joint_state_cb(self, msg):
        '''
        Get the joint states
        :param msg: joint state message
        :return:
        '''

        joint_names = msg.name
        joint_index = 0
        for joint_name in joint_names:
            joint_number = self.get_joint_number(joint_name)
            if joint_number>=0:
                if joint_name[:4] == "left":
                    self._left_joint_state[joint_number] = msg.position[joint_index]
                elif joint_name[:5] == 'right':
                    self._right_joint_state[joint_number] = msg.position[joint_index]

            joint_index += 1


    def get_joint_number(self, joint_name):
        '''
        Converts Baxter joint names e.g. 'left_w0' to number index from 1...7
        :param joint_name: string with joint name
        :return: joint_index: int between 0 and 6, -1 if not valid joint
        '''

        # first strip the prefix 'left_' or 'right_'
        joint = ''
        joint = joint_name.strip('left_')
        joint = joint.strip('right_')

        joint_dict = {'s0': 0, 's1': 1, 'e0': 2, 'e1': 3, 'w0': 4, 'w1': 5, 'w2': 6}

        if joint_dict.has_key(joint):
            return joint_dict[joint]

        else:
            return -1


    # Buttons --------------------

    # ============= Arm stuff ========================

    def toggle_arm_button_clicked(self):
        '''
        switch arm 'left' <--> 'right'
        :return:
        '''

        if self._arm == 'left_arm':
            self._arm = 'right_arm'

        else:
            self._arm = 'left_arm'

    def left_gripper_button_clicked(self):
        '''
        Turn on/off the left gripper
        :return:
        '''
        self.activate_gripper('left_arm')

    def right_gripper_button_clicked(self):
        '''
        turn on/off the right gripper
        :return:
        '''
        self.activate_gripper('right_arm')

    def activate_gripper(self, arm):
        '''
        turn on a gripper
        :param arm:
        :return:
        '''
        # only run if grippers have been detected
        if not self._grippers_initialized:
            return False

        if arm=='left_arm':
            self._left_gripper.command_suction(timeout=15.0)

        else:
            self._right_gripper.command_suction(timeout=15.0)

        return True

    def left_suction_off_button_clicked(self):

        if not self._grippers_initialized:
            return

        self._left_gripper.stop()

    def right_suction_off_button_clicked(self):

        if not self._grippers_initialized:
            return

        self._right_gripper.stop()



    def home_button_clicked(self):
        self.move_arm_bin('HOME')

    def bin_a_button_clicked(self):
        self.move_arm_bin('bin_A')

    def bin_b_button_clicked(self):
        self.move_arm_bin('bin_B')

    def bin_c_button_clicked(self):
        self.move_arm_bin('bin_C')

    def bin_d_button_clicked(self):
        self.move_arm_bin('bin_D')

    def bin_e_button_clicked(self):
        self.move_arm_bin('bin_E')

    def bin_f_button_clicked(self):
        self.move_arm_bin('bin_F')

    def bin_g_button_clicked(self):
        self.move_arm_bin('bin_G')

    def bin_h_button_clicked(self):
        self.move_arm_bin('bin_H')

    def bin_i_button_clicked(self):
        self.move_arm_bin('bin_I')

    def bin_j_button_clicked(self):
        self.move_arm_bin('bin_J')

    def bin_k_button_clicked(self):
        self.move_arm_bin('bin_K')

    def bin_l_button_clicked(self):
        self.move_arm_bin('bin_L')

    def move_to_bin_button_clicked(self):
        self.move_arm_bin(self._widget.bin_text_input.toPlainText())

    def move_arm_bin(self, bin):
        self._bin = bin
        goal = MoveArmGoal(goalBin=bin, arm=self._arm)
        self._move_arm_client.send_goal(goal)

    def click_to_pick_button_clicked(self):

        # only do this if JSON file has been initialized
        if not self._json_data == None:

            # get the target object for the bin
            for bin in self._json_data['work_order']:
                if bin['bin'] == self._bin:
                    obj = bin['item']
                    break

            goal = PickObjectGoal(targetObject=obj, bin_id=self._bin, arm=self._arm)
            self._pick_object_client.send_goal(goal)

        else:
            rospy.logwarn('[' + rospy.get_name() + ']: json file not initialized.')

    def place_button_clicked(self):

        return 0



    # ============= BT stuff ========================

    def start_bt_button_clicked(self):
        '''

        :return:
        '''
        rospy.set_param('/apc/task_manager/running', True)


    def pause_bt_button_clicked(self):
        '''

        :param self:
        :return:
        '''
        rospy.set_param('/apc/task_manager/running', False)

    # ============= SYS stuff ========================

    def button_component_general_clicked(self):
        if self._widget.button_component_general.isChecked():
            self._widget.button_component_general.setIcon(self._icon_node_stop)
            #launchfile = os.path.join(rospkg.RosPack().get_path(pkg_name), folder_name_launchfile, launchfile_name)
            #print( launchfile)
            self.runner_general = roslaunch.parent.ROSLaunchParent( rospy.get_param("/run_id"), self.args_general)
            self.runner_general.start()
            print "Running the KTH APC system"
        else:
            self.runner_general.shutdown()
            self._widget.button_component_general.setIcon(self._icon_node_start)


    def button_component_0_clicked(self):
        if self._widget.button_component_0.isChecked():
            self._widget.button_component_0.setIcon(self._icon_node_stop)

            print "Running the BT (standalone)"
            self.runner_bt = roslaunch.parent.ROSLaunchParent( rospy.get_param("/run_id"), self.args_bt)
            self.runner_bt.start()
        else:
            self.runner_bt.shutdown()
            self._widget.button_component_0.setIcon(self._icon_node_start)
            print "Killing the BT (standalone)"

    def button_component_1_clicked(self):
        if self._widget.button_component_1.isChecked():
            self._widget.button_component_1.setIcon(self._icon_node_stop)

            print "Running the PERCEPTION component (standalone)"
            self.runner_perception = roslaunch.parent.ROSLaunchParent( rospy.get_param("/run_id"), self.args_perception)
            self.runner_perception.start()
        else:
            self.runner_perception.shutdown()
            self._widget.button_component_1.setIcon(self._icon_node_start)
            print "Killing the PERCEPTION component (standalone)"

    def button_component_2_clicked(self):
        if self._widget.button_component_2.isChecked():
            self._widget.button_component_2.setIcon(self._icon_node_stop)

            print "Running the GRASPING component (standalone)"
            self.runner_grasping = roslaunch.parent.ROSLaunchParent( rospy.get_param("/run_id"), self.args_grasping)
            self.runner_grasping.start()
        else:
            self.runner_grasping.shutdown()
            self._widget.button_component_2.setIcon(self._icon_node_start)
            print "Killing the GRASPING component (standalone)"

    def button_calibration_0_clicked(self):
        if self._widget.button_calibration_0.isChecked():
            self._widget.button_calibration_0.setIcon(self._icon_node_stop)

            print "Running the CALIBRATION for the Kinect chest"
            self.runner_calibration_1 = roslaunch.parent.ROSLaunchParent( rospy.get_param("/run_id"), self.args_calibration_1)
            self.runner_calibration_1.start()
        else:
            self.runner_calibration_1.shutdown()
            self._widget.button_calibration_0.setIcon(self._icon_node_start)
            print "kILLING the CALIBRATION for the Kinect chest"

    def button_calibration_1_clicked(self):
        if self._widget.button_calibration_1.isChecked():
            self._widget.button_calibration_1.setIcon(self._icon_node_stop)

            print "Running the CALIBRATION for the Kinect head"
            self.runner_calibration_2 = roslaunch.parent.ROSLaunchParent( rospy.get_param("/run_id"), self.args_calibration_2)
            self.runner_calibration_2.start()
        else:
            self.runner_calibration_2.shutdown()
            self._widget.button_calibration_1.setIcon(self._icon_node_start)
            print "kILLING the CALIBRATION for the Kinect head"

    def button_calibration_2_clicked(self):
        if self._widget.button_calibration_2.isChecked():
            self._widget.button_calibration_2.setIcon(self._icon_node_stop)

            print "Running the CALIBRATION for the IDS camera"
            self.runner_calibration_3 = roslaunch.parent.ROSLaunchParent( rospy.get_param("/run_id"), self.args_calibration_3)
            self.runner_calibration_3.start()
        else:
            self.runner_calibration_3.shutdown()
            self._widget.button_calibration_2.setIcon(self._icon_node_start)
            print "kILLING the CALIBRATION for the IDS camera"
Exemplo n.º 2
0
class APCPlugin(Plugin):

    #Components   @TODO: consider list and dynamic widget
    folder_name_launchfile = 'launch'
    launchfile_name_general = rospy.get_param('BUTTON_GEN_LAUNCH',
                                              'openni2.launch')
    pkg_name_general = rospy.get_param('BUTTON_GEN_PKG', 'openni2_launch')
    launchfile_name_bt = rospy.get_param('BUTTON_BT_LAUNCH',
                                         'xs_bayer_rggb.launch')
    pkg_name_bt = rospy.get_param('BUTTON_BT_PKG', 'ueye_cam')
    launchfile_name_perception = rospy.get_param('BUTTON_PERCEPTION_LAUNCH',
                                                 'openni2.launch')
    pkg_name_perception = rospy.get_param('BUTTON_PERCEPTION_PKG',
                                          'openni2_launch')
    launchfile_name_grasping = rospy.get_param('BUTTON_GRASPING_LAUNCH',
                                               'openni2.launch')
    pkg_name_grasping = rospy.get_param('BUTTON_GRASPING_PKG',
                                        'openni2_launch')
    launchfile_name_calibration_1 = rospy.get_param(
        'BUTTON_CALIBRATION_1_LAUNCH', 'openni2.launch')
    pkg_name_calibration_1 = rospy.get_param('BUTTON_CALIBRATION_1_PKG',
                                             'openni2_launch')
    launchfile_name_calibration_2 = rospy.get_param(
        'BUTTON_CALIBRATION_2_LAUNCH', 'openni2.launch')
    pkg_name_calibration_2 = rospy.get_param('BUTTON_CALIBRATION_2_PKG',
                                             'openni2_launch')
    launchfile_name_calibration_3 = rospy.get_param(
        'BUTTON_CALIBRATION_3_LAUNCH', 'openni2.launch')
    pkg_name_calibration_3 = rospy.get_param('BUTTON_CALIBRATION_3_PKG',
                                             'openni2_launch')

    args_general = roslaunch.rlutil.resolve_launch_arguments(
        [pkg_name_general, launchfile_name_general])
    args_bt = roslaunch.rlutil.resolve_launch_arguments(
        [pkg_name_bt, launchfile_name_bt])
    args_perception = roslaunch.rlutil.resolve_launch_arguments(
        [pkg_name_perception, launchfile_name_perception])
    args_grasping = roslaunch.rlutil.resolve_launch_arguments(
        [pkg_name_grasping, launchfile_name_grasping])
    args_calibration_1 = roslaunch.rlutil.resolve_launch_arguments(
        [pkg_name_calibration_1, launchfile_name_calibration_1])
    args_calibration_2 = roslaunch.rlutil.resolve_launch_arguments(
        [pkg_name_calibration_2, launchfile_name_calibration_2])
    args_calibtration_3 = roslaunch.rlutil.resolve_launch_arguments(
        [pkg_name_calibration_3, launchfile_name_calibration_3])

    runner_general = []
    runner_bt = []
    runner_perception = []
    runner_grasping = []
    runner_calibration_1 = []
    runner_calibration_2 = []
    runner_calibration_3 = []

    #icons
    _icon_node_start = QIcon.fromTheme('media-playback-start')
    _icon_node_stop = QIcon.fromTheme('media-playback-stop')
    _icon_respawn_toggle = QIcon.fromTheme('view-refresh')

    def __init__(self, context):
        super(APCPlugin, self).__init__(context)
        # Give QObjects reasonable names
        self.setObjectName('APCPlugin')

        # Process standalone plugin command-line arguments
        from argparse import ArgumentParser
        parser = ArgumentParser()
        # Add argument(s) to the parser.
        parser.add_argument("-q",
                            "--quiet",
                            action="store_true",
                            dest="quiet",
                            help="Put plugin in silent mode")
        args, unknowns = parser.parse_known_args(context.argv())
        if not args.quiet:
            print 'arguments: ', args
            print 'unknowns: ', unknowns

        # Create QWidget
        self._widget = QMainWindow()
        # Get path to UI file which should be in the "resource" folder of this package
        ui_file = os.path.join(rospkg.RosPack().get_path('rqt_apc'),
                               'resource', 'apc.ui')
        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self._widget)
        # Give QObjects reasonable names
        self._widget.setObjectName('APCPlugin')
        # Show _widget.windowTitle on left-top of each plugin (when
        # it's set in _widget). This is useful when you open multiple
        # plugins at once. Also if you open multiple instances of your
        # plugin at once, these lines add number to make it easy to
        # tell from pane to pane.
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() +
                                        (' (%d)' % context.serial_number()))
        # Add widget to the user interface
        context.add_widget(self._widget)

        # Add function calls for each of the buttons.
        self._widget.button_component_general.clicked.connect(
            self.button_component_general_clicked)
        self._widget.button_component_0.clicked.connect(
            self.button_component_0_clicked)
        self._widget.button_component_1.clicked.connect(
            self.button_component_1_clicked)
        self._widget.button_component_2.clicked.connect(
            self.button_component_2_clicked)
        self._widget.button_calibration_0.clicked.connect(
            self.button_calibration_0_clicked)
        self._widget.button_calibration_1.clicked.connect(
            self.button_calibration_1_clicked)
        self._widget.button_calibration_2.clicked.connect(
            self.button_calibration_2_clicked)

        self._widget.toggle_arm_button.clicked[bool].connect(
            self.toggle_arm_button_clicked)

        self._widget.home_button.clicked[bool].connect(
            self.home_button_clicked)
        self._widget.bin_a_button.clicked[bool].connect(
            self.bin_a_button_clicked)
        self._widget.bin_b_button.clicked[bool].connect(
            self.bin_b_button_clicked)
        self._widget.bin_c_button.clicked[bool].connect(
            self.bin_c_button_clicked)
        self._widget.bin_d_button.clicked[bool].connect(
            self.bin_d_button_clicked)
        self._widget.bin_e_button.clicked[bool].connect(
            self.bin_e_button_clicked)
        self._widget.bin_f_button.clicked[bool].connect(
            self.bin_f_button_clicked)
        self._widget.bin_g_button.clicked[bool].connect(
            self.bin_g_button_clicked)
        self._widget.bin_h_button.clicked[bool].connect(
            self.bin_h_button_clicked)
        self._widget.bin_i_button.clicked[bool].connect(
            self.bin_i_button_clicked)
        self._widget.bin_j_button.clicked[bool].connect(
            self.bin_j_button_clicked)
        self._widget.bin_k_button.clicked[bool].connect(
            self.bin_k_button_clicked)
        self._widget.bin_l_button.clicked[bool].connect(
            self.bin_l_button_clicked)

        self._widget.move_to_bin_button.clicked[bool].connect(
            self.move_to_bin_button_clicked)

        self._widget.left_gripper_button.clicked[bool].connect(
            self.left_gripper_button_clicked)
        self._widget.right_gripper_button.clicked[bool].connect(
            self.right_gripper_button_clicked)
        self._widget.left_suction_off_button.clicked[bool].connect(
            self.left_suction_off_button_clicked)
        self._widget.right_suction_off_button.clicked[bool].connect(
            self.right_suction_off_button_clicked)

        self._widget.start_bt_button.clicked[bool].connect(
            self.start_bt_button_clicked)
        self._widget.pause_bt_button.clicked[bool].connect(
            self.pause_bt_button_clicked)

        self._widget.click_to_pick_button.clicked[bool].connect(
            self.click_to_pick_button_clicked)
        self._widget.place_button.clicked[bool].connect(
            self.place_button_clicked)

        #Set icons to buttons
        self._widget.button_component_general.setIcon(self._icon_node_start)
        self._widget.button_component_diagn.setIcon(self._icon_node_start)
        self._widget.button_component_0.setIcon(self._icon_node_start)
        self._widget.button_component_1.setIcon(self._icon_node_start)
        self._widget.button_component_2.setIcon(self._icon_node_start)
        self._widget.button_calibration_0.setIcon(self._icon_node_start)
        self._widget.button_calibration_1.setIcon(self._icon_node_start)
        self._widget.button_calibration_2.setIcon(self._icon_node_start)

        self._json_data = None

        # for storing the joint states
        self._left_joint_state = [0.0 for x in xrange(7)]
        self._right_joint_state = [0.0 for x in xrange(7)]

        # for selecting the arm
        self._arm = 'left_arm'

        # subscribers
        self._joint_state_sub = rospy.Subscriber('/robot/joint_states',
                                                 JointState,
                                                 self.joint_state_cb)
        self._tf_listener = tf.TransformListener()
        self._move_arm_action_server_name = '/apc/manipulation/move_arm'
        self._move_arm_client = actionlib.SimpleActionClient(
            self._move_arm_action_server_name, MoveArmAction)

        self._pick_object_action_server_name = '/apc/manipulation/pick_object'
        self._pick_object_client = actionlib.SimpleActionClient(
            self._pick_object_action_server_name, PickObjectAction)

        self._place_object_action_server_name = '/apc/manipulation/place_object'
        self._place_object_client = actionlib.SimpleActionClient(
            self._place_object_action_server_name, PlaceObjectAction)

        # baxter grippers
        self._grippers_initialized = False

        self._timer = rospy.Timer(rospy.Duration(1.0), self.timer_cb)

    @staticmethod
    def add_arguments(parser):
        group = parser.add_argument_group('Options for rqt_apc plugin')
        # group.add_argument('bagfiles', type=argparse.FileType('r'), nargs='*', default=[], help='Bagfiles to load')

    def get_desperation(self):
        desperation = rospy.get_param('/apc/task_manager/desperation', 1)

        if desperation == 1:
            return "All is good, I'm so confident"
        elif desperation == 2:
            return "Ooops, using plan B"
        elif desperation == 3:
            return "Double oops, using plan C"
        elif desperation == 4:
            return "Triple oops, using plan D"

    def timer_cb(self, event):

        # get the demo parameter
        while not rospy.is_shutdown():
            try:
                demo = rospy.get_param('/apc/demo', 'NO DEMO PARAM')
                object = rospy.get_param("/apc/task_manager/target_object",
                                         'NO OBJECT')
                bin = rospy.get_param("/apc/task_manager/target_bin", 'NO BIN')
                break
            except:
                rospy.sleep(random.uniform(0, 1))
                continue

        if rospy.is_shutdown():
            return

        if self._json_data == None:
            self.init_json()

        self._widget.demo_label.setText(demo)
        self._widget.action_label.setText(
            rospy.get_param('/apc/bt/action', 'no action'))
        self._widget.action_label_2.setText(self.get_desperation())
        self._widget.json_filename_label.setText('apc_bt_launcher/data/' +
                                                 self._json_filename)

        self._widget.current_arm_label.setText(self._arm)

        self._widget.item_label.setText(object)
        self._widget.bin_label.setText(bin)

        # check if gripper state has been published and initialize the grippers
        if not self._grippers_initialized:
            try:
                rospy.wait_for_message(
                    '/robot/end_effector/left_gripper/state', EndEffectorState,
                    1.0)
                rospy.wait_for_message(
                    '/robot/end_effector/right_gripper/state',
                    EndEffectorState, 1.0)
                self._left_gripper = baxter_interface.Gripper('left')
                self._right_gripper = baxter_interface.Gripper('right')
                self._grippers_initialized = True
            except rospy.ROSException:
                self._grippers_initialized = False
                rospy.logwarn(
                    '[' + rospy.get_name() +
                    ']: waiting for gripper state on topic /robot/end_effector/*_gripper/state'
                )

        # set the joint states

        left_joint_state_str = str(
            ['%.2f' % q for q in self._left_joint_state])
        right_joint_state_str = str(
            ['%.2f' % q for q in self._right_joint_state])

        if self._arm == "left_arm":
            self._widget.joint_pos_label.setText(str(left_joint_state_str))
            gripper_frame = 'left_gripper'
        else:
            self._widget.joint_pos_label.setText(str(right_joint_state_str))
            gripper_frame = 'right_gripper'

        # get the transform for the gripper tip

        try:
            gripper_pose = posemath.fromTf(
                self._tf_listener.lookupTransform('base', gripper_frame,
                                                  rospy.Time(0)))
        except (tf.LookupException, tf.ConnectivityException,
                tf.ExtrapolationException):
            return

        rpy = gripper_pose.M.GetRPY()
        pose = ['' for x in xrange(6)]

        for index in xrange(3):
            pose[index] = '%.2f' % gripper_pose.p[index]
            pose[index + 3] = '%.2f' % rpy[index]

        self._widget.pose_label.setText(str(pose))

    def init_json(self):

        # get the demo parameter
        while not rospy.is_shutdown():
            try:
                self._json_filename = rospy.get_param(
                    '/apc/task_manager/json_filename', 'NO JSON FILENAME')
                break
            except:
                rospy.sleep(random.uniform(0, 1))
                continue

        # open the json file and get the object to pick according to the bin
        rospack = rospkg.RosPack()
        json_path = rospack.get_path('apc_bt_launcher') + '/data/'
        json_file = open(json_path + self._json_filename + '.json')

        self._json_data = json.load(json_file)

        json_file.close()

    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

    # Callbacks ------------------
    def joint_state_cb(self, msg):
        '''
        Get the joint states
        :param msg: joint state message
        :return:
        '''

        joint_names = msg.name
        joint_index = 0
        for joint_name in joint_names:
            joint_number = self.get_joint_number(joint_name)
            if joint_number >= 0:
                if joint_name[:4] == "left":
                    self._left_joint_state[joint_number] = msg.position[
                        joint_index]
                elif joint_name[:5] == 'right':
                    self._right_joint_state[joint_number] = msg.position[
                        joint_index]

            joint_index += 1

    def get_joint_number(self, joint_name):
        '''
        Converts Baxter joint names e.g. 'left_w0' to number index from 1...7
        :param joint_name: string with joint name
        :return: joint_index: int between 0 and 6, -1 if not valid joint
        '''

        # first strip the prefix 'left_' or 'right_'
        joint = ''
        joint = joint_name.strip('left_')
        joint = joint.strip('right_')

        joint_dict = {
            's0': 0,
            's1': 1,
            'e0': 2,
            'e1': 3,
            'w0': 4,
            'w1': 5,
            'w2': 6
        }

        if joint_dict.has_key(joint):
            return joint_dict[joint]

        else:
            return -1

    # Buttons --------------------

    # ============= Arm stuff ========================

    def toggle_arm_button_clicked(self):
        '''
        switch arm 'left' <--> 'right'
        :return:
        '''

        if self._arm == 'left_arm':
            self._arm = 'right_arm'

        else:
            self._arm = 'left_arm'

    def left_gripper_button_clicked(self):
        '''
        Turn on/off the left gripper
        :return:
        '''
        self.activate_gripper('left_arm')

    def right_gripper_button_clicked(self):
        '''
        turn on/off the right gripper
        :return:
        '''
        self.activate_gripper('right_arm')

    def activate_gripper(self, arm):
        '''
        turn on a gripper
        :param arm:
        :return:
        '''
        # only run if grippers have been detected
        if not self._grippers_initialized:
            return False

        if arm == 'left_arm':
            self._left_gripper.command_suction(timeout=15.0)

        else:
            self._right_gripper.command_suction(timeout=15.0)

        return True

    def left_suction_off_button_clicked(self):

        if not self._grippers_initialized:
            return

        self._left_gripper.stop()

    def right_suction_off_button_clicked(self):

        if not self._grippers_initialized:
            return

        self._right_gripper.stop()

    def home_button_clicked(self):
        self.move_arm_bin('HOME')

    def bin_a_button_clicked(self):
        self.move_arm_bin('bin_A')

    def bin_b_button_clicked(self):
        self.move_arm_bin('bin_B')

    def bin_c_button_clicked(self):
        self.move_arm_bin('bin_C')

    def bin_d_button_clicked(self):
        self.move_arm_bin('bin_D')

    def bin_e_button_clicked(self):
        self.move_arm_bin('bin_E')

    def bin_f_button_clicked(self):
        self.move_arm_bin('bin_F')

    def bin_g_button_clicked(self):
        self.move_arm_bin('bin_G')

    def bin_h_button_clicked(self):
        self.move_arm_bin('bin_H')

    def bin_i_button_clicked(self):
        self.move_arm_bin('bin_I')

    def bin_j_button_clicked(self):
        self.move_arm_bin('bin_J')

    def bin_k_button_clicked(self):
        self.move_arm_bin('bin_K')

    def bin_l_button_clicked(self):
        self.move_arm_bin('bin_L')

    def move_to_bin_button_clicked(self):
        self.move_arm_bin(self._widget.bin_text_input.toPlainText())

    def move_arm_bin(self, bin):
        self._bin = bin
        goal = MoveArmGoal(goalBin=bin, arm=self._arm)
        self._move_arm_client.send_goal(goal)

    def click_to_pick_button_clicked(self):

        # only do this if JSON file has been initialized
        if not self._json_data == None:

            # get the target object for the bin
            for bin in self._json_data['work_order']:
                if bin['bin'] == self._bin:
                    obj = bin['item']
                    break

            goal = PickObjectGoal(targetObject=obj,
                                  bin_id=self._bin,
                                  arm=self._arm)
            self._pick_object_client.send_goal(goal)

        else:
            rospy.logwarn('[' + rospy.get_name() +
                          ']: json file not initialized.')

    def place_button_clicked(self):

        return 0

    # ============= BT stuff ========================

    def start_bt_button_clicked(self):
        '''

        :return:
        '''
        rospy.set_param('/apc/task_manager/running', True)

    def pause_bt_button_clicked(self):
        '''

        :param self:
        :return:
        '''
        rospy.set_param('/apc/task_manager/running', False)

    # ============= SYS stuff ========================

    def button_component_general_clicked(self):
        if self._widget.button_component_general.isChecked():
            self._widget.button_component_general.setIcon(self._icon_node_stop)
            #launchfile = os.path.join(rospkg.RosPack().get_path(pkg_name), folder_name_launchfile, launchfile_name)
            #print( launchfile)
            self.runner_general = roslaunch.parent.ROSLaunchParent(
                rospy.get_param("/run_id"), self.args_general)
            self.runner_general.start()
            print "Running the KTH APC system"
        else:
            self.runner_general.shutdown()
            self._widget.button_component_general.setIcon(
                self._icon_node_start)

    def button_component_0_clicked(self):
        if self._widget.button_component_0.isChecked():
            self._widget.button_component_0.setIcon(self._icon_node_stop)

            print "Running the BT (standalone)"
            self.runner_bt = roslaunch.parent.ROSLaunchParent(
                rospy.get_param("/run_id"), self.args_bt)
            self.runner_bt.start()
        else:
            self.runner_bt.shutdown()
            self._widget.button_component_0.setIcon(self._icon_node_start)
            print "Killing the BT (standalone)"

    def button_component_1_clicked(self):
        if self._widget.button_component_1.isChecked():
            self._widget.button_component_1.setIcon(self._icon_node_stop)

            print "Running the PERCEPTION component (standalone)"
            self.runner_perception = roslaunch.parent.ROSLaunchParent(
                rospy.get_param("/run_id"), self.args_perception)
            self.runner_perception.start()
        else:
            self.runner_perception.shutdown()
            self._widget.button_component_1.setIcon(self._icon_node_start)
            print "Killing the PERCEPTION component (standalone)"

    def button_component_2_clicked(self):
        if self._widget.button_component_2.isChecked():
            self._widget.button_component_2.setIcon(self._icon_node_stop)

            print "Running the GRASPING component (standalone)"
            self.runner_grasping = roslaunch.parent.ROSLaunchParent(
                rospy.get_param("/run_id"), self.args_grasping)
            self.runner_grasping.start()
        else:
            self.runner_grasping.shutdown()
            self._widget.button_component_2.setIcon(self._icon_node_start)
            print "Killing the GRASPING component (standalone)"

    def button_calibration_0_clicked(self):
        if self._widget.button_calibration_0.isChecked():
            self._widget.button_calibration_0.setIcon(self._icon_node_stop)

            print "Running the CALIBRATION for the Kinect chest"
            self.runner_calibration_1 = roslaunch.parent.ROSLaunchParent(
                rospy.get_param("/run_id"), self.args_calibration_1)
            self.runner_calibration_1.start()
        else:
            self.runner_calibration_1.shutdown()
            self._widget.button_calibration_0.setIcon(self._icon_node_start)
            print "kILLING the CALIBRATION for the Kinect chest"

    def button_calibration_1_clicked(self):
        if self._widget.button_calibration_1.isChecked():
            self._widget.button_calibration_1.setIcon(self._icon_node_stop)

            print "Running the CALIBRATION for the Kinect head"
            self.runner_calibration_2 = roslaunch.parent.ROSLaunchParent(
                rospy.get_param("/run_id"), self.args_calibration_2)
            self.runner_calibration_2.start()
        else:
            self.runner_calibration_2.shutdown()
            self._widget.button_calibration_1.setIcon(self._icon_node_start)
            print "kILLING the CALIBRATION for the Kinect head"

    def button_calibration_2_clicked(self):
        if self._widget.button_calibration_2.isChecked():
            self._widget.button_calibration_2.setIcon(self._icon_node_stop)

            print "Running the CALIBRATION for the IDS camera"
            self.runner_calibration_3 = roslaunch.parent.ROSLaunchParent(
                rospy.get_param("/run_id"), self.args_calibration_3)
            self.runner_calibration_3.start()
        else:
            self.runner_calibration_3.shutdown()
            self._widget.button_calibration_2.setIcon(self._icon_node_start)
            print "kILLING the CALIBRATION for the IDS camera"
Exemplo n.º 3
0
class PR2AmazonChallengePlugin(Plugin):

    def __init__(self, context):
        super(PR2AmazonChallengePlugin, self).__init__(context)
        # Give QObjects reasonable names
        self.setObjectName('PR2AmazonChallengePlugin')

        # Process standalone plugin command-line arguments
        from argparse import ArgumentParser
        parser = ArgumentParser()
        # Add argument(s) to the parser.
        parser.add_argument("-q", "--quiet", action="store_true",
                      dest="quiet",
                      help="Put plugin in silent mode")
        args, unknowns = parser.parse_known_args(context.argv())
        if not args.quiet:
            print 'arguments: ', args
            print 'unknowns: ', unknowns

        # Create QWidget
        self._widget = QMainWindow()
        # Get path to UI file which should be in the "resource" folder of this package
        ui_file = os.path.join(rospkg.RosPack().get_path('rqt_amazon_challenge'), 'resource', 'pr2amazonchallenge.ui')
        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self._widget)
        # Give QObjects reasonable names
        self._widget.setObjectName('PR2AmazonChallengePlugin')
        # Show _widget.windowTitle on left-top of each plugin (when
        # it's set in _widget). This is useful when you open multiple
        # plugins at once. Also if you open multiple instances of your
        # plugin at once, these lines add number to make it easy to
        # tell from pane to pane.
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
        # Add widget to the user interface
        context.add_widget(self._widget)


        while not rospy.is_shutdown():
            try:
                base_move_params = rospy.get_param('/base_move')
                self._tool_size = rospy.get_param('/tool_size', [0.16, 0.02, 0.04])
                self._contest = rospy.get_param('/contest', True)
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        # get base_move parameters

        self._l_gripper_pub = rospy.Publisher('/l_gripper_controller/command', Pr2GripperCommand)


        self._item = ''

        if self._contest:
            self._length_tool = 0.18 + self._tool_size[0]
        else:
            self._length_tool = 0.216 + self._tool_size[0]


        self._shelf_pose_sub = rospy.Subscriber("/pubShelfSep", PoseStamped, self.get_shelf_pose)
        self._got_shelf_pose = False

        # button click callbacks
        self._widget.l_arm_start_pos_button.clicked[bool].connect(self._handle_l_arm_start_pos_button_clicked)
        self._widget.l_arm_row_1_pos_button.clicked[bool].connect(self._handle_l_arm_row_1_pos_button_clicked)
        self._widget.l_arm_row_2_pos_button.clicked[bool].connect(self._handle_l_arm_row_2_pos_button_clicked)
        self._widget.l_arm_row_3_pos_button.clicked[bool].connect(self._handle_l_arm_row_3_pos_button_clicked)
        self._widget.l_arm_row_4_pos_button.clicked[bool].connect(self._handle_l_arm_row_4_pos_button_clicked)

        self._widget.r_arm_start_pos_button.clicked[bool].connect(self._handle_r_arm_start_pos_button_clicked)
        self._widget.r_arm_row_1_pos_button.clicked[bool].connect(self._handle_r_arm_row_1_pos_button_clicked)
        self._widget.r_arm_row_2_pos_button.clicked[bool].connect(self._handle_r_arm_row_2_pos_button_clicked)
        self._widget.r_arm_row_3_pos_button.clicked[bool].connect(self._handle_r_arm_row_3_pos_button_clicked)
        self._widget.r_arm_row_4_pos_button.clicked[bool].connect(self._handle_r_arm_row_4_pos_button_clicked)

        self._widget.torso_start_pos_button.clicked[bool].connect(self._handle_torso_start_pos_button_clicked)
        self._widget.torso_row_1_pos_button.clicked[bool].connect(self._handle_torso_row_1_pos_button_clicked)
        self._widget.torso_row_2_pos_button.clicked[bool].connect(self._handle_torso_row_2_pos_button_clicked)
        self._widget.torso_row_3_pos_button.clicked[bool].connect(self._handle_torso_row_3_pos_button_clicked)
        self._widget.torso_row_4_pos_button.clicked[bool].connect(self._handle_torso_row_4_pos_button_clicked)


        self._widget.base_calibration_button.clicked[bool].connect(self._handle_base_col_1_pos_button_clicked)
        self._widget.base_col_1_pos_button.clicked[bool].connect(self._handle_base_col_1_pos_button_clicked)
        self._widget.base_col_2_pos_button.clicked[bool].connect(self._handle_base_col_2_pos_button_clicked)
        self._widget.base_col_3_pos_button.clicked[bool].connect(self._handle_base_col_3_pos_button_clicked)
        self._widget.base_retreat_button.clicked[bool].connect(self._handle_base_retreat_button_clicked)

        self._widget.head_start_pos_button.clicked[bool].connect(self._handle_head_row_1_pos_button_clicked)
        self._widget.head_row_1_pos_button.clicked[bool].connect(self._handle_head_row_1_pos_button_clicked)
        self._widget.head_row_2_pos_button.clicked[bool].connect(self._handle_head_row_2_pos_button_clicked)
        self._widget.head_row_3_pos_button.clicked[bool].connect(self._handle_head_row_3_pos_button_clicked)
        self._widget.head_row_4_pos_button.clicked[bool].connect(self._handle_head_row_4_pos_button_clicked)


        self._widget.arms_start_pos_button.clicked[bool].connect(self._handle_arms_start_pos_button_clicked)

        self._widget.detector_button.clicked[bool].connect(self._handle_detector_button_clicked)
        self._widget.pregrasp_button.clicked[bool].connect(self._handle_pregrasp_button_clicked)

        self._widget.start_bt_button.clicked[bool].connect(self._handle_start_bt_button_clicked)

        self._widget.start_shelf_publisher_button.clicked[bool].connect(self._handle_start_shelf_publisher_button_clicked)

        self._mode = 'pregrasp'

        self._got_task = False
        self._timer = rospy.Timer(rospy.Duration(1.0), self.timer_cb)
        self._task_sub = rospy.Subscriber('/amazon_next_task', String, self.task_cb)
        self.get_moveit()

    @staticmethod
    def add_arguments(parser):
        group = parser.add_argument_group('Options for rqt_amazon_challenge plugin')
        # group.add_argument('bagfiles', type=argparse.FileType('r'), nargs='*', default=[], help='Bagfiles to load')


    def get_moveit(self):
        while not rospy.is_shutdown():
            rospy.loginfo('[GUI]: connecting to moveit...')
            try:
                self._robot = moveit_commander.RobotCommander()
                self._left_arm = self._robot.get_group('left_arm')
                self._right_arm = self._robot.get_group('right_arm')
                self._arms = self._robot.get_group('arms')
                self._torso = self._robot.get_group('torso')
                self._head = self._robot.get_group('head')
                self._arms_dict = {'left_arm': self._left_arm, 'right_arm': self._right_arm}
                break
            except:
                rospy.sleep(random.uniform(0, 1))
                pass

    def get_bm_srv(self):
        while not rospy.is_shutdown():
            try:
                rospy.wait_for_service('/base_move_server/move', 5.0)
                rospy.wait_for_service('/base_move_server/preempt', 5.0)
                break
            except:
                rospy.loginfo('[' + rospy.get_name() + ']: waiting for base move server')
                continue

        self._bm_move_srv = rospy.ServiceProxy('/base_move_server/move', BaseMove)
        self._bm_preempt_srv = rospy.ServiceProxy('/base_move_server/preempt', Empty)


    def get_shelf_pose(self, msg):
        self._shelf_pose = msg
        self._got_shelf_pose = True


    def timer_cb(self, event):
        rp = rospkg.RosPack()

        while not rospy.is_shutdown():
            try:
                json_pkg = rospy.get_param('/json_file/package', 'amazon_challenge_bt_actions')
                json_relative_path = rospy.get_param('/json_file/relative_path', 'src/example_full.json')
                contest = rospy.get_param('/contest', True)
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        if contest:
            json_path = os.getenv('HOME') + '/contest.json'
        else:
            json_pkg_path = rp.get_path(json_pkg)
            json_path = json_pkg_path + '/' + json_relative_path

        if contest:
            self._widget.contest_label.setText('true')

        else:
            self._widget.contest_label.setText('!!!!!!!!!!!!FALSE!!!!!!!!')

        self._widget.json_file_label.setText(json_path)


        if self._got_task:
            self._widget.item_label.setText(self._item)
            self._widget.bin_label.setText(self._bin)

    def task_cb(self, msg):
        text = msg.data
        text = text.replace('[','')
        text = text.replace(']','')
        words = text.split(',')
        self._bin = words[0]
        self._item = words[1]
        self._got_task = True

    def shutdown_plugin(self):
        # TODO unregister all publishers here
        pass

    def save_settings(self, plugin_settings, instance_settings):
        # TODO save intrinsic configuration, usually using:
        # instance_settings.set_value(k, v)
        pass

    def restore_settings(self, plugin_settings, instance_settings):
        # TODO restore intrinsic configuration, usually using:
        # v = instance_settings.value(k)
        pass

    #def trigger_configuration(self):
        # Comment in to signal that the plugin has a way to configure
        # This will enable a setting button (gear icon) in each dock widget title bar
        # Usually used to open a modal configuration dialog


    # mode callback

    def _handle_detector_button_clicked(self):
        self._mode = 'detector'
        rospy.loginfo('[GUI]: detector mode')


    def _handle_pregrasp_button_clicked(self):
        self._mode = 'pregrasp'
        rospy.loginfo('[GUI]: pregrasp mode')


    # left arm

    def _handle_l_arm_start_pos_button_clicked(self):
        rospy.loginfo('[GUI]: left arm start pos')

        while not rospy.is_shutdown():
            try:
                left_arm_joint_pos_dict = rospy.get_param('/left_arm_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        joint_pos_goal = left_arm_joint_pos_dict['start']

        self._left_arm.set_joint_value_target(joint_pos_goal)
        self._left_arm.go()

    def _handle_l_arm_row_1_pos_button_clicked(self):
        rospy.loginfo('[GUI]: left arm row 1 pos')

        while not rospy.is_shutdown():
            try:
                left_arm_joint_pos_dict = rospy.get_param('/left_arm_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        joint_pos_goal = left_arm_joint_pos_dict[self._mode]['row_1']

        self._left_arm.set_joint_value_target(joint_pos_goal)
        self._left_arm.go()

    def _handle_l_arm_row_2_pos_button_clicked(self):
        rospy.loginfo('[GUI]: left arm row 2 pos')

        while not rospy.is_shutdown():
            try:
                left_arm_joint_pos_dict = rospy.get_param('/left_arm_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        joint_pos_goal = left_arm_joint_pos_dict[self._mode]['row_2']

        self._left_arm.set_joint_value_target(joint_pos_goal)
        self._left_arm.go()

    def _handle_l_arm_row_3_pos_button_clicked(self):
        rospy.loginfo('[GUI]: left arm row 3 pos')


        while not rospy.is_shutdown():
            try:
                left_arm_joint_pos_dict = rospy.get_param('/left_arm_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        joint_pos_goal = left_arm_joint_pos_dict[self._mode]['row_3']

        self._left_arm.set_joint_value_target(joint_pos_goal)
        self._left_arm.go()

    def _handle_l_arm_row_4_pos_button_clicked(self):
        rospy.loginfo('[GUI]: left arm row 4 pos')
        while not rospy.is_shutdown():
            try:
                left_arm_joint_pos_dict = rospy.get_param('/left_arm_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        joint_pos_goal = left_arm_joint_pos_dict[self._mode]['row_4']

        self._left_arm.set_joint_value_target(joint_pos_goal)
        self._left_arm.go()


    # right arm

    def _handle_r_arm_start_pos_button_clicked(self):
        rospy.loginfo('[GUI]: right arm start pos')

        while not rospy.is_shutdown():
            try:
                right_arm_joint_pos_dict = rospy.get_param('/right_arm_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        joint_pos_goal = right_arm_joint_pos_dict['start']

        self._right_arm.set_joint_value_target(joint_pos_goal)
        self._right_arm.go()

    def _handle_r_arm_row_1_pos_button_clicked(self):
        rospy.loginfo('[GUI]: right arm row 1 pos')

        while not rospy.is_shutdown():
            try:
                right_arm_joint_pos_dict = rospy.get_param('/right_arm_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        joint_pos_goal = right_arm_joint_pos_dict[self._mode]['row_1']

        self._right_arm.set_joint_value_target(joint_pos_goal)
        self._right_arm.go()

    def _handle_r_arm_row_2_pos_button_clicked(self):
        rospy.loginfo('[GUI]: right arm row 2 pos')

        while not rospy.is_shutdown():
            try:
                right_arm_joint_pos_dict = rospy.get_param('/right_arm_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        joint_pos_goal = right_arm_joint_pos_dict[self._mode]['row_2']

        self._right_arm.set_joint_value_target(joint_pos_goal)
        self._right_arm.go()

    def _handle_r_arm_row_3_pos_button_clicked(self):
        rospy.loginfo('[GUI]: right arm row 3 pos')

        while not rospy.is_shutdown():
            try:
                right_arm_joint_pos_dict = rospy.get_param('/right_arm_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        joint_pos_goal = right_arm_joint_pos_dict[self._mode]['row_3']

        self._right_arm.set_joint_value_target(joint_pos_goal)
        self._right_arm.go()

    def _handle_r_arm_row_4_pos_button_clicked(self):
        rospy.loginfo('[GUI]: right arm row 4 pos')

        while not rospy.is_shutdown():
            try:
                right_arm_joint_pos_dict = rospy.get_param('/right_arm_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        joint_pos_goal = right_arm_joint_pos_dict[self._mode]['row_4']

        self._right_arm.set_joint_value_target(joint_pos_goal)
        self._right_arm.go()

    # torso

    def _handle_torso_start_pos_button_clicked(self):
        rospy.loginfo('[GUI]: torso start pos')

        while not rospy.is_shutdown():
            try:
                torso_joint_pos_dict = rospy.get_param('/torso_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        joint_pos_goal = torso_joint_pos_dict['start']

        self._torso.set_joint_value_target(joint_pos_goal)
        self._torso.go()

    def _handle_torso_row_1_pos_button_clicked(self):
        rospy.loginfo('[GUI]: torso row 1 pos')

        while not rospy.is_shutdown():
            try:
                torso_joint_pos_dict = rospy.get_param('/torso_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        joint_pos_goal = torso_joint_pos_dict[self._mode]['row_1']

        self._torso.set_joint_value_target(joint_pos_goal)
        self._torso.go()

    def _handle_torso_row_2_pos_button_clicked(self):
        rospy.loginfo('[GUI]: torso row 2 pos')

        while not rospy.is_shutdown():
            try:
                torso_joint_pos_dict = rospy.get_param('/torso_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        joint_pos_goal = torso_joint_pos_dict[self._mode]['row_2']

        self._torso.set_joint_value_target(joint_pos_goal)
        self._torso.go()

    def _handle_torso_row_3_pos_button_clicked(self):
        rospy.loginfo('[GUI]: torso row 3 pos')

        while not rospy.is_shutdown():
            try:
                torso_joint_pos_dict = rospy.get_param('/torso_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        joint_pos_goal = torso_joint_pos_dict[self._mode]['row_3']

        self._torso.set_joint_value_target(joint_pos_goal)
        self._torso.go()


    def _handle_torso_row_4_pos_button_clicked(self):
        rospy.loginfo('[GUI]: torso row 4 pos')

        while not rospy.is_shutdown():
            try:
                torso_joint_pos_dict = rospy.get_param('/torso_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        joint_pos_goal = torso_joint_pos_dict[self._mode]['row_4']

        self._torso.set_joint_value_target(joint_pos_goal)
        self._torso.go()

    def _handle_arms_start_pos_button_clicked(self):
        rospy.loginfo('[GUI]: arms start pos')

        while not rospy.is_shutdown():
            try:
                left_arm_joint_pos_dict = rospy.get_param('/left_arm_joint_pos_dict')
                right_arm_joint_pos_dict = rospy.get_param('/right_arm_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        left_arm_joint_pos_goal = left_arm_joint_pos_dict['start']
        right_arm_joint_pos_goal = right_arm_joint_pos_dict['start']
        joint_pos_goal = left_arm_joint_pos_goal + right_arm_joint_pos_goal

        self._arms.set_joint_value_target(joint_pos_goal)
        self._arms.go()

    def _handle_base_col_1_pos_button_clicked(self):
        # put arms in start position
        self._handle_arms_start_pos_button_clicked()
        rospy.loginfo('[GUI]: base col 1 pos')

        while not rospy.is_shutdown():
            try:
                base_pos_dict = rospy.get_param('/base_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        base_pos_goal = base_pos_dict['column_1']

        rospy.loginfo('Base setpoint: ')
        rospy.loginfo(base_pos_goal)

        self.get_bm_srv()
        self._bm_preempt_srv.call(EmptyRequest())

        req = BaseMoveRequest()
        req.x = base_pos_goal[0]
        req.y = base_pos_goal[1]
        req.theta = base_pos_goal[5]

        res = self._bm_move_srv.call(req)


    def _handle_base_col_2_pos_button_clicked(self):
        # put arms in start position
        self._handle_arms_start_pos_button_clicked()
        rospy.loginfo('[GUI]: base col 2 pos')

        while not rospy.is_shutdown():
            try:
                base_pos_dict = rospy.get_param('/base_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        base_pos_goal = base_pos_dict['column_2']

        rospy.loginfo('Base setpoint: ')
        rospy.loginfo(base_pos_goal)

        self.get_bm_srv()
        self._bm_preempt_srv.call(EmptyRequest())

        req = BaseMoveRequest()
        req.x = base_pos_goal[0]
        req.y = base_pos_goal[1]
        req.theta = base_pos_goal[5]

        res = self._bm_move_srv.call(req)


    def _handle_base_col_3_pos_button_clicked(self):
        # put arms in start position
        self._handle_arms_start_pos_button_clicked()
        rospy.loginfo('[GUI]: base col 3 pos')

        while not rospy.is_shutdown():
            try:
                base_pos_dict = rospy.get_param('/base_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        base_pos_goal = base_pos_dict['column_3']

        rospy.loginfo('Base setpoint: ')
        rospy.loginfo(base_pos_goal)

        self.get_bm_srv()
        self._bm_preempt_srv.call(EmptyRequest())

        req = BaseMoveRequest()
        req.x = base_pos_goal[0]
        req.y = base_pos_goal[1]
        req.theta = base_pos_goal[5]

        res = self._bm_move_srv.call(req)


    def _handle_base_retreat_button_clicked(self):
        rospy.loginfo('[GUI]: base retreat')

        r = rospy.Rate(1.0)
        while not self._got_shelf_pose:
            rospy.loginfo('[' + rospy.get_name() + ']: waiting for shelf pose')
            r.sleep()

        base_pos_goal = [-1.42, -self._shelf_pose.pose.position.y, 0.0, 0.0, 0.0, 0.0]

        rospy.loginfo('Base setpoint: ')
        rospy.loginfo(base_pos_goal)

        self.get_bm_srv()
        self._bm_preempt_srv.call(EmptyRequest())

        req = BaseMoveRequest()
        req.x = base_pos_goal[0]
        req.y = base_pos_goal[1]
        req.theta = base_pos_goal[5]

        res = self._bm_move_srv.call(req)

    def _handle_head_row_1_pos_button_clicked(self):
        rospy.loginfo('[GUI]: head row 1 pos')

        while not rospy.is_shutdown():
            try:
                head_joint_pos_dict = rospy.get_param('/head_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        head_pos_goal = head_joint_pos_dict['row_1']

        self._head.set_joint_value_target(head_pos_goal)
        self._head.go()

    def _handle_head_row_2_pos_button_clicked(self):
        rospy.loginfo('[GUI]: head row 2 pos')

        while not rospy.is_shutdown():
            try:
                head_joint_pos_dict = rospy.get_param('/head_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        head_pos_goal = head_joint_pos_dict['row_2']

        self._head.set_joint_value_target(head_pos_goal)
        self._head.go()

    def _handle_head_row_3_pos_button_clicked(self):
        rospy.loginfo('[GUI]: head row 3 pos')

        while not rospy.is_shutdown():
            try:
                head_joint_pos_dict = rospy.get_param('/head_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        head_pos_goal = head_joint_pos_dict['row_3']

        self._head.set_joint_value_target(head_pos_goal)
        self._head.go()

    def _handle_head_row_4_pos_button_clicked(self):
        rospy.loginfo('[GUI]: head row 4 pos')

        while not rospy.is_shutdown():
            try:
                head_joint_pos_dict = rospy.get_param('/head_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        head_pos_goal = head_joint_pos_dict['row_4']

        self._head.set_joint_value_target(head_pos_goal)
        self._head.go()

    def _handle_start_bt_button_clicked(self):
        rospy.loginfo('[GUI]: start bt button clicked')
        rospy.loginfo('[GUI]: waiting for /kick_ass service')
        try:
            rospy.wait_for_service('/kick_ass', 10.0)
        except:
            rospy.logerr('[GUI]: timed out waiting for BT service /kick_ass')
            return
        
        srv = rospy.ServiceProxy('/kick_ass', Empty)

        rospy.loginfo('[GUI]: BT started!!!')

        srv.call()

    def _handle_start_shelf_publisher_button_clicked(self):
        rospy.loginfo('[GUI]: starting shelf publisher')
        try:
            rospy.wait_for_service('/shelf_publisher/start', 10.0)
        except:
            rospy.logerr('[GUI]: timed out waiting for start publisher service')
            return

        srv = rospy.ServiceProxy('/shelf_publisher/start', Empty)

        rospy.loginfo('[GUI]: started shelf publisher!')

        srv.call()
class MyPlugin(Plugin):

    def __init__(self, context):
        super(MyPlugin, self).__init__(context)
        # Give QObjects reasonable names
        self.setObjectName('MyPlugin')

        # Process standalone plugin command-line arguments
        from argparse import ArgumentParser
        parser = ArgumentParser()
        # Add argument(s) to the parser.
        parser.add_argument("-q", "--quiet", action="store_true",
                      dest="quiet",
                      help="Put plugin in silent mode")
        args, unknowns = parser.parse_known_args(context.argv())
        if not args.quiet:
            print 'arguments: ', args
            print 'unknowns: ', unknowns

        # Create QWidget
        self._widget = QMainWindow()
        # Get path to UI file which should be in the "resource" folder of this package
        ui_file = os.path.join(rospkg.RosPack().get_path('cdpr3'), 'src', 'cabledrivenparallerobot.ui')
	#ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'cabledrivenparallerobot.ui')
        # Extend the widget with all attributes and children from UI file
        loadUi(ui_file, self._widget)
        # Give QObjects reasonable names
        self._widget.setObjectName('MyPluginUi')
        # Show _widget.windowTitle on left-top of each plugin (when 
        # it's set in _widget). This is useful when you open multiple 
        # plugins at once. Also if you open multiple instances of your 
        # plugin at once, these lines add number to make it easy to 
        # tell from pane to pane.
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
        # Add widget to the user interface
        context.add_widget(self._widget)
	QtCore.QObject.connect(self._widget.pushButton_2, QtCore.SIGNAL("clicked()"), self.pSaveParameter)
	QtCore.QObject.connect(self._widget.pushButton_3, QtCore.SIGNAL("clicked()"), self.pLoadParameter)
 		
    def shutdown_plugin(self):
        # TODO unregister all publishers here
        pass

    def save_settings(self, plugin_settings, instance_settings):
        # TODO save intrinsic configuration, usually using:
        # instance_settings.set_value(k, v)
        pass

    def restore_settings(self, plugin_settings, instance_settings):
        # TODO restore intrinsic configuration, usually using:
        # v = instance_settings.value(k)
        pass

    #def trigger_configuration(self):
        # Comment in to signal that the plugin has a way to configure
        # This will enable a setting button (gear icon) in each dock widget title bar
        # Usually used to open a modal configuration dialog
    	object = self._widget
    def pSaveParameter(object):
		SaveParameter(object)

    def pLoadParameter(object):
		LoadParameter(object)