class DockWidgetContainer(DockWidget): """`DockWidget` containing a main window acting as a container for other dock widgets.""" def __init__(self, container_manager, serial_number): super(DockWidgetContainer, self).__init__(container_manager) self._serial_number = serial_number self._settings = None self.main_window = QMainWindow() self.main_window.setDockNestingEnabled(True) self.setWidget(self.main_window) def serial_number(self): return self._serial_number def setObjectName(self, name): super(DockWidget, self).setObjectName(name) self.main_window.setObjectName(name + '__MainWindow') def save_settings(self, settings): mw_settings = settings.get_settings('mainwindow') self._save_geometry(mw_settings) self._save_state(mw_settings) super(DockWidgetContainer, self).save_settings(settings) def _save_geometry(self, settings): # unmaximizing widget before saveGeometry works around bug to restore dock-widgets # still the non-maximized size can not correctly be restored maximized = self.isMaximized() if maximized: self.showNormal() settings.set_value('geometry', self.main_window.saveGeometry()) if maximized: self.showMaximized() def _save_state(self, settings): if self._settings is not None: self._settings.set_value('state', self.main_window.saveState()) def restore_settings(self, settings): super(DockWidgetContainer, self).restore_settings(settings) mw_settings = settings.get_settings('mainwindow') self._settings = mw_settings # only restore geometry, restoring state is triggered after PluginManager has been updated self._restore_geometry(mw_settings) def _restore_geometry(self, settings): if settings.contains('geometry'): self.main_window.restoreGeometry(settings.value('geometry')) def restore_state(self): if self._settings.contains('state'): self.main_window.restoreState(self._settings.value('state'))
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"
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 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"
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)