def __init__(self, options, title='Exclusive Options', selected_index=None): super(ExclusiveOptionGroup, self).__init__() self.setTitle(title) self.setLayout(QVBoxLayout()) self._button_group = QButtonGroup() self._button_group.setExclusive(True) self._options = options button_id = 0 for option in self._options: button_id += 1 radio_button = QRadioButton( option.get('title', 'option %d' % button_id)) radio_button.setEnabled(option.get('enabled', True)) radio_button.setChecked( option.get('selected', False) or (button_id - 1) == selected_index) radio_button.setToolTip(option.get('tooltip', '')) widget = QWidget() widget.setLayout(QVBoxLayout()) widget.layout().addWidget(radio_button) if 'description' in option: widget.layout().addWidget(QLabel(option['description'])) self._button_group.addButton(radio_button, button_id) self.layout().addWidget(widget)
class PyConsole(Plugin): """ Plugin providing an interactive Python console """ def __init__(self, context): super(PyConsole, self).__init__(context) self.setObjectName('PyConsole') self._context = context self._use_spyderlib = _has_spyderlib self._console_widget = None self._widget = QWidget() self._widget.setLayout(QVBoxLayout()) self._widget.layout().setContentsMargins(0, 0, 0, 0) if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + \ (' (%d)' % context.serial_number())) self._context.add_widget(self._widget) def _switch_console_widget(self): self._widget.layout().removeWidget(self._console_widget) self.shutdown_console_widget() if _has_spyderlib and self._use_spyderlib: self._console_widget = SpyderConsoleWidget(self._context) self._widget.setWindowTitle('SpyderConsole') else: self._console_widget = PyConsoleWidget(self._context) self._widget.setWindowTitle('PyConsole') if self._context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number())) self._widget.layout().addWidget(self._console_widget) def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('use_spyderlib', self._use_spyderlib) def restore_settings(self, plugin_settings, instance_settings): self._use_spyderlib = _has_spyderlib and (instance_settings.value('use_spyderlib', True) in [True, 'true']) self._switch_console_widget() def trigger_configuration(self): options = [ {'title': 'SpyderConsole', 'description': 'Advanced Python console with tab-completion (needs spyderlib to be installed).', 'enabled': _has_spyderlib}, {'title': 'PyConsole', 'description': 'Simple Python console.'}, ] dialog = SimpleSettingsDialog(title='PyConsole Options') dialog.add_exclusive_option_group(title='Console Type', options=options, selected_index=int(not self._use_spyderlib)) console_type = dialog.get_settings()[0] new_use_spyderlib = {0: True, 1: False}.get(console_type['selected_index'], self._use_spyderlib) if self._use_spyderlib != new_use_spyderlib: self._use_spyderlib = new_use_spyderlib self._switch_console_widget() def shutdown_console_widget(self): if self._console_widget is not None and hasattr(self._console_widget, 'shutdown'): self._console_widget.shutdown() def shutdown_plugin(self): self.shutdown_console_widget()
class PyConsole(Plugin): """ Plugin providing an interactive Python console """ def __init__(self, context): super(PyConsole, self).__init__(context) self.setObjectName('PyConsole') self._context = context self._use_spyderlib = _has_spyderlib self._console_widget = None self._widget = QWidget() self._widget.setLayout(QVBoxLayout()) self._widget.layout().setContentsMargins(0, 0, 0, 0) if context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) self._context.add_widget(self._widget) def _switch_console_widget(self): self._widget.layout().removeWidget(self._console_widget) self.shutdown_console_widget() if _has_spyderlib and self._use_spyderlib: self._console_widget = SpyderConsoleWidget(self._context) self._widget.setWindowTitle('SpyderConsole') else: self._console_widget = PyConsoleWidget(self._context) self._widget.setWindowTitle('PyConsole') if self._context.serial_number() > 1: self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number())) self._widget.layout().addWidget(self._console_widget) def save_settings(self, plugin_settings, instance_settings): instance_settings.set_value('use_spyderlib', self._use_spyderlib) def restore_settings(self, plugin_settings, instance_settings): self._use_spyderlib = _has_spyderlib and (instance_settings.value('use_spyderlib', True) in [True, 'true']) self._switch_console_widget() def trigger_configuration(self): options = [ {'title': 'SpyderConsole', 'description': 'Advanced Python console with tab-completion (needs spyderlib to be installed).', 'enabled': _has_spyderlib}, {'title': 'PyConsole', 'description': 'Simple Python console.'}, ] dialog = SimpleSettingsDialog(title='PyConsole Options') dialog.add_exclusive_option_group(title='Console Type', options=options, selected_index=int(not self._use_spyderlib)) console_type = dialog.get_settings()[0] new_use_spyderlib = {0: True, 1: False}.get(console_type['selected_index'], self._use_spyderlib) if self._use_spyderlib != new_use_spyderlib: self._use_spyderlib = new_use_spyderlib self._switch_console_widget() def shutdown_console_widget(self): if self._console_widget is not None and hasattr(self._console_widget, 'shutdown'): self._console_widget.shutdown() def shutdown_plugin(self): self.shutdown_console_widget()
class RqtCalibrationEvaluator(Plugin): def __init__(self, context): super(RqtCalibrationEvaluator, self).__init__(context) # Give QObjects reasonable names self.setObjectName('CalibrationEvaluator') # 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 = QWidget() self._infoWidget = QWidget() # 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_easy_handeye'), 'resource', 'rqt_handeye_evaluator.ui') ui_info_file = os.path.join( rospkg.RosPack().get_path('rqt_easy_handeye'), 'resource', 'rqt_handeye_info.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) loadUi(ui_info_file, self._infoWidget) self._widget.layout().insertWidget(0, self._infoWidget) # Give QObjects reasonable names self._widget.setObjectName('RqtHandeyeCalibrationUI') # 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) self._infoWidget.calibNameLineEdit.setText(rospy.get_namespace()) if rospy.get_param('eye_on_hand', False): self._infoWidget.calibTypeLineEdit.setText("eye on hand") else: self._infoWidget.calibTypeLineEdit.setText("eye on base") self._infoWidget.trackingBaseFrameLineEdit.setText( rospy.get_param('tracking_base_frame', 'optical_origin')) self._infoWidget.trackingMarkerFrameLineEdit.setText( rospy.get_param('tracking_marker_frame', 'optical_target')) self._infoWidget.robotBaseFrameLineEdit.setText( rospy.get_param('robot_base_frame', 'base_link')) self._infoWidget.robotEffectorFrameLineEdit.setText( rospy.get_param('robot_effector_frame', 'tool0')) self.output_label = self._widget.label_message self.output_label.setText('Waiting for samples...') self._widget.pushButton_reset.clicked.connect(self.reset) self.is_eye_on_hand = rospy.get_param('~eye_on_hand') self.robot_base_frame = rospy.get_param('~robot_base_frame') self.robot_effector_frame = rospy.get_param('~robot_effector_frame') self.tracking_measurement_frame = rospy.get_param( '~tracking_marker_frame') if self.is_eye_on_hand: self.robot_measurement_frame = self.robot_base_frame else: self.robot_measurement_frame = self.robot_effector_frame self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer) self.update_timer = QTimer(self) self.update_timer.timeout.connect(self.tick) self.update_timer.start(500) self.last_robot_transform = None # used to see if we are in a steady state self.measurement_transforms = [ ] # used to measure the quality of the calibration: should have the same value self.robot_transforms = [ ] # used to determine when to sample: we wait for a steady state that has not been sampled yet self._widget.show() def tick(self): # wait for steady state to avoid problems with lag # if next to a copy already in the dataset, tell to move # if could not sample transform, tell how old the last one is # show average error; show dataset in 3D? try: new_robot_transform = self.tf_buffer.lookup_transform( self.robot_base_frame, self.robot_effector_frame, rospy.Time.now(), rospy.Duration.from_sec(0.2)) new_measurement_transform = self.tf_buffer.lookup_transform( self.robot_measurement_frame, self.tracking_measurement_frame, rospy.Time.now(), rospy.Duration.from_sec(0.2)) if new_robot_transform is None: rospy.logwarn( 'Could not sample transform between {} and {}'.format( self.robot_base_frame, self.robot_effector_frame)) if new_measurement_transform is None: rospy.logwarn( 'Could not sample transform between {} and {}'.format( self.robot_measurement_frame, self.tracking_measurement_frame)) if self.last_robot_transform is None: self.last_robot_transform = new_robot_transform self.updateUI() rospy.loginfo('Sampled first transform') return if RqtCalibrationEvaluator.transform_too_far( new_robot_transform, self.last_robot_transform, absolute_tolerance=0.001): self.last_robot_transform = new_robot_transform msg = 'Waiting for steady state' rospy.loginfo(msg) self.output_label.setText(msg) return if self.robot_transform_is_too_close_to_previous_sample( new_robot_transform, absolute_tolerance=0.003): self.updateUI() rospy.loginfo( 'Now we have {} samples\ntoo close to an old pose, move around!' .format(len(self.measurement_transforms))) self.output_label.setText( 'Too close to an old pose, move around!') return self.robot_transforms.append(new_robot_transform) self.measurement_transforms.append(new_measurement_transform) rospy.loginfo('Appending transform; we got {} now'.format( len(self.measurement_transforms))) self.updateUI() # TODO: provide feedback if the data is sufficient except (LookupException, ExtrapolationException, ConnectivityException) as e: msg = 'Could not sample pose!' rospy.loginfo(msg) rospy.logerr(e) self.output_label.setText(msg) def reset(self): self.robot_transforms = [] self.measurement_transforms = [] self.updateUI() def updateUI(self): self._widget.spinBox_samples.setValue(len(self.measurement_transforms)) if len(self.measurement_transforms) > 2: def translation_from_msg(msg): t = msg.transform.translation return t.x, t.y, t.z translations = [ translation_from_msg(t) for t in self.measurement_transforms ] translations_np = np.array(translations) translations_avg = translations_np.mean(axis=0) translations_from_avg = translations_np - translations_avg translations_max_divergence = np.max(translations_from_avg) rospy.loginfo( "Maximum divergence: {}".format(translations_max_divergence)) self._widget.doubleSpinBox_error.setEnabled(True) self._widget.doubleSpinBox_error.setValue( translations_max_divergence.max()) else: self._widget.doubleSpinBox_error.setValue(0) self._widget.doubleSpinBox_error.setEnabled(False) @staticmethod def transform_to_concatenated_translation_quaternion(transform): tr = transform.transform.translation quat = transform.transform.rotation return np.array([tr.x, tr.y, tr.z, quat.x, quat.y, quat.z, quat.w]) def robot_transform_is_too_close_to_previous_sample( self, new_robot_transform, absolute_tolerance): # TODO: use a meaningful metric posevec = RqtCalibrationEvaluator.transform_to_concatenated_translation_quaternion( new_robot_transform) for t in reversed(self.robot_transforms): old_posevec = RqtCalibrationEvaluator.transform_to_concatenated_translation_quaternion( t) if np.allclose(posevec, old_posevec, atol=absolute_tolerance): return True return False @staticmethod def transform_too_far(t1, t2, absolute_tolerance): # TODO: use a meaningful metric return not np.allclose( RqtCalibrationEvaluator. transform_to_concatenated_translation_quaternion(t1), RqtCalibrationEvaluator. transform_to_concatenated_translation_quaternion(t2), atol=absolute_tolerance) def shutdown_plugin(self): # TODO unregister all publishers here pass def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) pass def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) pass
class VelocityControl(Plugin): signal_topic = Signal(JointState) def __init__(self, context): super(VelocityControl, self).__init__(context) # Give QObjects reasonable names self.setObjectName('VelocityControl') # 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()) # Create QWidget self._widget = QWidget() # Get path to UI file which is a sibling of this file # in this example the .ui and .py file are in the same folder #ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'MyPlugin.ui') # Extend the widget with all attributes and children from UI file #loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('VelocityControlUi') vLayout = QVBoxLayout(self._widget) vLayout.setContentsMargins(0, 0, 0, 0) vLayout.setSpacing(0) self._widget.setLayout(vLayout) self._widget.layout().setSpacing(0) self._widget.setWindowTitle("VelocityControl"); # 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) self.context = context self._topic_list = '' self._topic_commands = {} self._jointgroups = {} self.signal_topic.connect( self.signal_callback_list ) rospy.on_shutdown(self.on_ros_shutdown) #handle the ROS shutdown commands def on_ros_shutdown(self, *args): from python_qt_binding.QtGui import QApplication QApplication.exit(0) def shutdown_plugin(self): # TODO unregister all publishers here self.shutdownRosComm() def save_settings(self, plugin_settings, instance_settings): # TODO save intrinsic configuration, usually using: # instance_settings.set_value(k, v) self._topic_commands.clear() for caller, joint_frame in self._jointgroups.items(): self._topic_commands[caller] = joint_frame.get_command_topic() instance_settings.set_value('topic_commands', self._topic_commands) instance_settings.set_value('topic_list', self._topic_list) def restore_settings(self, plugin_settings, instance_settings): # TODO restore intrinsic configuration, usually using: # v = instance_settings.value(k) self.shutdownRosComm() self._topic_commands = instance_settings.value('topic_commands', {}) self._topic_list = instance_settings.value('topic_list', 'joint_state') self.fill_widget() self.reinitRosComm() def reinitRosComm(self): # if self._topic_command: # self._topic_command = rosgraph.names.script_resolve_name('rostopic', self._topic_command) # if (self._topic_command): # self._publisher_command = rospy.Publisher(self._topic_command, PowerSwitch, queue_size=1) if self._topic_list: self._topic_list = rosgraph.names.script_resolve_name('rostopic', self._topic_list) if self._topic_list: self._subscriber_list = rospy.Subscriber(self._topic_list, JointState, self.callback_jointstate) def clearstuff(self): pass # for name,p in self._devices.items(): # self._widget.layout().removeWidget(p) # p.clearParent() # self._devices.clear(); def shutdownRosComm(self): self.clearstuff() # if self._topic_command: # self._publisher_command.unregister() # self._publisher_command = None if self._topic_list: self._subscriber_list.unregister() self._subscriber_list = None def fill_widget(self): # for i in range(len(self._topics)): self._widget.layout().addItem(QSpacerItem(1,1,QSizePolicy.Expanding, QSizePolicy.Expanding)) def callback_jointstate(self, msg): self.signal_topic.emit(msg) def signal_callback_list( self, msg ): # update caller = msg._connection_header['callerid'] if caller not in self._jointgroups: stored_topic = None if caller in self._topic_commands: stored_topic = self._topic_commands[caller] group_frame = JointStateGroup(caller, msg, stored_topic=stored_topic) self._widget.layout().insertWidget(self._widget.layout().count() - 1, group_frame) self._jointgroups[caller] = group_frame else: # update the values self._jointgroups[caller].update(msg) def trigger_configuration(self): # Comment in to signal that the plugin has a way to configure it # Usually used to open a configuration dialog self.dialog_config = QDialog() ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'iop_rqt_velocity_config.ui') loadUi(ui_file, self.dialog_config) self.dialog_config.accepted.connect(self.on_dialog_config_accepted) # fill configuration dialog ti = TopicInfo() ti.fill_published_topics(self.dialog_config.comboBox_listTopic, "sensor_msgs/JointState", self._topic_list) # ti.fill_subscribed_topics(self.dialog_config.comboBox_commandTopic, "std_msgs/Float64MultiArray", self._topic_command) # stop on cancel pressed if not self.dialog_config.exec_(): return def on_dialog_config_accepted(self): self.shutdownRosComm() # self._topic_command = self.dialog_config.comboBox_commandTopic.currentText() self._topic_list = self.dialog_config.comboBox_listTopic.currentText() self.reinitRosComm()
class RqtHandeyeCalibration(Plugin): def __init__(self, context): super(RqtHandeyeCalibration, self).__init__(context) # Give QObjects reasonable names self.setObjectName('HandeyeCalibration') # 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 QWidgets self._widget = QWidget() self._infoWidget = QWidget() # 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_easy_handeye'), 'resource', 'rqt_handeye.ui') ui_info_file = os.path.join( rospkg.RosPack().get_path('rqt_easy_handeye'), 'resource', 'rqt_handeye_info.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) loadUi(ui_info_file, self._infoWidget) self._widget.horizontalLayout_infoAndActions.insertWidget( 0, self._infoWidget) self._calibration_algorithm_combobox = QComboBox() self._calibration_algorithm_layout = QHBoxLayout() self._calibration_algorithm_layout.insertWidget( 0, QLabel('Calibration algorithm: ')) self._calibration_algorithm_layout.insertWidget( 1, self._calibration_algorithm_combobox) self._infoWidget.layout().insertLayout( -1, self._calibration_algorithm_layout) # Give QObjects reasonable names self._widget.setObjectName('RqtHandeyeCalibrationUI') # 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) self.client = HandeyeClient() resp = self.client.list_algorithms() for i, a in enumerate(resp.algorithms): self._calibration_algorithm_combobox.insertItem(i, a) index_of_curr_alg = resp.algorithms.index(resp.current_algorithm) self._calibration_algorithm_combobox.setCurrentIndex(index_of_curr_alg) self._calibration_algorithm_combobox.currentTextChanged.connect( self.client.set_algorithm) self._infoWidget.calibNameLineEdit.setText(rospy.get_namespace()) self._infoWidget.trackingBaseFrameLineEdit.setText( self.client.parameters.tracking_base_frame) self._infoWidget.trackingMarkerFrameLineEdit.setText( self.client.parameters.tracking_marker_frame) self._infoWidget.robotBaseFrameLineEdit.setText( self.client.parameters.robot_base_frame) self._infoWidget.robotEffectorFrameLineEdit.setText( self.client.parameters.robot_effector_frame) if self.client.parameters.eye_on_hand: self._infoWidget.calibTypeLineEdit.setText("eye on hand") else: self._infoWidget.calibTypeLineEdit.setText("eye on base") self._widget.takeButton.clicked[bool].connect(self.handle_take_sample) self._widget.removeButton.clicked[bool].connect( self.handle_remove_sample) self._widget.computeButton.clicked[bool].connect( self.handle_compute_calibration) self._widget.saveButton.clicked[bool].connect( self.handle_save_calibration) self._widget.removeButton.setEnabled(False) self._widget.computeButton.setEnabled(False) self._widget.saveButton.setEnabled(False) 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 def _display_sample_list(self, sample_list): self._widget.sampleListWidget.clear() for i in range(len(sample_list.hand_world_samples)): formatted_robot_sample = format_sample( sample_list.hand_world_samples[i]) formatted_tracking_sample = format_sample( sample_list.camera_marker_samples[i]) self._widget.sampleListWidget.addItem( '{}) \n hand->world \n {} \n camera->marker\n {}\n'.format( i + 1, formatted_robot_sample, formatted_tracking_sample)) self._widget.sampleListWidget.setCurrentRow( len(sample_list.hand_world_samples) - 1) self._widget.removeButton.setEnabled( len(sample_list.hand_world_samples) > 0) def handle_take_sample(self): sample_list = self.client.take_sample() self._display_sample_list(sample_list) self._widget.computeButton.setEnabled( len(sample_list.hand_world_samples) > 1) self._widget.saveButton.setEnabled(False) def handle_remove_sample(self): index = self._widget.sampleListWidget.currentRow() sample_list = self.client.remove_sample(index) self._display_sample_list(sample_list) self._widget.computeButton.setEnabled( len(sample_list.hand_world_samples) > 1) self._widget.saveButton.setEnabled(False) def handle_compute_calibration(self): result = self.client.compute_calibration() self._widget.computeButton.setEnabled(False) if result.valid: self._widget.outputBox.setPlainText( str(result.calibration.transform.transform)) self._widget.saveButton.setEnabled(True) else: self._widget.outputBox.setPlainText( 'The calibration could not be computed') self._widget.saveButton.setEnabled(False) def handle_save_calibration(self): self.client.save() self._widget.saveButton.setEnabled(False)
class multiQuadGuiPlugin(Plugin): CMD = [] CMD_LAND = ("LAND", Qt.Key_L) CMD_QUALYSIS_CONNECT = ("QUALISYS CONNECT", Qt.Key_C) CMD_STOP = ("STOP", Qt.Key_S) CMD_GO_UP = ("GO UP", Qt.Key_Q) CMD_GO_MIDDLE = ("GO MIDDLE", Qt.Key_A) CMD_GO_DOWN = ("GO DOWN", Qt.Key_Z) CMD_GOTO = ("GOTO", Qt.Key_G) CMD_ARM = ("ARM", Qt.Key_T) CMD_UNARM = ("UNARM", Qt.Key_Y) CMD_SET_MODE = ("SET_MODE", Qt.Key_U) CMD_SET_CONTROLLER = ("SET_CONTROLLER", Qt.Key_I) CMD = [CMD_LAND,CMD_QUALYSIS_CONNECT,CMD_STOP,CMD_GO_UP,CMD_GO_MIDDLE,CMD_GO_DOWN,CMD_GOTO,CMD_ARM,CMD_UNARM,CMD_SET_MODE,CMD_SET_CONTROLLER] def __init__(self, context,namespace = None): # it is either "" or the input given at creation of plugin self.namespace = self._parse_args(context.argv()) super(multiQuadGuiPlugin, self).__init__(context) # Give QObjects reasonable names self.setObjectName('multiQuadGuiPlugin') # 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 = QWidget() # Get path to UI file which is a sibling of this file # in this example the .ui and .py file are in the same folder ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'multiQuadGui.ui') # Extend the widget with all attributes and children from UI file loadUi(ui_file, self._widget) # Give QObjects reasonable names self._widget.setObjectName('multiQuadGuiUi') # 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 self.arena = Arena(self) self._widget.layout().addWidget(self.arena,0,1) self._widget.keyPressEvent = self.keyPressEvent self._widget.keyReleaseEvent = self.keyReleaseEvent self._widget.table.keyPressEvent = self.keyPressEvent self._widget.table.keyReleaseEvent = self.keyReleaseEvent context.add_widget(self._widget) # # Adding all the tabs self.quad_ns_list = [] self.quads = {} self.quad_select = set() self.command = None self.update_timer = QTimer(self) self.update_timer.setInterval(1000) self.update_timer.timeout.connect(self.update_quad_list) self.update_timer.start() self.update_draw = QTimer(self) self.update_draw.setInterval(1000/30) self.update_draw.timeout.connect(self.arena.repaint) self.update_draw.timeout.connect(self.update_table) self.update_draw.start() table_sizes = [25,100,150,60,100,120,70,70,100] for i in range(len(table_sizes)): self._widget.table.setColumnWidth(i,table_sizes[i]) def _parse_args(self, argv): parser = argparse.ArgumentParser(prog='saver', add_help=False) # args = parser.parse_args(argv) if argv: namespace = argv[0] return namespace else: # is argv is empty return empty string return "" def get_id(self,ns): i = re.findall('\d+', ns)[0] return int(i) #@Slot(bool) def update_quad_list(self): self.quad_ns_list = rospy.get_param("/quad_ns_list") for ns in self.quad_ns_list: qid = self.get_id(ns) if qid not in self.quads.keys(): self.add_quad(ns,qid) def add_quad(self,ns,qid): print "Add new quad: " + str(ns) self.quads[qid] = Quad(ns) idR = self.getRowId(qid) self._widget.table.insertRow(idR) def getRowId(self,idq): ids = sorted(self.quads.keys()) return ids.index(idq) #@Slot(bool) def update_table(self): for (idq,q) in self.quads.iteritems(): t = self.arena.getTable(q) idT = self.getRowId(idq) t = [(idq,None)] + t for (idx,value) in enumerate(t): item = self._widget.table.item(idT, idx) if item==None: item = QTableWidgetItem() self._widget.table.setItem(idT,idx,item) if idx==0: if idq in self.quad_select: item.setBackground(QColor(25,0,255,100)) else: item.setBackground(Qt.white) else: if value[1] == False: item.setBackground(QColor(250,0,25,150)) elif value[1] == True: item.setBackground(QColor(0,250,25,150)) else: item.setBackground(Qt.white) item.setText(str(value[0])) def print_console(self,s): self._widget.console.append(s) def call_command(self,cmd,arg=None): for i in self.quad_select: func = lambda : call_command(self.quads[i],cmd,arg) threading.Thread(target=func).start() self.print_console(' '.join([str(l) for l in self.quad_select]) + " " + cmd[0]) def select(self,i): if i==0: for d in self.quads.keys(): self.quad_select.add(d) if i in self.quads.keys(): self.quad_select.add(i) self.update_table() def unselect(self,i): if i==0: self.quad_select.clear() if i in self.quads.keys() and i in self.quad_select: self.quad_select.remove(i) self.update_table() def keyPressEvent(self, event): for c in multiQuadGuiPlugin.CMD: if type(event) == QKeyEvent and event.key() == c[1]: self.call_command(c) if type(event) == QKeyEvent and (event.key() >= Qt.Key_0 and event.key() <= Qt.Key_9): self.select(event.key()-Qt.Key_0) def keyReleaseEvent(self,event): if type(event) == QKeyEvent and (event.key() >= Qt.Key_0 and event.key() <= Qt.Key_9): self.unselect(event.key()-Qt.Key_0)